AP_Mount: switch to RC_TARGETING on RC input

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -570,6 +570,47 @@ void AP_Mount_Backend::calculate_poi()
 }
 #endif
 
+// change to RC_TARGETING mode if rc inputs have changed by more than the dead zone
+// should be called on every update
+void AP_Mount_Backend::set_rctargeting_on_rcinput_change()
+{
+    // exit immediately if no RC input
+    if (!rc().has_valid_input()) {
+        return;
+    }
+
+    const RC_Channel *roll_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_ROLL : RC_Channel::AUX_FUNC::MOUNT2_ROLL);
+    const RC_Channel *pitch_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_PITCH : RC_Channel::AUX_FUNC::MOUNT2_PITCH);
+    const RC_Channel *yaw_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_YAW : RC_Channel::AUX_FUNC::MOUNT2_YAW);
+
+    // get rc input
+    const int16_t roll_in = (roll_ch == nullptr) ? 0 : roll_ch->get_radio_in();
+    const int16_t pitch_in = (pitch_ch == nullptr) ? 0 : pitch_ch->get_radio_in();
+    const int16_t yaw_in = (yaw_ch == nullptr) ? 0 : yaw_ch->get_radio_in();
+
+    // if not in RC_TARGETING or RETRACT modes then check for RC change
+    if (get_mode() != MAV_MOUNT_MODE_RC_TARGETING && get_mode() != MAV_MOUNT_MODE_RETRACT) {
+        // get dead zones
+        const int16_t roll_dz = (roll_ch == nullptr) ? 10 : MAX(roll_ch->get_dead_zone(), 10);
+        const int16_t pitch_dz = (pitch_ch == nullptr) ? 10 : MAX(pitch_ch->get_dead_zone(), 10);
+        const int16_t yaw_dz = (yaw_ch == nullptr) ? 10 : MAX(yaw_ch->get_dead_zone(), 10);
+
+        // check if RC input has changed by more than the dead zone
+        if ((abs(last_rc_input.roll_in - roll_in) > roll_dz) ||
+            (abs(last_rc_input.pitch_in - pitch_in) > pitch_dz) ||
+            (abs(last_rc_input.yaw_in - yaw_in) > yaw_dz)) {
+            set_mode(MAV_MOUNT_MODE_RC_TARGETING);
+        }
+    }
+
+    // if in RC_TARGETING or RETRACT mode then store last RC input
+    if (get_mode() == MAV_MOUNT_MODE_RC_TARGETING || get_mode() == MAV_MOUNT_MODE_RETRACT) {
+        last_rc_input.roll_in = roll_in;
+        last_rc_input.pitch_in = pitch_in;
+        last_rc_input.yaw_in = yaw_in;
+    }
+}
+
 // get pilot input (in the range -1 to +1) received through RC
 void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const
 {

libraries/AP_Mount/AP_Mount_Backend.h

@@ -240,6 +240,10 @@ class AP_Mount_Backend
     void calculate_poi();
 #endif
 
+    // change to RC_TARGETTING mode if rc inputs have changed by more than the dead zone
+    // should be called on every update
+    void set_rctargeting_on_rcinput_change();
+
     // get pilot input (in the range -1 to +1) received through RC
     void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
 
@@ -308,6 +312,13 @@ class AP_Mount_Backend
 
     uint32_t _last_warning_ms;      // system time of last warning sent to GCS
 
+    // structure holding the last RC inputs
+    struct {
+        int16_t roll_in;
+        int16_t pitch_in;
+        int16_t yaw_in;
+    } last_rc_input;
+
     // structure holding mavlink sysid and compid of controller of this gimbal
     // see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS
     struct {

libraries/AP_Mount/AP_Mount_Gremsy.cpp

@@ -20,6 +20,9 @@ void AP_Mount_Gremsy::update()
         return;
     }
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // update based on mount mode
     switch (get_mode()) {
 

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -18,6 +18,9 @@ void AP_Mount_SToRM32::update()
         return;
     }
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // flag to trigger sending target angles to gimbal
     bool resend_now = false;
 

libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp

@@ -29,6 +29,9 @@ void AP_Mount_SToRM32_serial::update()
 
     read_incoming(); // read the incoming messages from the gimbal
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // flag to trigger sending target angles to gimbal
     bool resend_now = false;
 

libraries/AP_Mount/AP_Mount_Scripting.cpp

@@ -15,6 +15,9 @@ extern const AP_HAL::HAL& hal;
 // update mount position - should be called periodically
 void AP_Mount_Scripting::update()
 {
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // update based on mount mode
     switch (get_mode()) {
         // move mount to a "retracted" position.  To-Do: remove support and replace with a relaxed mode?

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -27,6 +27,9 @@ void AP_Mount_Servo::init()
 // update mount position - should be called periodically
 void AP_Mount_Servo::update()
 {
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     auto mount_mode = get_mode();
     switch (mount_mode) {
         // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -103,6 +103,9 @@ void AP_Mount_Siyi::update()
     // run zoom control
     update_zoom_control();
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // Get the target angles or rates first depending on the current mount mode
     switch (get_mode()) {
         case MAV_MOUNT_MODE_RETRACT: {

libraries/AP_Mount/AP_Mount_SoloGimbal.cpp

@@ -31,6 +31,9 @@ void AP_Mount_SoloGimbal::update()
         return;
     }
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // update based on mount mode
     switch(get_mode()) {
         // move mount to a "retracted" position.  we do not implement a separate servo based retract mechanism

libraries/AP_Mount/AP_Mount_Viewpro.cpp

@@ -76,6 +76,9 @@ void AP_Mount_Viewpro::update()
     // send vehicle attitude and position
     send_m_ahrs();
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // if tracking is active we do not send new targets to the gimbal
     if (_last_tracking_status == TrackingStatus::SEARCHING || _last_tracking_status == TrackingStatus::TRACKING) {
         return;

libraries/AP_Mount/AP_Mount_Xacti.cpp

@@ -110,6 +110,9 @@ void AP_Mount_Xacti::update()
         return;
     }
 
+    // change to RC_TARGETING mode if RC input has changed
+    set_rctargeting_on_rcinput_change();
+
     // update based on mount mode
     switch (get_mode()) {
         // move mount to a "retracted" position.  To-Do: remove support and replace with a relaxed mode?