Plane: refactor NAV_ALTITUDE_WAIT to AutoMode:: to match nav_delay()

magicrub · Apr 30, 2024 · 04da992 · 04da992
1 parent 29cbd8d
commit 04da992
Show file tree

Hide file tree

Showing 5 changed files with 54 additions and 40 deletions.
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -480,10 +480,6 @@ class Plane : public AP_Vehicle {
         // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
         int16_t takeoff_pitch_cd;
 
-        // used to 'wiggle' servos in idle mode to prevent them freezing
-        // at high altitudes
-        uint8_t idle_wiggle_stage;
-
         // Flag for using gps ground course instead of INS yaw.  Set false when takeoff command in process.
         bool takeoff_complete;
 
@@ -1110,7 +1106,6 @@ class Plane : public AP_Vehicle {
     void avoidance_adsb_update(void);
 
     // servos.cpp
-    void set_servos_idle(void);
     void set_servos();
     float apply_throttle_limits(float throttle_in);
     void set_throttle(void);

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -92,6 +92,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_ALTITUDE_WAIT:
+        mode_auto.do_altitude_wait();
         break;
 
 #if HAL_QUADPLANE_ENABLED
@@ -284,7 +285,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
         return verify_continue_and_change_alt();
 
     case MAV_CMD_NAV_ALTITUDE_WAIT:
-        return verify_altitude_wait(cmd);
+        return mode_auto.verify_altitude_wait(cmd);
 
 #if HAL_QUADPLANE_ENABLED
     case MAV_CMD_NAV_VTOL_TAKEOFF:
@@ -842,29 +843,33 @@ bool Plane::verify_continue_and_change_alt()
     return false;
 }
 
+void ModeAuto::do_altitude_wait()
+{
+    wiggle.last_ms = 0;
+}
+
 /*
   see if we have reached altitude or descent speed
  */
-bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
+bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
 {
-    if (current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
+    if (plane.current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
         gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
         return true;
     }
-    if (auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)auto_state.sink_rate);
+    if (plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate);
         return true;        
     }
 
     // if requested, wiggle servos
     if (cmd.content.altitude_wait.wiggle_time != 0) {
-        static uint32_t last_wiggle_ms;
-        if (auto_state.idle_wiggle_stage == 0 &&
-            AP_HAL::millis() - last_wiggle_ms > cmd.content.altitude_wait.wiggle_time*1000) {
-            auto_state.idle_wiggle_stage = 1;
-            last_wiggle_ms = AP_HAL::millis();
+        if (wiggle.stage == 0 &&
+            AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) {
+            wiggle.stage = 1;
+            wiggle.last_ms = AP_HAL::millis();
+            // idle_wiggle_stage is updated in wiggle_servos()
         }
-        // idle_wiggle_stage is updated in set_servos_idle()
     }
 
     return false;

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -233,6 +233,9 @@ class ModeAuto : public Mode
     void do_nav_delay(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
 
+    void do_altitude_wait();
+    bool verify_altitude_wait(const AP_Mission::Mission_Command& cmd);
+
     void run() override;
 
 protected:
@@ -249,6 +252,12 @@ class ModeAuto : public Mode
         uint32_t time_start_ms;
     } nav_delay;
 
+    // wiggle state and timer for NAV_ALTITUDE_WAIT
+    void wiggle_servos();
+    struct {
+        uint8_t stage;
+        uint32_t last_ms;
+    } wiggle;
 };
 
 

diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp
@@ -167,8 +167,16 @@ bool ModeAuto::is_landing() const
 void ModeAuto::run()
 {
     if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
-        // Wiggle servos
-        plane.set_servos_idle();
+
+        wiggle_servos();
+
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
+
+        SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
+        SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleLeft);
+        SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleRight);
 
         // Relax attitude control
         reset_controllers();

diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -348,43 +348,40 @@ void Plane::airbrake_update(void)
 }
 
 /*
-  setup servos for idle mode
+  setup servos for idle wiggle mode
   Idle mode is used during balloon launch to keep servos still, apart
   from occasional wiggle to prevent freezing up
  */
-void Plane::set_servos_idle(void)
+void ModeAuto::wiggle_servos()
 {
+    // This is only active while in AUTO running NAV_ALTITUDE_WAIT with wiggle_time > 0
+    if (wiggle.last_ms == 0) {
+        return;
+    }
+
     int16_t servo_value;
     // move over full range for 2 seconds
-    if (auto_state.idle_wiggle_stage != 0) {
-        auto_state.idle_wiggle_stage += 2;
+    if (wiggle.stage != 0) {
+        wiggle.stage += 2;
     }
-    if (auto_state.idle_wiggle_stage == 0) {
+    if (wiggle.stage == 0) {
         servo_value = 0;
-    } else if (auto_state.idle_wiggle_stage < 50) {
-        servo_value = auto_state.idle_wiggle_stage * (4500 / 50);
-    } else if (auto_state.idle_wiggle_stage < 100) {
-        servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);        
-    } else if (auto_state.idle_wiggle_stage < 150) {
-        servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);        
-    } else if (auto_state.idle_wiggle_stage < 200) {
-        servo_value = (auto_state.idle_wiggle_stage-200) * (4500 / 50);        
+    } else if (wiggle.stage < 50) {
+        servo_value = wiggle.stage * (4500 / 50);
+    } else if (wiggle.stage < 100) {
+        servo_value = (100 - wiggle.stage) * (4500 / 50);        
+    } else if (wiggle.stage < 150) {
+        servo_value = (100 - wiggle.stage) * (4500 / 50);        
+    } else if (wiggle.stage < 200) {
+        servo_value = (wiggle.stage-200) * (4500 / 50);        
     } else {
-        auto_state.idle_wiggle_stage = 0;
+        wiggle.stage = 0;
         servo_value = 0;
     }
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value);
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value);
     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value);
 
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
-
-    SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
-    SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleLeft);
-    SRV_Channels::set_output_to_trim(SRV_Channel::k_throttleRight);
-
 }