Merge pull request #811 from mavlink/mavlink_update

Mavlink update

v1.0 @ 4cd38400

-Subproject commit 613e56970458ff9003b966c5c6296a1d2eb05d2c
+Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742

src/comm/MAVLinkSimulationLink.cc

@@ -394,13 +394,7 @@ void MAVLinkSimulationLink::mainloop()
 //        y = (y < -5.0f) ? -5.0f : y;
 //        z = (z < -3.0f) ? -3.0f : z;
 
-        // Send back new setpoint
         mavlink_message_t ret;
-        mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); // spYaw/180.0*M_PI);
-        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
-        //add data into datastream
-        memcpy(stream+streampointer,buffer, bufferlength);
-        streampointer += bufferlength;
 
         // Send back new position
         mavlink_msg_local_position_ned_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed);
@@ -702,24 +696,6 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                 system.base_mode = mode.base_mode;
             }
             break;
-            case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
-            {
-                mavlink_set_local_position_setpoint_t set;
-                mavlink_msg_set_local_position_setpoint_decode(&msg, &set);
-                spX = set.x;
-                spY = set.y;
-                spZ = set.z;
-                spYaw = set.yaw;
-
-                // Send back new setpoint
-                mavlink_message_t ret;
-                mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw);
-                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
-                //add data into datastream
-                memcpy(stream+streampointer,buffer, bufferlength);
-                streampointer += bufferlength;
-            }
-            break;
             // EXECUTE OPERATOR ACTIONS
             case MAVLINK_MSG_ID_COMMAND_LONG:
             {

src/comm/MAVLinkSimulationMAV.cc

@@ -518,28 +518,5 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
     //        }
     //    }
     break;
-    case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: {
-        mavlink_set_local_position_setpoint_t sp;
-        mavlink_msg_set_local_position_setpoint_decode(&msg, &sp);
-        if (sp.target_system == this->systemid) {
-            nav_mode = 0;
-            previousSPX = nextSPX;
-            previousSPY = nextSPY;
-            previousSPZ = nextSPZ;
-            nextSPX = sp.x;
-            nextSPY = sp.y;
-            nextSPZ = sp.z;
-
-            // Rotary wing
-            //nextSPYaw = sp.yaw;
-
-            // Airplane
-            //yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
-
-            //if (!firstWP) firstWP = true;
-        }
-        //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
-    }
-    break;
     }
 }

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -522,33 +522,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
 */
 void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
 {
-    if(seq < waypoints->size()) {
-        mavlink_mission_item_t *cur = waypoints->at(seq);
-
-        // send new set point to local IMU
-        if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) {
-            mavlink_message_t msg;
-            mavlink_set_local_position_setpoint_t PControlSetPoint;
-
-            PControlSetPoint.target_system = systemid;
-            PControlSetPoint.target_component = MAV_COMP_ID_IMU;
-            PControlSetPoint.x = cur->x;
-            PControlSetPoint.y = cur->y;
-            PControlSetPoint.z = cur->z;
-            PControlSetPoint.yaw = cur->param4;
-            PControlSetPoint.coordinate_frame = cur->frame;
-            
-            mavlink_msg_set_local_position_setpoint_encode(systemid, compid, &msg, &PControlSetPoint);
-            link->sendMAVLinkMessage(&msg);
-            emit messageSent(msg);
-
-            uint64_t now = QGC::groundTimeMilliseconds();
-            timestamp_last_send_setpoint = now;
-        } else if (verbose) {
-            qDebug("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
-        }
-
-    }
+    Q_UNUSED(seq);
 }
 
 void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count)