Mount_MAVLink: handle RC and GPS targeting in lib

Previously we expected the mount to do this but it is likely that the
first versions of MAVLink enable mounts will only be capable of pointing
at a particular angle

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -11,14 +11,43 @@ void AP_Mount_MAVLink::init()
 // update mount position - should be called periodically
 void AP_Mount_MAVLink::update()
 {
-    // nothing to do for a MAVlink gimbal that supports all modes:
-    //  MAV_MOUNT_MODE_RETRACT - nothing to do if the mount holds the retracted angles and
-    //                           we do not implement a separate servo based retract mechanism
-    //  MAV_MOUNT_MODE_NEUTRAL - nothing to do if the mount holds the neutral angles
-    //  MAV_MOUNT_MODE_MAVLINK_TARGETING - targets should already have been sent by a call to control_msg
-    //  MAV_MOUNT_MODE_RC_TARGETING - mount should be able to see RC inputs published by flight controller
-    //  MAV_MOUNT_MODE_GPS_POINT - mount should have received target sent from control_msg and
-    //                             should know vehicle's position which has been published by flight controller
+
+    // update based on mount mode
+    switch(_frontend.get_mode(_instance)) {
+        // move mount to a "retracted" position.  we do not implement a separate servo based retract mechanism
+        case MAV_MOUNT_MODE_RETRACT:
+            send_angle_target(_frontend.state[_instance]._retract_angles.get(), true);
+            break;
+
+        // move mount to a neutral position, typically pointing forward
+        case MAV_MOUNT_MODE_NEUTRAL:
+            send_angle_target(_frontend.state[_instance]._neutral_angles.get(), true);
+            break;
+
+        // point to the angles given by a mavlink message
+        case MAV_MOUNT_MODE_MAVLINK_TARGETING:
+            // do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
+            break;
+
+        // RC radio manual angle control, but with stabilization from the AHRS
+        case MAV_MOUNT_MODE_RC_TARGETING:
+            // update targets using pilot's rc inputs
+            update_targets_from_rc();
+            send_angle_target(_angle_ef_target_rad, false);
+            break;
+
+        // point mount to a GPS point given by the mission planner
+        case MAV_MOUNT_MODE_GPS_POINT:
+            if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+                calc_angle_to_location(_frontend.state[_instance]._roi_target, _angle_ef_target_rad, true, true);
+                send_angle_target(_angle_ef_target_rad, false);
+            }
+            break;
+
+        default:
+            // we do not know this mode so do nothing
+            break;
+    }
 }
 
 // has_pan_control - returns true if this mount can control it's pan (required for multicopters)
@@ -36,17 +65,33 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
         return;
     }
 
+    // map requested mode to mode that mount can actually support
+    enum MAV_MOUNT_MODE mode_to_send = mode;
+    switch (mode) {
+        case MAV_MOUNT_MODE_RETRACT:
+        case MAV_MOUNT_MODE_NEUTRAL:
+        case MAV_MOUNT_MODE_MAVLINK_TARGETING:
+        case MAV_MOUNT_MODE_RC_TARGETING:
+        case MAV_MOUNT_MODE_GPS_POINT:
+            mode_to_send = MAV_MOUNT_MODE_MAVLINK_TARGETING;
+            break;
+        default:
+            // unknown mode so just send it and hopefully gimbal supports it
+            break;
+    }
+
     // prepare and send command_long message with DO_SET_MODE command
     mavlink_msg_command_long_send(
             _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
             MAV_CMD_DO_SET_MODE,    // command number
             0,  // confirmation: 0=first confirmation of this command
-            mode,   // param1: mode
+            mode_to_send,   // param1: mode
             0,      // param2: custom mode
             0.0f, 0.0f, 0.0f,0.0f, 0.0f);    // param3 ~ param 7: not used
 
     // record the mode change
     _frontend.state[_instance]._mode = mode;
+    _last_mode = mode_to_send;
 }
 
 // set_roi_target - sets target location that mount should attempt to point towards
@@ -124,13 +169,24 @@ void AP_Mount_MAVLink::find_mount()
 }
 
 // send_angle_target - send earth-frame angle targets to mount
-void AP_Mount_MAVLink::send_angle_target(const Vector3f& target_deg)
+void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_degrees)
 {
     // exit immediately if mount has not been found
     if (!_enabled) {
         return;
     }
 
+    // convert to degrees if necessary
+    Vector3f target_deg = target;
+    if (!target_in_degrees) {
+        target_deg *= RAD_TO_DEG;
+    }
+
+    // exit immediately if mode and targets have not changed since last time they were sent
+    if (_frontend.state[_instance]._mode == MAV_MOUNT_MODE_MAVLINK_TARGETING && target_deg == _last_angle_target) {
+        return;
+    }
+
     // prepare and send command_long message with DO_MOUNT_CONTROL command
     mavlink_msg_command_long_send(
             _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
@@ -141,4 +197,8 @@ void AP_Mount_MAVLink::send_angle_target(const Vector3f& target_deg)
             target_deg.z,   // param3: yaw (in degrees) or alt (in meters).
             0.0f,0.0f,0.0f,                     // param4 ~ param6 : not used
             MAV_MOUNT_MODE_MAVLINK_TARGETING);  // param7: MAV_MOUNT_MODE enum value
+
+    // store sent target and mode
+    _last_angle_target = target_deg;
+    _last_mode = MAV_MOUNT_MODE_MAVLINK_TARGETING;
 }

libraries/AP_Mount/AP_Mount_MAVLink.h

@@ -25,7 +25,8 @@ public:
     AP_Mount_MAVLink(AP_Mount &frontend, uint8_t instance) :
         AP_Mount_Backend(frontend, instance),
         _enabled(false),
-        _chan(MAVLINK_COMM_0)
+        _chan(MAVLINK_COMM_0),
+        _last_mode(MAV_MOUNT_MODE_RETRACT)
     {}
 
     // init - performs any required initialisation for this instance
@@ -57,12 +58,15 @@ private:
     // find_mount - search for MAVLink enabled mount
     void find_mount();
 
-    // send_angle_target - send earth-frame angle targets (in degrees) to mount
-    void send_angle_target(const Vector3f& target_deg);
+    // send_angle_target - send earth-frame angle targets to mount
+    //  set target_in_degrees to true to send degree targets, false if target in radians
+    void send_angle_target(const Vector3f& target, bool target_in_degrees);
 
     // internal variables
     bool    _enabled;           // gimbal becomes enabled once the mount has been discovered
     mavlink_channel_t _chan;    // telemetry channel used to communicate with mount
+    enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount
+    Vector3f _last_angle_target;    // last angle target sent to mount
 };
 
 #endif // __AP_MOUNT_MAVLINK_H__