Updated to v1.0

src/comm/MAVLinkSimulationLink.cc

@@ -546,14 +546,14 @@ void MAVLinkSimulationLink::mainloop()
         static int typeCounter = 0;
         uint8_t mavType;
         if (typeCounter < 10) {
-            mavType = MAV_QUADROTOR;
+            mavType = MAV_TYPE_QUADROTOR;
         } else {
-            mavType = typeCounter % (OCU);
+            mavType = typeCounter % (MAV_TYPE_OCU);
         }
         typeCounter++;
 
         // Pack message and get size of encoded byte string
-        messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK);
+        messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_CLASS_PIXHAWK);
         // Allocate buffer with packet data
         bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
         //add data into datastream
@@ -722,31 +722,32 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
 
                 qDebug() << "SIM" << "received action" << action.action << "for system" << action.target;
 
-                switch (action.action) {
-                case MAV_ACTION_LAUNCH:
-                    status.status = MAV_STATE_ACTIVE;
-                    status.mode = MAV_MODE_AUTO;
-                    break;
-                case MAV_ACTION_RETURN:
-                    status.status = MAV_STATE_ACTIVE;
-                    break;
-                case MAV_ACTION_MOTORS_START:
-                    status.status = MAV_STATE_ACTIVE;
-                    status.mode = MAV_MODE_LOCKED;
-                    break;
-                case MAV_ACTION_MOTORS_STOP:
-                    status.status = MAV_STATE_STANDBY;
-                    status.mode = MAV_MODE_LOCKED;
-                    break;
-                case MAV_ACTION_EMCY_KILL:
-                    status.status = MAV_STATE_EMERGENCY;
-                    status.mode = MAV_MODE_MANUAL;
-                    break;
-                case MAV_ACTION_SHUTDOWN:
-                    status.status = MAV_STATE_POWEROFF;
-                    status.mode = MAV_MODE_LOCKED;
-                    break;
-                }
+                // FIXME MAVLINKV10PORTINGNEEDED
+//                switch (action.action) {
+//                case MAV_ACTION_LAUNCH:
+//                    status.status = MAV_STATE_ACTIVE;
+//                    status.mode = MAV_MODE_AUTO;
+//                    break;
+//                case MAV_ACTION_RETURN:
+//                    status.status = MAV_STATE_ACTIVE;
+//                    break;
+//                case MAV_ACTION_MOTORS_START:
+//                    status.status = MAV_STATE_ACTIVE;
+//                    status.mode = MAV_MODE_LOCKED;
+//                    break;
+//                case MAV_ACTION_MOTORS_STOP:
+//                    status.status = MAV_STATE_STANDBY;
+//                    status.mode = MAV_MODE_LOCKED;
+//                    break;
+//                case MAV_ACTION_EMCY_KILL:
+//                    status.status = MAV_STATE_EMERGENCY;
+//                    status.mode = MAV_MODE_MANUAL;
+//                    break;
+//                case MAV_ACTION_SHUTDOWN:
+//                    status.status = MAV_STATE_POWEROFF;
+//                    status.mode = MAV_MODE_LOCKED;
+//                    break;
+//                }
             }
             break;
 #ifdef MAVLINK_ENABLED_PIXHAWK
@@ -771,7 +772,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                 for (i = onboardParams.begin(); i != onboardParams.end(); ++i) {
                     if (j != 5) {
                         // Pack message and get size of encoded byte string
-                        mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
+                        mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
                         // Allocate buffer with packet data
                         bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                         //add data into datastream
@@ -799,7 +800,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                         onboardParams.insert(key, set.param_value);
 
                         // Pack message and get size of encoded byte string
-                        mavlink_msg_param_value_pack(set.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key));
+                        mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key));
                         // Allocate buffer with packet data
                         bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                         //add data into datastream
@@ -820,7 +821,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                     float paramValue = onboardParams.value(key);
 
                     // Pack message and get size of encoded byte string
-                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
+                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
                     // Allocate buffer with packet data
                     bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                     //add data into datastream
@@ -832,7 +833,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                     float paramValue = onboardParams.value(key);
 
                     // Pack message and get size of encoded byte string
-                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
+                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
                     // Allocate buffer with packet data
                     bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                     //add data into datastream

src/comm/MAVLinkSimulationMAV.cc

@@ -68,7 +68,7 @@ void MAVLinkSimulationMAV::mainloop()
     // 1 Hz execution
     if (timer1Hz <= 0) {
         mavlink_message_t msg;
-        mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version);
+        mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_CLASS_PIXHAWK);
         link->sendMAVLinkMessage(&msg);
         planner.handleMessage(msg);
 
@@ -306,30 +306,31 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
         if (systemid == mode.target) sys_mode = mode.mode;
     }
     break;
-    case MAVLINK_MSG_ID_ACTION: {
-        mavlink_action_t action;
-        mavlink_msg_action_decode(&msg, &action);
-        if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) {
-            mavlink_action_ack_t ack;
-            ack.action = action.action;
-            switch (action.action) {
-            case MAV_ACTION_TAKEOFF:
-                flying = true;
-                nav_mode = MAV_NAV_LIFTOFF;
-                ack.result = 1;
-                break;
-            default: {
-                ack.result = 0;
-            }
-            break;
-            }
-
-            // Give feedback about action
-            mavlink_message_t r_msg;
-            mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack);
-            link->sendMAVLinkMessage(&r_msg);
-        }
-    }
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    case MAVLINK_MSG_ID_ACTION: {
+//        mavlink_action_t action;
+//        mavlink_msg_action_decode(&msg, &action);
+//        if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) {
+//            mavlink_action_ack_t ack;
+//            ack.action = action.action;
+////            switch (action.action) {
+////            case MAV_ACTION_TAKEOFF:
+////                flying = true;
+////                nav_mode = MAV_NAV_LIFTOFF;
+////                ack.result = 1;
+////                break;
+////            default: {
+////                ack.result = 0;
+////            }
+////            break;
+////            }
+
+//            // Give feedback about action
+//            mavlink_message_t r_msg;
+//            mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack);
+//            link->sendMAVLinkMessage(&r_msg);
+//        }
+//    }
     break;
     case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: {
         mavlink_local_position_setpoint_set_t sp;

src/comm/MAVLinkSimulationMAV.h

@@ -55,16 +55,17 @@ protected:
     bool flying;
     int mavlink_version;
 
-    static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) {
-        uint16_t i = 0;
-        msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) {
+//        uint16_t i = 0;
+//        msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
 
-        i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
-        i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
-        i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version
+//        i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
+//        i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
+//        i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version
 
-        return mavlink_finalize_message(msg, system_id, component_id, i);
-    }
+//        return mavlink_finalize_message(msg, system_id, component_id, i);
+//    }
 
 };
 

src/uas/QGCMAVLinkUASFactory.cc

@@ -10,16 +10,21 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
 {
     QPointer<QObject> p;
 
-    if (parent != NULL) {
+    if (parent != NULL)
+    {
         p = parent;
-    } else {
+    }
+    else
+    {
         p = mavlink;
     }
 
     UASInterface* uas;
 
-    switch (heartbeat->autopilot) {
-    case MAV_AUTOPILOT_GENERIC: {
+    switch (heartbeat->autopilot)
+    {
+    case MAV_CLASS_GENERIC:
+    {
         UAS* mav = new UAS(mavlink, sysid);
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
@@ -28,7 +33,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         uas = mav;
     }
     break;
-    case MAV_AUTOPILOT_PIXHAWK: {
+    case MAV_CLASS_PIXHAWK:
+    {
         PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid);
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
@@ -40,7 +46,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         uas = mav;
     }
     break;
-    case MAV_AUTOPILOT_SLUGS: {
+    case MAV_CLASS_SLUGS:
+    {
         SlugsMAV* mav = new SlugsMAV(mavlink, sysid);
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
@@ -52,7 +59,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         uas = mav;
     }
     break;
-    case MAV_AUTOPILOT_ARDUPILOTMEGA: {
+    case MAV_CLASS_ARDUPILOTMEGA:
+    {
         ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid);
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
@@ -64,7 +72,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         uas = mav;
     }
     break;
-    default: {
+    default:
+    {
         UAS* mav = new UAS(mavlink, sysid);
         mav->setSystemType((int)heartbeat->type);
         // Connect this robot to the UAS object

src/uas/UAS.cc

@@ -205,10 +205,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 {
                     switch (type)
                     {
-                    case MAV_FIXED_WING:
+                    case MAV_TYPE_FIXED_WING:
                         setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR);
                         break;
-                    case MAV_QUADROTOR:
+                    case MAV_TYPE_QUADROTOR:
                         setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
                         break;
                     default:
@@ -930,7 +930,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 QByteArray b;
                 b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
-                mavlink_msg_statustext_get_text(&message, (int8_t*)b.data());
+                mavlink_msg_statustext_get_text(&message, b.data());
                 //b.append('\0');
                 QString text = QString(b);
                 int severity = mavlink_msg_statustext_get_severity(&message);
@@ -1163,14 +1163,15 @@ void UAS::setLocalOriginAtCurrentGPSPosition()
 
     if (ret == QMessageBox::Yes)
     {
-        mavlink_message_t msg;
-        mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN);
-        // Send message twice to increase chance that it reaches its goal
-        sendMessage(msg);
-        // Wait 5 ms
-        MG::SLEEP::usleep(5000);
-        // Send again
-        sendMessage(msg);
+        // FIXME MAVLINKV10PORTINGNEEDED
+//        mavlink_message_t msg;
+//        mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN);
+//        // Send message twice to increase chance that it reaches its goal
+//        sendMessage(msg);
+//        // Wait 5 ms
+//        MG::SLEEP::usleep(5000);
+//        // Send again
+//        sendMessage(msg);
         result = true;
     }
 }
@@ -1204,52 +1205,58 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
 }
 
 void UAS::startRadioControlCalibration()
-{
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC);
-    sendMessage(msg);
+{// FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_DO_RC_CALIB;
+//    sendMessage(msg);
 }
 
 void UAS::startDataRecording()
 {
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START);
+//    sendMessage(msg);
 }
 
 void UAS::pauseDataRecording()
 {
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE);
+//    sendMessage(msg);
 }
 
 void UAS::stopDataRecording()
 {
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP);
+//    sendMessage(msg);
 }
 
 void UAS::startMagnetometerCalibration()
 {
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG);
+//    sendMessage(msg);
 }
 
 void UAS::startGyroscopeCalibration()
 {
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO);
+//    sendMessage(msg);
 }
 
 void UAS::startPressureCalibration()
 {
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE);
+//    sendMessage(msg);
 }
 
 quint64 UAS::getUnixTime(quint64 time)
@@ -1589,19 +1596,21 @@ void UAS::requestParameters()
 
 void UAS::writeParametersToStorage()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE);
-    //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE);
+//    //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE);
+//    sendMessage(msg);
 }
 
 void UAS::readParametersFromStorage()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ);
+//    sendMessage(msg);
 }
 
 void UAS::enableAllDataTransmission(int rate)
@@ -1894,10 +1903,10 @@ void UAS::setSystemType(int systemType)
     {
         switch (systemType)
         {
-        case MAV_FIXED_WING:
+        case MAV_TYPE_FIXED_WING:
             airframe = QGC_AIRFRAME_EASYSTAR;
             break;
-        case MAV_QUADROTOR:
+        case MAV_TYPE_QUADROTOR:
             airframe = QGC_AIRFRAME_MIKROKOPTER;
             break;
         }
@@ -1945,31 +1954,19 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
     sendMessage(msg);
 }
 
-/**
- * Sets an action
- *
- **/
-void UAS::setAction(MAV_ACTION action)
-{
-    mavlink_message_t msg;
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action);
-    // Send message twice to increase chance that it reaches its goal
-    sendMessage(msg);
-    sendMessage(msg);
-}
-
 /**
  * Launches the system
  *
  **/
 void UAS::launch()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 /**
@@ -1978,12 +1975,12 @@ void UAS::launch()
  **/
 void UAS::enable_motors()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+//    mavlink_message_t msg;
+//    // FIXME MAVLINKV10PORTINGNEEDED
+//    //mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 /**
@@ -1992,12 +1989,12 @@ void UAS::enable_motors()
  **/
 void UAS::disable_motors()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+//    mavlink_message_t msg;
+//    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust)
@@ -2113,33 +2110,36 @@ void UAS::clearWaypointList()
 
 void UAS::halt()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 void UAS::go()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_CONTINUE);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_CONTINUE);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 /** Order the robot to return home / to land on the runway **/
 void UAS::home()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_RETURN);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_RETURN);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 /**
@@ -2148,96 +2148,102 @@ void UAS::home()
  */
 void UAS::emergencySTOP()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    // FIXME MAVLINKV10PORTINGNEEDED
+//    //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 /**
- * All systems are immediately shut down (e.g. the main power line is cut).
+ * Shut down this mav - All onboard systems are immediately shut down (e.g. the main power line is cut).
  * @warning This might lead to a crash
  *
  * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards
  */
 bool UAS::emergencyKILL()
 {
-    bool result = false;
-    QMessageBox msgBox;
-    msgBox.setIcon(QMessageBox::Critical);
-    msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS");
-    msgBox.setInformativeText("Do you want to cut power on all systems?");
-    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
-    msgBox.setDefaultButton(QMessageBox::Cancel);
-    int ret = msgBox.exec();
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    bool result = false;
+//    QMessageBox msgBox;
+//    msgBox.setIcon(QMessageBox::Critical);
+//    msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS");
+//    msgBox.setInformativeText("Do you want to cut power on all systems?");
+//    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
+//    msgBox.setDefaultButton(QMessageBox::Cancel);
+//    int ret = msgBox.exec();
 
-    // Close the message box shortly after the click to prevent accidental clicks
-    QTimer::singleShot(5000, &msgBox, SLOT(reject()));
+//    // Close the message box shortly after the click to prevent accidental clicks
+//    QTimer::singleShot(5000, &msgBox, SLOT(reject()));
 
 
-    if (ret == QMessageBox::Yes)
-    {
-        mavlink_message_t msg;
-        // TODO Replace MG System ID with static function call and allow to change ID in GUI
-        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL);
-        // Send message twice to increase chance of reception
-        sendMessage(msg);
-        sendMessage(msg);
-        result = true;
-    }
-    return result;
+//    if (ret == QMessageBox::Yes)
+//    {
+//        mavlink_message_t msg;
+//        // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL);
+//        // Send message twice to increase chance of reception
+//        sendMessage(msg);
+//        sendMessage(msg);
+//        result = true;
+//    }
+//    return result;
+    return false;
 }
 
 void UAS::startHil()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_START_HILSIM);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_START_HILSIM);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 void UAS::stopHil()
 {
-    mavlink_message_t msg;
-    // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_STOP_HILSIM);
-    // Send message twice to increase chance of reception
-    sendMessage(msg);
-    sendMessage(msg);
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    mavlink_message_t msg;
+//    // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_STOP_HILSIM);
+//    // Send message twice to increase chance of reception
+//    sendMessage(msg);
+//    sendMessage(msg);
 }
 
 
 void UAS::shutdown()
 {
-    bool result = false;
-    QMessageBox msgBox;
-    msgBox.setIcon(QMessageBox::Critical);
-    msgBox.setText("Shutting down the UAS");
-    msgBox.setInformativeText("Do you want to shut down the onboard computer?");
+    // FIXME MAVLINKV10PORTINGNEEDED
+//    bool result = false;
+//    QMessageBox msgBox;
+//    msgBox.setIcon(QMessageBox::Critical);
+//    msgBox.setText("Shutting down the UAS");
+//    msgBox.setInformativeText("Do you want to shut down the onboard computer?");
 
-    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
-    msgBox.setDefaultButton(QMessageBox::Cancel);
-    int ret = msgBox.exec();
+//    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
+//    msgBox.setDefaultButton(QMessageBox::Cancel);
+//    int ret = msgBox.exec();
 
-    // Close the message box shortly after the click to prevent accidental clicks
-    QTimer::singleShot(5000, &msgBox, SLOT(reject()));
+//    // Close the message box shortly after the click to prevent accidental clicks
+//    QTimer::singleShot(5000, &msgBox, SLOT(reject()));
 
-    if (ret == QMessageBox::Yes)
-    {
-        // If the active UAS is set, execute command
-        mavlink_message_t msg;
-        // TODO Replace MG System ID with static function call and allow to change ID in GUI
-        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN);
-        // Send message twice to increase chance of reception
-        sendMessage(msg);
-        sendMessage(msg);
+//    if (ret == QMessageBox::Yes)
+//    {
+//        // If the active UAS is set, execute command
+//        mavlink_message_t msg;
+//        // TODO Replace MG System ID with static function call and allow to change ID in GUI
+//        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN);
+//        // Send message twice to increase chance of reception
+//        sendMessage(msg);
+//        sendMessage(msg);
 
-        result = true;
-    }
+//        result = true;
+//    }
 }
 
 void UAS::setTargetPosition(float x, float y, float z, float yaw)

src/uas/UAS.h

@@ -253,8 +253,6 @@ public slots:
     }
     /** @brief Set a new name **/
     void setUASName(const QString& name);
-    /** @brief Executes an action **/
-    void setAction(MAV_ACTION action);
     /** @brief Executes a command **/
     void executeCommand(MAV_CMD command);
     /** @brief Executes a command **/

src/uas/UASInterface.h

@@ -188,8 +188,6 @@ public slots:
 
     /** @brief Set a new name for the system */
     virtual void setUASName(const QString& name) = 0;
-    /** @brief Sets an action **/
-    virtual void setAction(MAV_ACTION action) = 0;
     /** @brief Execute command immediately **/
     virtual void executeCommand(MAV_CMD command) = 0;
     /** @brief Executes a command **/

src/uas/UASWaypointManager.cc

@@ -586,8 +586,10 @@ int UASWaypointManager::getLocalFrameCount()
     // Search through all waypoints,
     // counting only those in global frame
     int i = 0;
-    foreach (Waypoint* p, waypoints) {
-        if (p->getFrame() == MAV_FRAME_GLOBAL) {
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_GLOBAL)
+        {
             i++;
         }
     }
@@ -600,8 +602,10 @@ int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
     // Search through all waypoints,
     // counting only those in local frame
     int i = 0;
-    foreach (Waypoint* p, waypoints) {
-        if (p->getFrame() == MAV_FRAME_LOCAL) {
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
+        {
             if (p == wp) {
                 return i;
             }
@@ -617,7 +621,8 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
     // Search through all waypoints,
     // counting only those in mission frame
     int i = 0;
-    foreach (Waypoint* p, waypoints) {
+    foreach (Waypoint* p, waypoints)
+    {
         if (p->getFrame() == MAV_FRAME_MISSION) {
             if (p == wp) {
                 return i;

src/ui/DebugConsole.cc

@@ -355,7 +355,7 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes)
         for (int j = 0; j < len; j++)
         {
             unsigned char byte = bytes.at(j);
-            // Filter MAVLink (http://pixhawk.ethz.ch/wiki/mavlink/) messages out of the stream.
+            // Filter MAVLink (http://qgroundcontrol.org/mavlink/) messages out of the stream.
             if (filterMAVLINK)
             {
                 if (this->bytesToIgnore > 0)

thirdParty/mavlink/include/protocol.h

@@ -6,10 +6,6 @@
 
 #include "checksum.h"
 
-#ifdef MAVLINK_CHECK_LENGTH
-extern const uint8_t mavlink_msg_lengths[256];
-#endif
-
 /**
  * @brief Initialize the communication stack
  *
@@ -439,521 +435,4 @@ static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint
 #define mavlink_send_msg( a, b ) mavlink_send_uart( a, b )
 #endif
 
-#define FILE_FINISHED
-
-#ifndef FILE_FINISHED
-/**
- * This is a convenience function which handles the complete MAVLink parsing.
- * the function will parse one byte at a time and return the complete packet once
- * it could be successfully decoded. Checksum and other failures will be silently
- * ignored.
- *
- * @param chan     ID of the current channel. This allows to parse different channels with this function.
- *                 a channel is not a physical message channel like a serial port, but a logic partition of
- *                 the communication streams in this case. COMM_NB is the limit for the number of channels
- *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
- * @param c        The char to barse
- *
- * @param returnMsg NULL if no message could be decoded, the message data else
- * @return 0 if no message could be decoded, 1 else
- *
- * A typical use scenario of this function call is:
- *
- * @code
- * #include <inttypes.h> // For fixed-width uint8_t type
- *
- * mavlink_message_t msg;
- * int chan = 0;
- *
- *
- * while(serial.bytesAvailable > 0)
- * {
- *   uint8_t byte = serial.getNextByte();
- *   if (mavlink_parse_char(chan, byte, &msg))
- *     {
- *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
- *     }
- * }
- *
- *
- * @endcode
- */
-
-#define MAVLINK_PACKET_START_CANDIDATES 50
-/*
-static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
-{
-	#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
-	    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
-	    static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2];
-	    static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH];
-	    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
-	    static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES];
-	    static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH];
-	    static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH];
-	#else
-	    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
-	    static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2];
-	    static uint8_t m_msgbuf_index[MAVLINK_COMM_NB];
-	    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
-	    static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES];
-	    static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB];
-	    static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB];
-	#endif
-
-	    // Set a packet start candidate index if sign is start sign
-	    if (c == MAVLINK_STX)
-	    {
-	    	m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan];
-	    }
-
-	    // Parse normally, if a CRC mismatch occurs retry with the next packet index
-}
-//#if (defined linux) | (defined __linux) | (defined  __MACH__) | (defined _WIN32)
-//    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
-//    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
-//#else
-//    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
-//    static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
-//#endif
-//// Initializes only once, values keep unchanged after first initialization
-//    mavlink_parse_state_initialize(&m_mavlink_status[chan]);
-//
-//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
-//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status
-//int bufferIndex = 0;
-//
-//status->msg_received = 0;
-//
-//switch (status->parse_state)
-//{
-//case MAVLINK_PARSE_STATE_UNINIT:
-//case MAVLINK_PARSE_STATE_IDLE:
-//            if (c == MAVLINK_STX)
-//	{
-//		status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
-//		mavlink_start_checksum(rxmsg);
-//	}
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_STX:
-//	if (status->msg_received)
-//	{
-//		status->buffer_overrun++;
-//		status->parse_error++;
-//		status->msg_received = 0;
-//		status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-//	}
-//	else
-//	{
-//		// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
-//		rxmsg->len = c;
-//		status->packet_idx = 0;
-//		mavlink_update_checksum(rxmsg, c);
-//		status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
-//	}
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_LENGTH:
-//	rxmsg->seq = c;
-//	mavlink_update_checksum(rxmsg, c);
-//	status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_SEQ:
-//	rxmsg->sysid = c;
-//	mavlink_update_checksum(rxmsg, c);
-//	status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_SYSID:
-//	rxmsg->compid = c;
-//	mavlink_update_checksum(rxmsg, c);
-//	status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_COMPID:
-//	rxmsg->msgid = c;
-//	mavlink_update_checksum(rxmsg, c);
-//	if (rxmsg->len == 0)
-//	{
-//		status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-//	}
-//	else
-//	{
-//		status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
-//	}
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_MSGID:
-//	rxmsg->payload[status->packet_idx++] = c;
-//	mavlink_update_checksum(rxmsg, c);
-//	if (status->packet_idx == rxmsg->len)
-//	{
-//		status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
-//	}
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
-//	if (c != rxmsg->ck_a)
-//	{
-//		// Check first checksum byte
-//		status->parse_error++;
-//		status->msg_received = 0;
-//		status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-//	}
-//	else
-//	{
-//		status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
-//	}
-//	break;
-//
-//case MAVLINK_PARSE_STATE_GOT_CRC1:
-//	if (c != rxmsg->ck_b)
-//	{// Check second checksum byte
-//		status->parse_error++;
-//		status->msg_received = 0;
-//		status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-//	}
-//	else
-//	{
-//		// Successfully got message
-//		status->msg_received = 1;
-//		status->parse_state = MAVLINK_PARSE_STATE_IDLE;
-//		memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
-//	}
-//	break;
-//}
-
-bufferIndex++;
-// If a message has been sucessfully decoded, check index
-if (status->msg_received == 1)
-{
-    //while(status->current_seq != rxmsg->seq)
-	//{
-	//	status->packet_rx_drop_count++;
-    //               status->current_seq++;
-	//}
-	status->current_seq = rxmsg->seq;
-	// Initial condition: If no packet has been received so far, drop count is undefined
-	if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
-	// Count this packet as received
-	status->packet_rx_success_count++;
-}
-
-r_mavlink_status->current_seq = status->current_seq+1;
-r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
-r_mavlink_status->packet_rx_drop_count = status->parse_error;
-return status->msg_received;
-}
- */
-
-
-typedef union __generic_16bit
-{
-	uint8_t b[2];
-	int16_t s;
-} generic_16bit;
-
-typedef union __generic_32bit
-{
-	uint8_t b[4];
-	float f;
-	int32_t i;
-	int16_t s;
-} generic_32bit;
-
-typedef union __generic_64bit
-{
-	uint8_t b[8];
-	int64_t ll; ///< Long long (64 bit)
-} generic_64bit;
-
-/**
- * @brief Place an unsigned byte into the buffer
- *
- * @param b the byte to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer)
-{
-	*(buffer + bindex) = b;
-	return sizeof(b);
-}
-
-/**
- * @brief Place a signed byte into the buffer
- *
- * @param b the byte to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer)
-{
-	*(buffer + bindex) = (uint8_t)b;
-	return sizeof(b);
-}
-
-/**
- * @brief Place two unsigned bytes into the buffer
- *
- * @param b the bytes to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer)
-{
-	buffer[bindex]   = (b>>8)&0xff;
-	buffer[bindex+1] = (b & 0xff);
-	return sizeof(b);
-}
-
-/**
- * @brief Place two signed bytes into the buffer
- *
- * @param b the bytes to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer)
-{
-	return put_uint16_t_by_index(b, bindex, buffer);
-}
-
-/**
- * @brief Place four unsigned bytes into the buffer
- *
- * @param b the bytes to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer)
-{
-	buffer[bindex]   = (b>>24)&0xff;
-	buffer[bindex+1] = (b>>16)&0xff;
-	buffer[bindex+2] = (b>>8)&0xff;
-	buffer[bindex+3] = (b & 0xff);
-	return sizeof(b);
-}
-
-/**
- * @brief Place four signed bytes into the buffer
- *
- * @param b the bytes to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer)
-{
-	buffer[bindex]   = (b>>24)&0xff;
-	buffer[bindex+1] = (b>>16)&0xff;
-	buffer[bindex+2] = (b>>8)&0xff;
-	buffer[bindex+3] = (b & 0xff);
-	return sizeof(b);
-}
-
-/**
- * @brief Place four unsigned bytes into the buffer
- *
- * @param b the bytes to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer)
-{
-	buffer[bindex]   = (b>>56)&0xff;
-	buffer[bindex+1] = (b>>48)&0xff;
-	buffer[bindex+2] = (b>>40)&0xff;
-	buffer[bindex+3] = (b>>32)&0xff;
-	buffer[bindex+4] = (b>>24)&0xff;
-	buffer[bindex+5] = (b>>16)&0xff;
-	buffer[bindex+6] = (b>>8)&0xff;
-	buffer[bindex+7] = (b & 0xff);
-	return sizeof(b);
-}
-
-/**
- * @brief Place four signed bytes into the buffer
- *
- * @param b the bytes to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer)
-{
-	return put_uint64_t_by_index(b, bindex, buffer);
-}
-
-/**
- * @brief Place a float into the buffer
- *
- * @param b the float to add
- * @param bindex the position in the packet
- * @param buffer the packet buffer
- * @return the new position of the last used byte in the buffer
- */
-static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer)
-{
-	generic_32bit g;
-	g.f = b;
-	return put_int32_t_by_index(g.i, bindex, buffer);
-}
-
-/**
- * @brief Place an array into the buffer
- *
- * @param b the array to add
- * @param length size of the array (for strings: length WITH '\0' char)
- * @param bindex the position in the packet
- * @param buffer packet buffer
- * @return new position of the last used byte in the buffer
- */
-static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer)
-{
-	memcpy(buffer+bindex, b, length);
-	return length;
-}
-
-/**
- * @brief Place a string into the buffer
- *
- * @param b the string to add
- * @param maxlength size of the array (for strings: length WITHOUT '\0' char)
- * @param bindex the position in the packet
- * @param buffer packet buffer
- * @return new position of the last used byte in the buffer
- */
-static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer)
-{
-	uint16_t length = 0;
-	// Copy string into buffer, ensuring not to exceed the buffer size
-	int i;
-	for (i = 1; i < maxlength; i++)
-	{
-		length++;
-		// String characters
-		if (i < (maxlength - 1))
-		{
-			buffer[bindex+i] = b[i];
-			// Stop at null character
-			if (b[i] == '\0')
-			{
-				break;
-			}
-		}
-		// Enforce null termination at end of buffer
-		else if (i == (maxlength - 1))
-		{
-			buffer[i] = '\0';
-		}
-	}
-	// Write length into first field
-	put_uint8_t_by_index(length, bindex, buffer);
-	return length;
-}
-
-/**
- * @brief Put a bitfield of length 1-32 bit into the buffer
- *
- * @param b the value to add, will be encoded in the bitfield
- * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
- * @param packet_index the position in the packet (the index of the first byte to use)
- * @param bit_index the position in the byte (the index of the first bit to use)
- * @param buffer packet buffer to write into
- * @return new position of the last used byte in the buffer
- */
-static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
-{
-	uint16_t bits_remain = bits;
-	// Transform number into network order
-	generic_32bit bin;
-	generic_32bit bout;
-	uint8_t i_bit_index, i_byte_index, curr_bits_n;
-	bin.i = b;
-	bout.b[0] = bin.b[3];
-	bout.b[1] = bin.b[2];
-	bout.b[2] = bin.b[1];
-	bout.b[3] = bin.b[0];
-
-	// buffer in
-	// 01100000 01000000 00000000 11110001
-	// buffer out
-	// 11110001 00000000 01000000 01100000
-
-	// Existing partly filled byte (four free slots)
-	// 0111xxxx
-
-	// Mask n free bits
-	// 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
-	// = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
-
-	// Shift n bits into the right position
-	// out = in >> n;
-
-	// Mask and shift bytes
-	i_bit_index = bit_index;
-	i_byte_index = packet_index;
-	if (bit_index > 0)
-	{
-		// If bits were available at start, they were available
-		// in the byte before the current index
-		i_byte_index--;
-	}
-
-	// While bits have not been packed yet
-	while (bits_remain > 0)
-	{
-		// Bits still have to be packed
-		// there can be more than 8 bits, so
-		// we might have to pack them into more than one byte
-
-		// First pack everything we can into the current 'open' byte
-		//curr_bits_n = bits_remain << 3; // Equals  bits_remain mod 8
-		//FIXME
-		if (bits_remain <= (8 - i_bit_index))
-		{
-			// Enough space
-			curr_bits_n = bits_remain;
-		}
-		else
-		{
-			curr_bits_n = (8 - i_bit_index);
-		}
-		
-		// Pack these n bits into the current byte
-		// Mask out whatever was at that position with ones (xxx11111)
-		buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
-		// Put content to this position, by masking out the non-used part
-		buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i);
-		
-		// Increment the bit index
-		i_bit_index += curr_bits_n;
-
-		// Now proceed to the next byte, if necessary
-		bits_remain -= curr_bits_n;
-		if (bits_remain > 0)
-		{
-			// Offer another 8 bits / one byte
-			i_byte_index++;
-			i_bit_index = 0;
-		}
-	}
-	
-	*r_bit_index = i_bit_index;
-	// If a partly filled byte is present, mark this as consumed
-	if (i_bit_index != 7) i_byte_index++;
-	return i_byte_index - packet_index;
-}
-*/
-#endif /* FILE_FINISHED */
-
 #endif /* _MAVLINK_PROTOCOL_H_ */