AP_Mount: mount_open servo output should work for all backends ArduPi…

…lot#26892
dhruvak001 · Jun 29, 2024 · 8d03f1c · 8d03f1c
1 parent 2b60e93
commit 8d03f1c
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 3 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -59,10 +59,22 @@ bool AP_Mount_Backend::valid_mode(MAV_MOUNT_MODE mode) const
 
 bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
 {
-    if (!valid_mode(mode)) {
+    // Add a check for the new mode
+    if (!valid_mode(mode) && mode != MAV_MOUNT_MODE_TOGGLE_RC) {
         return false;
     }
+
     _mode = mode;
+
+    // handle the toggle mode
+    if (_mode == MAV_MOUNT_MODE_TOGGLE_RC) {
+        if (_mode == MAV_MOUNT_MODE_RC_TARGETING) {
+            _mode = MAV_MOUNT_MODE_NEUTRAL;
+        } else {
+            _mode = MAV_MOUNT_MODE_RC_TARGETING;
+        }
+    }
+
     return true;
 }
 

diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp
@@ -40,6 +40,15 @@ void AP_Mount_Servo::update()
             break;
         }
 
+        case MAV_MOUNT_MODE_TOGGLE_RC: {
+            if (_mode == MAV_MOUNT_MODE_RC_TARGETING) {
+                set_mode(MAV_MOUNT_MODE_NEUTRAL);
+            } else {
+                set_mode(MAV_MOUNT_MODE_RC_TARGETING);
+            }
+            break;
+        }
+
         // move mount to a neutral position, typically pointing forward
         case MAV_MOUNT_MODE_NEUTRAL: {
             _angle_bf_output_rad = _params.neutral_angles.get() * DEG_TO_RAD;
@@ -102,8 +111,7 @@ void AP_Mount_Servo::update()
             update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
             FALLTHROUGH;
         case MountTargetType::ANGLE:
-            // update _angle_bf_output_rad based on angle target
-            if ((mount_mode != MAV_MOUNT_MODE_RETRACT) & (mount_mode != MAV_MOUNT_MODE_NEUTRAL)) {
+            if ((mount_mode != MAV_MOUNT_MODE_RETRACT) && (mount_mode != MAV_MOUNT_MODE_NEUTRAL)) {
                 update_angle_outputs(mnt_target.angle_rad);
             }
             break;