From a0d526bf99a49a6030e8caa09dc1c65c649eaaef Mon Sep 17 00:00:00 2001 From: lm Date: Tue, 8 Jun 2010 08:37:00 +0200 Subject: [PATCH] Cleaned up compile environment --- src/comm/MAVLinkXMLParser.cc | 4 + src/uas/PxQuadMAV.cc | 171 +++++++++++++++++++++-------------- src/uas/UAS.cc | 22 +---- src/ui/HSIDisplay.cc | 10 +- src/ui/HSIDisplay.h | 10 +- 5 files changed, 116 insertions(+), 101 deletions(-) diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index c1afa25fd..62a47ee55 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -73,6 +73,8 @@ bool MAVLinkXMLParser::generate() QList< QPair > cFiles; QString lcmStructDefs = ""; + QString pureFileName; + // Run through root children while(!n.isNull()) { @@ -103,6 +105,7 @@ bool MAVLinkXMLParser::generate() { QFileInfo rInfo(this->fileName); fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName; + pureFileName = rInfo.baseName().split(".").first(); } QFile file(fileName); @@ -365,6 +368,7 @@ bool MAVLinkXMLParser::generate() // Mark all code as C code mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n"; mainHeader += "\n#include \"protocol.h\"\n"; + mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n"; QString includeLine = "#include \"%1\"\n"; QString mainHeaderName = "mavlink.h"; QString messagesDirName = "generated"; diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 539b6e6cf..debea0f2a 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -20,93 +20,123 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) UAS::receiveMessage(link, message); mavlink_message_t* msg = &message; - //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid; + //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid; +// Only compile this portion if matching MAVLink packets have been compiled +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES - // Handle your special messages - switch (msg->msgid) + if (message.sysid == uasId) { - case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT: + QString uasState; + QString stateDescription; + QString patternPath; + switch (message.msgid) { - mavlink_watchdog_heartbeat_t payload; - mavlink_msg_watchdog_heartbeat_decode(msg, &payload); - - emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count); - } - break; - + case MAVLINK_MSG_ID_RAW_AUX: + { + mavlink_raw_aux_t raw; + mavlink_msg_raw_aux_decode(&message, &raw); + quint64 time = getUnixTime(0); + emit valueChanged(uasId, "Pressure", raw.baro, time); + emit valueChanged(uasId, "Temperature", raw.temp, time); + } + break; + case MAVLINK_MSG_ID_PATTERN_DETECTED: + { + QByteArray b; + b.resize(256); + mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data()); + b.append('\0'); + QString path = QString(b); + bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false ); + emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected); + } + break; + case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT: + { + mavlink_watchdog_heartbeat_t payload; + mavlink_msg_watchdog_heartbeat_decode(msg, &payload); + + emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count); + } + break; + case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO: - { - mavlink_watchdog_process_info_t payload; - mavlink_msg_watchdog_process_info_decode(msg, &payload); - - emit processReceived(this->uasId, payload.watchdog_id, payload.process_id, QString((const char*)payload.name), QString((const char*)payload.arguments), payload.timeout); - } - break; - + { + mavlink_watchdog_process_info_t payload; + mavlink_msg_watchdog_process_info_decode(msg, &payload); + + emit processReceived(this->uasId, payload.watchdog_id, payload.process_id, QString((const char*)payload.name), QString((const char*)payload.arguments), payload.timeout); + } + break; + case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS: - { - mavlink_watchdog_process_status_t payload; - mavlink_msg_watchdog_process_status_decode(msg, &payload); - emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid); - } - break; + { + mavlink_watchdog_process_status_t payload; + mavlink_msg_watchdog_process_status_decode(msg, &payload); + emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid); + } + break; case MAVLINK_MSG_ID_DEBUG_VECT: - { - mavlink_debug_vect_t vect; - mavlink_msg_debug_vect_decode(msg, &vect); - QString str((const char*)vect.name); - emit valueChanged(uasId, str+".x", vect.x, MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, str+".y", vect.y, MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow()); - } - break; + { + mavlink_debug_vect_t vect; + mavlink_msg_debug_vect_decode(msg, &vect); + QString str((const char*)vect.name); + emit valueChanged(uasId, str+".x", vect.x, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, str+".y", vect.y, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow()); + } + break; case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: - { - mavlink_vision_position_estimate_t pos; - mavlink_msg_vision_position_estimate_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vis. roll", pos.roll, time); - emit valueChanged(uasId, "vis. pitch", pos.pitch, time); - emit valueChanged(uasId, "vis. yaw", pos.yaw, time); - emit valueChanged(uasId, "vis. x", pos.x, time); - emit valueChanged(uasId, "vis. y", pos.y, time); - emit valueChanged(uasId, "vis. z", pos.z, time); - emit valueChanged(uasId, "vis. vx", pos.vx, time); - emit valueChanged(uasId, "vis. vy", pos.vy, time); - emit valueChanged(uasId, "vis. vz", pos.vz, time); - emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time); - // Set internal state - if (!positionLock) { - // If position was not locked before, notify positive - GAudioOutput::instance()->notifyPositive(); + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "vis. roll", pos.roll, time); + emit valueChanged(uasId, "vis. pitch", pos.pitch, time); + emit valueChanged(uasId, "vis. yaw", pos.yaw, time); + emit valueChanged(uasId, "vis. x", pos.x, time); + emit valueChanged(uasId, "vis. y", pos.y, time); + emit valueChanged(uasId, "vis. z", pos.z, time); + emit valueChanged(uasId, "vis. vx", pos.vx, time); + emit valueChanged(uasId, "vis. vy", pos.vy, time); + emit valueChanged(uasId, "vis. vz", pos.vz, time); + emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time); + // Set internal state + if (!positionLock) + { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); + } + positionLock = true; } - positionLock = true; - } - break; + break; case MAVLINK_MSG_ID_AUX_STATUS: - { - mavlink_aux_status_t status; - mavlink_msg_aux_status_decode(&message, &status); - emit loadChanged(this, status.load/10.0f); - emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count); - emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count); - emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); - emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); - emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); - emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); - } - break; + { + mavlink_aux_status_t status; + mavlink_msg_aux_status_decode(&message, &status); + emit loadChanged(this, status.load/10.0f); + emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count); + emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count); + emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); + emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); + emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); + emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); + } + break; default: - // Do nothing - break; + // Do nothing + break; + } } + +#endif } void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES mavlink_watchdog_command_t payload; payload.target_system_id = uasId; payload.watchdog_id = watchdogId; @@ -116,4 +146,5 @@ void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int c mavlink_message_t msg; mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload); sendMessage(msg); +#endif } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b4d860144..947201400 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -266,15 +266,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "Mag. Z", raw.zmag, time); } break; - case MAVLINK_MSG_ID_RAW_AUX: - { - mavlink_raw_aux_t raw; - mavlink_msg_raw_aux_decode(&message, &raw); - quint64 time = getUnixTime(0); - emit valueChanged(uasId, "Pressure", raw.baro, time); - emit valueChanged(uasId, "Temperature", raw.temp, time); - } - break; case MAVLINK_MSG_ID_ATTITUDE: //std::cerr << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; @@ -349,7 +340,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) //qDebug() << "GOT GPS STATUS FOR "<< pos.satellites_visible << "SATELLITES"; for(int i = 0; i < (int)pos.satellites_visible; i++) { - emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast(pos.satellite_used[i])); + emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_elevation[i], pos.satellite_azimuth[i], pos.satellite_snr[i], static_cast(pos.satellite_used[i])); } } break; @@ -390,17 +381,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit textMessageReceived(uasId, severity, text); } break; - case MAVLINK_MSG_ID_PATTERN_DETECTED: - { - QByteArray b; - b.resize(256); - mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data()); - b.append('\0'); - QString path = QString(b); - bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false ); - emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected); - } - break; default: { if (!unknownPackets.contains(message.msgid)) diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 3fb71ca10..d0b6532b5 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -146,7 +146,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) //} } -void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used) +void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used) { Q_UNUSED(uasid); //qDebug() << "UPDATED SATELLITE"; @@ -159,12 +159,12 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float dire if (gpsSatellites.at(satid) == NULL) { - gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used)); + gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used)); } else { // Satellite exists, update it - gpsSatellites.at(satid)->update(satid, azimuth, direction, snr, used); + gpsSatellites.at(satid)->update(satid, elevation, azimuth, snr, used); } } @@ -230,8 +230,8 @@ void HSIDisplay::drawGPS() painter.setPen(color); painter.setBrush(brush); - float xPos = xCenter + sin(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius; - float yPos = yCenter + cos(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius; + float xPos = xCenter + sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * (sat->elevation/180.0f * M_PI) * radius; + float yPos = yCenter + cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * (sat->elevation/180.0f * M_PI) * radius; drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter); } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 6f8960e37..3cc46b904 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -68,28 +68,28 @@ protected: class GPSSatellite { public: - GPSSatellite(int id, float azimuth, float direction, float snr, bool used) : + GPSSatellite(int id, float elevation, float azimuth, float snr, bool used) : id(id), + elevation(elevation), azimuth(azimuth), - direction(direction), snr(snr), used(used) { } - void update(int id, float azimuth, float direction, float snr, bool used) + void update(int id, float elevation, float azimuth, float snr, bool used) { this->id = id; + this->elevation = elevation; this->azimuth = azimuth; - this->direction = direction; this->snr = snr; this->used = used; } int id; + float elevation; float azimuth; - float direction; float snr; bool used; -- 2.22.0