Mount: improve yaw lock reporting to GCS

libraries/AP_Mount/AP_Mount.cpp

@@ -249,7 +249,7 @@ void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode)
     backend->set_mode(mode);
 }
 
-// set yaw_lock.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
+// set yaw_lock used in RC_TARGETING mode.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
 // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
 void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock)
 {
@@ -358,6 +358,12 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
         return MAV_RESULT_ACCEPTED;
     }
 
+    // if neither angles nor rates were provided set the RC_TARGETING yaw lock state
+    if (isnan(pitch_angle_deg) && isnan(yaw_angle_deg) && isnan(pitch_rate_degs) && isnan(yaw_rate_degs)) {
+        backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
+        return MAV_RESULT_ACCEPTED;
+    }
+
     return MAV_RESULT_FAILED;
 }
 
@@ -498,6 +504,12 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg)
         backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
         return;
     }
+
+    // if neither angles nor rates were provided set the RC_TARGETING yaw lock state
+    if (isnan(packet.pitch) && isnan(packet.yaw) && isnan(packet.pitch_rate) && isnan(packet.yaw_rate)) {
+        backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
+        return;
+    }
 }
 
 MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet)

libraries/AP_Mount/AP_Mount.h

@@ -151,7 +151,7 @@ class AP_Mount
     void set_mode_to_default() { set_mode_to_default(_primary); }
     void set_mode_to_default(uint8_t instance);
 
-    // set yaw_lock.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
+    // set yaw_lock used in RC_TARGETING mode.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
     // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
     void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); }
     void set_yaw_lock(uint8_t instance, bool yaw_lock);

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -91,6 +91,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y
 
     // set the mode to mavlink targeting
     set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(yaw_is_earth_frame);
+    }
 }
 
 // sets rate target in deg/s
@@ -106,6 +111,11 @@ void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float
 
     // set the mode to mavlink targeting
     set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(yaw_is_earth_frame);
+    }
 }
 
 // set_roi_target - sets target location that mount should attempt to point towards
@@ -117,6 +127,11 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc)
 
     // set the mode to GPS tracking mode
     set_mode(MAV_MOUNT_MODE_GPS_POINT);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(true);
+    }
 }
 
 // clear_roi_target - clears target location that mount should attempt to point towards
@@ -139,6 +154,11 @@ void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
 
     // set the mode to sysid tracking mode
     set_mode(MAV_MOUNT_MODE_SYSID_TARGET);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(true);
+    }
 }
 
 #if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED
@@ -797,10 +817,45 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad
 // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
 uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
 {
+    // get yaw lock state by mode
+    bool yaw_lock_state = false;
+    switch (_mode) {
+    case MAV_MOUNT_MODE_RETRACT:
+    case MAV_MOUNT_MODE_NEUTRAL:
+        // these modes always use body-frame yaw (aka follow)
+        yaw_lock_state = false;
+        break;
+    case MAV_MOUNT_MODE_MAVLINK_TARGETING:
+        switch (mnt_target.target_type) {
+        case MountTargetType::RATE:
+            yaw_lock_state = mnt_target.rate_rads.yaw_is_ef;
+            break;
+        case MountTargetType::ANGLE:
+            yaw_lock_state = mnt_target.angle_rad.yaw_is_ef;
+            break;
+        }
+        break;
+    case MAV_MOUNT_MODE_RC_TARGETING:
+        yaw_lock_state = _yaw_lock;
+        break;
+    case MAV_MOUNT_MODE_GPS_POINT:
+    case MAV_MOUNT_MODE_SYSID_TARGET:
+    case MAV_MOUNT_MODE_HOME_LOCATION:
+        // these modes always use earth-frame yaw (aka lock)
+        yaw_lock_state = true;
+        break;
+    case MAV_MOUNT_MODE_ENUM_END:
+        // unsupported
+        yaw_lock_state = false;
+        break;
+    }
+
     const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
                            (get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
                            GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
-                           GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // pitch angle is always earth-frame, yaw_angle is always body-frame
+                           GIMBAL_DEVICE_FLAGS_PITCH_LOCK| // pitch angle is always earth-frame, yaw_angle is always body-frame
+                           GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // yaw angle is always in vehicle-frame
+                           (yaw_lock_state ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
     return flags;
 }
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -79,7 +79,7 @@ class AP_Mount_Backend
     // set mount's mode
     bool set_mode(enum MAV_MOUNT_MODE mode);
 
-    // set yaw_lock.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
+    // set yaw_lock used in RC_TARGETING mode.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
     // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
     void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }
 
@@ -228,6 +228,12 @@ class AP_Mount_Backend
         void set(const Vector3f& rpy, bool yaw_is_ef_in);
     };
 
+    // options parameter bitmask handling
+    enum class Options : uint8_t {
+        RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode
+    };
+    bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; }
+
     // returns true if user has configured a valid yaw angle range
     // allows user to disable yaw even on 3-axis gimbal
     bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }
@@ -283,7 +289,7 @@ class AP_Mount_Backend
     uint8_t     _instance;  // this instance's number
 
     MAV_MOUNT_MODE  _mode;          // current mode (see MAV_MOUNT_MODE enum)
-    bool _yaw_lock;                 // True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame
+    bool _yaw_lock;                 // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame
 
     // structure for MAVLink Targeting angle and rate targets
     struct {

libraries/AP_Mount/AP_Mount_Params.cpp

@@ -165,6 +165,13 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO_FLAGS("_DEVID", 15, AP_Mount_Params, dev_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
 
+    // @Param: _OPTIONS
+    // @DisplayName: Mount options
+    // @Description: Mount options bitmask
+    // @Bitmask: 0:RC lock state from previous mode
+    // @User: Standard
+    AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0),
+
     AP_GROUPEND
 };
 

libraries/AP_Mount/AP_Mount_Params.h

@@ -31,4 +31,5 @@ class AP_Mount_Params {
     AP_Float    pitch_stb_lead;     // pitch lead control gain (only used by servo backend)
     AP_Int8     sysid_default;      // target sysid for mount to follow
     AP_Int32    dev_id;             // Device id taking into account bus
+    AP_Int8     options;            // mount options bitmask
 };