Adjusted to updated MAVLink packets

src/Waypoint.cc

@@ -32,7 +32,7 @@ This file is part of the QGROUNDCONTROL project
 #include "Waypoint.h"
 #include <QStringList>
 
-Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action)
+Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_COMMAND _action)
     : id(_id),
     x(_x),
     y(_y),
@@ -72,7 +72,7 @@ bool Waypoint::load(QTextStream &loadStream)
     {
         this->id = wpParams[0].toInt();
         this->frame = (MAV_FRAME) wpParams[1].toInt();
-        this->action = (MAV_ACTION) wpParams[2].toInt();
+        this->action = (MAV_COMMAND) wpParams[2].toInt();
         this->orbit = wpParams[3].toDouble();
         this->orbitDirection = wpParams[4].toInt();
         this->param1 = wpParams[5].toDouble();
@@ -134,14 +134,14 @@ void Waypoint::setYaw(double yaw)
 
 void Waypoint::setAction(int action)
 {
-    if (this->action != (MAV_ACTION)action)
+    if (this->action != (MAV_COMMAND)action)
     {
-        this->action = (MAV_ACTION)action;
+        this->action = (MAV_COMMAND)action;
         emit changed(this);
     }
 }
 
-void Waypoint::setAction(MAV_ACTION action)
+void Waypoint::setAction(MAV_COMMAND action)
 {
     if (this->action != action)
     {

src/Waypoint.h

@@ -36,7 +36,7 @@ This file is part of the PIXHAWK project
 #include <QObject>
 #include <QString>
 #include <QTextStream>
-#include "mavlink_types.h"
+#include "QGCMAVLink.h"
 
 class Waypoint : public QObject
 {
@@ -45,7 +45,7 @@ class Waypoint : public QObject
 public:
     Waypoint(quint16 id = 0, double x = 0.0f, double y = 0.0f, double z = 0.0f, double yaw = 0.0f, bool autocontinue = false,
              bool current = false, double orbit = 0.15f, int holdTime = 0,
-             MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE);
+             MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_COMMAND action=MAV_CMD_NAV_WAYPOINT);
     ~Waypoint();
 
     quint16 getId() const { return id; }
@@ -66,7 +66,7 @@ public:
     double getParam6() const { return z; }
     int getTurns() const { return param1; }
     MAV_FRAME getFrame() const { return frame; }
-    MAV_ACTION getAction() const { return action; }
+    MAV_COMMAND getAction() const { return action; }
     const QString& getName() const { return name; }
 
     void save(QTextStream &saveStream);
@@ -80,7 +80,7 @@ protected:
     double z;
     double yaw;
     MAV_FRAME frame;
-    MAV_ACTION action;
+    MAV_COMMAND action;
     bool autocontinue;
     bool current;
     double orbit;
@@ -98,7 +98,7 @@ public slots:
     void setYaw(double yaw);
     /** @brief Set the waypoint action */
     void setAction(int action);
-    void setAction(MAV_ACTION action);
+    void setAction(MAV_COMMAND action);
     void setFrame(MAV_FRAME frame);
     void setAutocontinue(bool autoContinue);
     void setCurrent(bool current);

src/comm/MAVLinkProtocol.cc

@@ -180,7 +180,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                 // Write message to buffer
                 mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
                 QByteArray b((const char*)buf, len);
-                if(m_logfile->write(b) < MAVLINK_MAX_PACKET_LEN+sizeof(quint64))
+                if(m_logfile->write(b) < static_cast<int>(MAVLINK_MAX_PACKET_LEN+sizeof(quint64)))
                 {
                     emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName()));
                     // Stop logging

src/comm/MAVLinkSimulationMAV.cc

@@ -134,8 +134,8 @@ void MAVLinkSimulationMAV::mainloop()
         pos.alt = z*1000.0;
         pos.lat = y*1E7;
         pos.lon = x*1E7;
-        pos.vx = sin(yaw)*10.0f*100.0f;
-        pos.vy = cos(yaw)*10.0f*100.0f;
+        pos.vx = 10.0f*100.0f;
+        pos.vy = 0;
         pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
         mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
         link->sendMAVLinkMessage(&msg);
@@ -165,11 +165,11 @@ void MAVLinkSimulationMAV::mainloop()
 
         // VFR HUD
         mavlink_vfr_hud_t hud;
-        hud.airspeed = 10;
-        hud.groundspeed = 9;
-        hud.alt = 123;
-        hud.heading = 90;
-        hud.climb = 0.1f;
+        hud.airspeed = pos.vx;
+        hud.groundspeed = pos.vx;
+        hud.alt = pos.alt;
+        hud.heading = ((yaw/M_PI)*180.0f+180.0f);
+        hud.climb = pos.vz;
         hud.throttle = 90;
         mavlink_msg_vfr_hud_encode(systemid, MAV_COMP_ID_IMU, &msg, &hud);
         link->sendMAVLinkMessage(&msg);
@@ -180,9 +180,9 @@ void MAVLinkSimulationMAV::mainloop()
         nav.nav_pitch = pitch;
         nav.nav_bearing = yaw;
         nav.target_bearing = yaw;
-        nav.wp_dist = 2;
-        nav.alt_error = 0.5;
-        // xtrack
+        nav.wp_dist = 2.0f;
+        nav.alt_error = 0.5f;
+        nav.xtrack_error = 0.2f;
         mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav);
         link->sendMAVLinkMessage(&msg);
     }

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -337,7 +337,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui
                 wp->target_system = target_systemid;
                 wp->target_component = target_compid;
 
-                if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue);
+                if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue);
 
                 mavlink_msg_waypoint_encode(systemid, compid, &msg, wp);
                 link->sendMAVLinkMessage(&msg);

src/comm/MAVLinkXMLParser.cc

@@ -247,7 +247,7 @@ bool MAVLinkXMLParser::generate()
 
                                         // Everything sane, starting with enum content
                                         currEnum = "enum " + enumName.toUpper() + "\n{\n";
-                                        currEnumEnd = "};\n\n";
+                                        currEnumEnd = QString("\n%1_ENUM_END\n};\n\n").arg(enumName.toUpper());
 
                                         int nextEnumValue = 0;
 

src/uas/UAS.cc

@@ -440,6 +440,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "climbrate", "m/s", hud.climb, time);
                 emit valueChanged(uasId, "throttle", "m/s", hud.throttle, time);
                 emit thrustChanged(this, hud.throttle/100.0);
+                emit altitudeChanged(uasId, hud.alt);
+                yaw = (hud.heading-180.0f/360.0f)*M_PI;
+                emit attitudeChanged(this, roll, pitch, yaw, getUnixTime());
+                emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, getUnixTime());
             }
             break;
             case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
@@ -455,7 +459,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "wp dist", "m", nav.wp_dist, time);
                 emit valueChanged(uasId, "alt err", "m", nav.alt_error, time);
                 emit valueChanged(uasId, "airspeed err", "m/s", nav.alt_error, time);
-                //emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time);
+                emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time);
             }
             break;
         case MAVLINK_MSG_ID_LOCAL_POSITION:
@@ -1337,27 +1341,27 @@ void UAS::enableRawControllerDataTransmission(int rate)
     sendMessage(msg);
 }
 
-void UAS::enableRawSensorFusionTransmission(int rate)
-{
-    // Buffers to write data to
-    mavlink_message_t msg;
-    mavlink_request_data_stream_t stream;
-    // Select the message to request from now on
-    stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION;
-    // Select the update rate in Hz the message should be send
-    stream.req_message_rate = rate;
-    // Start / stop the message
-    stream.start_stop = (rate) ? 1 : 0;
-    // The system which should take this command
-    stream.target_system = uasId;
-    // The component / subsystem which should take this command
-    stream.target_component = 0;
-    // Encode and send the message
-    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
-}
+//void UAS::enableRawSensorFusionTransmission(int rate)
+//{
+//    // Buffers to write data to
+//    mavlink_message_t msg;
+//    mavlink_request_data_stream_t stream;
+//    // Select the message to request from now on
+//    stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION;
+//    // Select the update rate in Hz the message should be send
+//    stream.req_message_rate = rate;
+//    // Start / stop the message
+//    stream.start_stop = (rate) ? 1 : 0;
+//    // The system which should take this command
+//    stream.target_system = uasId;
+//    // The component / subsystem which should take this command
+//    stream.target_component = 0;
+//    // Encode and send the message
+//    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
+//}
 
 void UAS::enablePositionTransmission(int rate)
 {

src/uas/UAS.h

@@ -275,7 +275,7 @@ public slots:
     void enableExtendedSystemStatusTransmission(int rate);
     void enableRCChannelDataTransmission(int rate);
     void enableRawControllerDataTransmission(int rate);
-    void enableRawSensorFusionTransmission(int rate);
+    //void enableRawSensorFusionTransmission(int rate);
     void enablePositionTransmission(int rate);
     void enableExtra1Transmission(int rate);
     void enableExtra2Transmission(int rate);

src/uas/UASInterface.h

@@ -259,7 +259,7 @@ public slots:
     virtual void enableExtendedSystemStatusTransmission(int rate) = 0;
     virtual void enableRCChannelDataTransmission(int rate) = 0;
     virtual void enableRawControllerDataTransmission(int rate) = 0;
-    virtual void enableRawSensorFusionTransmission(int rate) = 0;
+    //virtual void enableRawSensorFusionTransmission(int rate) = 0;
 
     virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0;
     virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0;
@@ -390,6 +390,7 @@ signals:
     void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
     void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
     void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec);
+    void altitudeChanged(int uasid, double altitude);
     /** @brief Update the status of one satellite used for localization */
     void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     void speedChanged(UASInterface*, double x, double y, double z, quint64 usec);

src/uas/UASWaypointManager.cc

@@ -138,8 +138,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
 
         if(wp->seq == current_wp_id)
         {
-            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
-            Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action);
+            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_COMMAND) wp->command;
+            Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_COMMAND) wp->command);
             addWaypoint(lwp, false);
 
             //get next waypoint
@@ -667,7 +667,7 @@ void UASWaypointManager::writeWaypoints()
                 cur_d->param1 = cur_s->getParam1();
                 cur_d->param2 = cur_s->getParam2();
                 cur_d->frame = cur_s->getFrame();
-                cur_d->action = cur_s->getAction();
+                cur_d->command = cur_s->getAction();
                 cur_d->seq = i;     // don't read out the sequence number of the waypoint class
                 cur_d->x = cur_s->getX();
                 cur_d->y = cur_s->getY();

src/ui/WaypointList.cc

@@ -188,7 +188,7 @@ void WaypointList::add()
         else
         {
             // Create global frame waypoint per default
-            wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE);
+            wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT);
             uas->getWaypointManager()->addWaypoint(wp);
         }
     }

src/ui/WaypointView.cc

@@ -416,12 +416,12 @@ void WaypointView::updateValues()
     }
 
     // Update action
-    MAV_ACTION action = wp->getAction();
+    MAV_COMMAND action = wp->getAction();
     int action_index = m_ui->comboBox_action->findData(action);
     // Set to "Other" action if it was -1
     if (action_index == -1)
     {
-        action_index = m_ui->comboBox_action->findData(MAV_ACTION_NB);
+        action_index = m_ui->comboBox_action->findData(MAV_COMMAND_ENUM_END);
     }
     // Only update if changed
     if (m_ui->comboBox_action->currentIndex() != action_index)