AP_Mount: Do not override default mode when first connecting to RC

ArduPilot · Apr 10, 2024 · 9e17d65 · 9e17d65
1 parent a83c29c
commit 9e17d65
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 12 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -618,22 +618,33 @@ void AP_Mount_Backend::set_rctargeting_on_rcinput_change()
     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 not in RC_TARGETING or RETRACT modes then check for RC change
+
+        if(last_rc_input.initialised)
+        {
+            // 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);
+                }
+        }
+        else
+        {
+            // The first time through, initial RC inputs should be set, but not used
+            last_rc_input.initialised = true;
+            last_rc_input.roll_in = roll_in;
+            last_rc_input.pitch_in = pitch_in;
+            last_rc_input.yaw_in = yaw_in;
         }
     }
 
-    // if in RC_TARGETING or RETRACT mode then store last RC input
+    // if NOW in RC_TARGETING or RETRACT mode then store last RC input (mode might have changed)
     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;

diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -329,6 +329,7 @@ class AP_Mount_Backend
 
     // structure holding the last RC inputs
     struct {
+        bool    initialised;
         int16_t roll_in;
         int16_t pitch_in;
         int16_t yaw_in;