ShipOps: More fixes

lthall · Jul 15, 2024 · 4ee3fab · 4ee3fab
1 parent b9af9ba
commit 4ee3fab
Showing 1 changed file with 13 additions and 13 deletions.
diff --git a/ArduCopter/mode_ship_ops.cpp b/ArduCopter/mode_ship_ops.cpp
@@ -319,7 +319,7 @@ void ModeShipOperation::run()
     float keep_out_angle_rad = wrap_2PI(radians(keep_out_CW) - keep_out_CCW_rad);
     float keep_out_CW_rad = keep_out_angle_rad + keep_out_CCW_rad;
     float koz_center_heading_rad = wrap_2PI(ship.heading + (keep_out_CW_rad + keep_out_CCW_rad) / 2.0);
-    if (fabsf(wrap_PI(radians(perch_angle) - koz_center_heading_rad)) <  keep_out_angle_rad / 2.0) {
+    if (fabs(wrap_PI(radians(perch_angle) - koz_center_heading_rad)) <  keep_out_angle_rad / 2.0) {
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Perch in KOZ");
         copter.do_failsafe_action(Copter::FailsafeAction::AUTO_DO_LAND_START, ModeReason::UNKNOWN);
     }
@@ -476,7 +476,7 @@ void ModeShipOperation::run()
             if (aircraft_vector_cm.length() < deck_radius * 100.0) {
                 // I did this to ensure I can't get a divide by zero but I suspect I could have just use || and been fine.
                 set_keep_out_zone_mode(KeepOutZoneMode::NO_ACTION);
-            } else if (fabsf(wrap_PI(aircraft_bearing_rad - koz_center_heading_rad)) >  keep_out_angle_rad / 2.0 - safe_asin(deck_radius * 100.0 / aircraft_vector_cm.length())) {
+            } else if (fabs(wrap_PI(aircraft_bearing_rad - koz_center_heading_rad)) >  keep_out_angle_rad / 2.0 - safe_asin(deck_radius * 100.0 / aircraft_vector_cm.length())) {
                 set_keep_out_zone_mode(KeepOutZoneMode::NO_ACTION);
             } else if (aircraft_vector_cm.length() < keep_out_radius * 100.0) {
                 set_keep_out_zone_mode(KeepOutZoneMode::EXIT_KOZ);
@@ -547,7 +547,7 @@ void ModeShipOperation::run()
             offset.z = -perch_height;
             break;
         case SubMode::OVER_SPOT:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::PAYLOAD_PLACE:
             // move over landing Spot at Perch altitude
             offset.zero();
@@ -581,18 +581,18 @@ void ModeShipOperation::run()
             pos_control->input_vel_accel_xy(vel_ned_ms.xy(), accel_ned.xy());
             break;
         case SubMode::RETURN_TO_PERCH:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::PERCH:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::OVER_SPOT:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::LAUNCH_RECOVERY:
             // relax horizontal position target if we might be landed
             // Do not put this in payload place as it does this already
             if (copter.ap.land_complete_maybe) {
                 pos_control->soften_for_landing_xy();
             }
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::PAYLOAD_PLACE:
             // move to target position and velocity
             Vector3p pos = ship.pos_ned.topostype();
@@ -605,11 +605,11 @@ void ModeShipOperation::run()
         // vertical navigation
         switch (_state) {
         case SubMode::CLIMB_TO_RTL:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::RETURN_TO_PERCH:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::PERCH:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::OVER_SPOT:
 // include vertical offset
             pos_control->set_alt_target_with_slew(-(ship.pos_ned.z + offset.z));
@@ -660,9 +660,9 @@ void ModeShipOperation::run()
             yaw_rate_cds = pos_control->get_yaw_rate_cds();
             break;
         case SubMode::PERCH:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::OVER_SPOT:
-            // FALLTHROUGH
+            FALLTHROUGH;
         case SubMode::PAYLOAD_PLACE:
             switch (g2.follow.get_yaw_behave()) {
                 case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
@@ -706,7 +706,7 @@ void ModeShipOperation::run()
         Vector3f pos_error = ship.pos_ned.tofloat() + offset - pos_control->get_pos_target_cm().tofloat();
         bool pos_check;
         // altitude is less than 5% of the Perch height
-        bool alt_check = fabsf(-(ship.pos_ned.z + offset.z) - pos_control->get_pos_target_cm().z) < perch_height * 0.05f;
+        bool alt_check = fabs(-(ship.pos_ned.z + offset.z) - pos_control->get_pos_target_cm().z) < perch_height * 0.05f;
         switch (_state) {
         case SubMode::CLIMB_TO_RTL:
             // check altitude is within 5% of perch_height from RTL altitude