AP_Mount: improve handling of invalid target

rmackay9 · Jul 20, 2023 · 66a6f67 · 66a6f67
1 parent 6bfe4ad
commit 66a6f67
Show file tree

Hide file tree

Showing 6 changed files with 23 additions and 24 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp
@@ -45,13 +45,12 @@ void AP_Mount_Alexmos::update()
 
         // point to the angles given by a mavlink message
         case MAV_MOUNT_MODE_MAVLINK_TARGETING:
-            // mavlink targets are stored while handling the incoming message and are considered valid
+            // mavlink targets are stored while handling the incoming message
             break;
 
         // RC radio manual angle control, but with stabilization from the AHRS
         case MAV_MOUNT_MODE_RC_TARGETING: {
             // update targets using pilot's RC inputs
-
             if (get_rc_rate_target(mnt_target.rate_rads)) {
                 mnt_target.target_type = MountTargetType::RATE;
             } else if (get_rc_angle_target(mnt_target.angle_rad)) {
@@ -87,13 +86,11 @@ void AP_Mount_Alexmos::update()
         case MountTargetType::RATE:
             update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
             FALLTHROUGH;
+        case MountTargetType::INVALID:
         case MountTargetType::ANGLE:
-            // send latest targets to gimbal
+            // send latest angle targets to gimbal
             control_axis(mnt_target.angle_rad);
             break;
-        default:
-            // invalid target, Do nothing
-            break;
     }
 }
 

diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp
@@ -40,7 +40,7 @@ void AP_Mount_Gremsy::update()
         }
 
         case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
-            // mavlink targets are stored while handling the incoming message set_angle_target() or set_rate_target() and are considered valid
+            // mavlink targets are stored while handling the incoming message set_angle_target() or set_rate_target()
             break;
         }
 
@@ -82,15 +82,16 @@ void AP_Mount_Gremsy::update()
 
     // send target angles or rates depending on the target type
     switch (mnt_target.target_type) {
+        case MountTargetType::INVALID:
+            // send zero rates
+            send_gimbal_device_set_rate(0, 0, 0, false);
+            break;
         case MountTargetType::ANGLE:
             send_gimbal_device_set_attitude(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
             break;
         case MountTargetType::RATE:
             send_gimbal_device_set_rate(mnt_target.rate_rads.roll, mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
             break;
-        default:
-            // invalid target, Do nothing
-            break;
     }
 }
 

diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp
@@ -92,15 +92,14 @@ void AP_Mount_Servo::update()
         case MountTargetType::RATE:
             update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
             FALLTHROUGH;
+        case MountTargetType::INVALID:
+            // use lastest angle targets
         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)) {
                 update_angle_outputs(mnt_target.angle_rad);
             }
             break;
-        default:
-            // invalid target, Do nothing
-            break;
     }
 
     // move mount to a "retracted position" into the fuselage with a fourth servo

diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp
@@ -85,7 +85,7 @@ void AP_Mount_Siyi::update()
         }
 
         case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
-            // mavlink targets are stored while handling the incoming message and are considered valid
+            // mavlink targets are stored while handling the incoming message
             break;
         }
 
@@ -127,15 +127,16 @@ void AP_Mount_Siyi::update()
 
     // send target angles or rates depending on the target type
     switch (mnt_target.target_type) {
+        case MountTargetType::INVALID:
+            // send zero rates
+            send_target_rates(0, 0, false);
+            break;
         case MountTargetType::ANGLE:
             send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
             break;
         case MountTargetType::RATE:
             send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
             break;
-        default:
-            // invalid target, Do nothing
-            break;
     }
 }
 

diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp
@@ -131,15 +131,16 @@ void AP_Mount_Viewpro::update()
 
     // send target angles or rates depending on the target type
     switch (mnt_target.target_type) {
+        case MountTargetType::INVALID:
+            // send zero rates
+            send_target_rates(0, 0, false);
+            break;
         case MountTargetType::ANGLE:
             send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
             break;
         case MountTargetType::RATE:
             send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
             break;
-        default:
-            // invalid target, Do nothing
-            break;
     }
 }
 

diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp
@@ -67,7 +67,7 @@ void AP_Mount_Xacti::update()
         }
 
         case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
-            // mavlink targets are stored while handling the incoming message and are considered valid
+            // mavlink targets are set while handling the incoming message
             break;
         }
 
@@ -109,15 +109,15 @@ void AP_Mount_Xacti::update()
 
     // send target angles or rates depending on the target type
     switch (mnt_target.target_type) {
+        case MountTargetType::INVALID:
+            send_target_rates(0, 0, false);
+            break;
         case MountTargetType::ANGLE:
             send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
             break;
         case MountTargetType::RATE:
             send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
             break;
-        default:
-            // invalid target, Do nothing
-            break;
     }
 }