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

ArduPilot · Apr 10, 2024 · 891104d · 891104d
1 parent 5e3f267
commit 891104d
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -618,8 +618,16 @@ 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 (!last_rc_input.initialised)
+    {
+            // 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 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) {
+    else 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);
@@ -629,11 +637,11 @@ void AP_Mount_Backend::set_rctargeting_on_rcinput_change()
         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);
+                set_mode(MAV_MOUNT_MODE_RC_TARGETING);
         }
     }
 
-    // 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;