Rover: add support for SET_POSITION_TARGET_GLOBAL_INT

Also supports SET_POSITION_TARGET_LOCAL_NED but for both messages only the position fields are consumed.  Support for velocity, acceleration and yaw fields are not included.

APMrover2/GCS_Mavlink.cpp

@@ -1250,6 +1250,88 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
+        {
+            // decode packet
+            mavlink_set_position_target_local_ned_t packet;
+            mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
+
+            // exit if vehicle is not in Guided mode
+            if (rover.control_mode != GUIDED) {
+                break;
+            }
+
+            // check for supported coordinate frames
+            if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
+                packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
+                packet.coordinate_frame != MAV_FRAME_BODY_NED &&
+                packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
+                break;
+            }
+
+            bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
+
+            // prepare and send target position
+            if (!pos_ignore) {
+                Location loc = rover.current_loc;
+                if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
+                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+                    // rotate from body-frame to NE frame
+                    float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw();
+                    float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw();
+                    // add offset to current location
+                    location_offset(loc, ne_x, ne_y);
+                } else if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED) {
+                    // add offset to current location
+                    location_offset(loc, packet.x, packet.y);
+                } else {
+                    // MAV_FRAME_LOCAL_NED interpret as an offset from home
+                    loc = rover.ahrs.get_home();
+                    location_offset(loc, packet.x, packet.y);
+                }
+
+                rover.guided_WP = loc;
+                rover.rtl_complete = false;
+                rover.set_guided_WP();
+            }
+
+            break;
+        }
+
+    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
+        {
+            // decode packet
+            mavlink_set_position_target_global_int_t packet;
+            mavlink_msg_set_position_target_global_int_decode(msg, &packet);
+
+            // exit if vehicle is not in Guided mode
+            if (rover.control_mode != GUIDED) {
+                break;
+            }
+
+            // check for supported coordinate frames
+            if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
+                packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
+                packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
+                packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
+                break;
+            }
+
+            bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
+
+            // prepare and send target position
+            if (!pos_ignore) {
+                Location loc = rover.current_loc;
+                loc.lat = packet.lat_int;
+                loc.lng = packet.lon_int;
+                rover.guided_WP = loc;
+                rover.rtl_complete = false;
+                rover.set_guided_WP();
+            }
+
+            break;
+        }
+
     case MAVLINK_MSG_ID_GPS_INPUT:
         {
             rover.gps.handle_msg(msg);

APMrover2/capabilities.cpp

@@ -6,5 +6,7 @@ void Rover::init_capabilities(void)
 {
     hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
                                MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
-                               MAV_PROTOCOL_CAPABILITY_MISSION_INT);
+                               MAV_PROTOCOL_CAPABILITY_MISSION_INT |
+                               MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
+                               MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
 }

APMrover2/defines.h

@@ -68,3 +68,11 @@ enum mode {
 #define MASK_LOG_RC     		(1<<14)
 #define MASK_LOG_ARM_DISARM     (1<<15)
 #define MASK_LOG_IMU_RAW        (1UL<<19)
+
+// for mavlink SET_POSITION_TARGET messages
+#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE      ((1<<0) | (1<<1) | (1<<2))
+#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE      ((1<<3) | (1<<4) | (1<<5))
+#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE      ((1<<6) | (1<<7) | (1<<8))
+#define MAVLINK_SET_POS_TYPE_MASK_FORCE           (1<<9)
+#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE      (1<<10)
+#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)