Add min ground speed when not using air speed

ArduCustom · Feb 15, 2025 · 97ef60d · 97ef60d
1 parent 7736628
commit 97ef60d
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 1 deletion.
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
@@ -635,18 +635,49 @@ void Plane::update_alt()
             throttle_min = MAX(0, throttle_min);
         }
 
+        float mingndspd_throttle_nudge = 0;
+
+        if (TECS_controller.is_using_airspeed() || flight_stage != AP_Vehicle::FixedWing::FLIGHT_NORMAL) {
+            update_altitute_last_us = 0;
+        } else if (aparm.min_gndspeed_cm > 0) {
+            const uint64_t now = AP_HAL::micros64();
+
+            if (update_altitute_last_us) {
+                const float dt = (now - update_altitute_last_us) * 1.0e-6f;
+
+                Vector2f ground_speed_v;
+                {
+                    WITH_SEMAPHORE(ahrs.get_semaphore());
+                    ground_speed_v = ahrs.groundspeed_vector();
+                }
+                const float ground_speed = ground_speed_v.length();
+                const float speed_error = aparm.min_gndspeed_cm * 0.01f - ground_speed;
+                const float p_term = speed_error * (aparm.throttle_max - aparm.throttle_cruise) / (aparm.airspeed_max - aparm.airspeed_cruise_cm * 0.01f);
+                const float d_term = (speed_error - mingndspd_last_error) * dt * g2.mingndspd_d;
+                const float max_nudge = aparm.throttle_max - aparm.throttle_cruise;
+                mingndspd_throttle_nudge_i = constrain_float(mingndspd_throttle_nudge_i + speed_error * dt * g2.mingndspd_i * (1/3.6f), 0, max_nudge);
+                mingndspd_throttle_nudge = constrain_float(p_term + d_term + mingndspd_throttle_nudge_i, 0, max_nudge);
+
+                mingndspd_last_error = speed_error;
+            }
+
+            update_altitute_last_us = now;
+        }
+
         TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
                                                  target_airspeed_cm,
                                                  flight_stage,
                                                  distance_beyond_land_wp,
                                                  get_takeoff_pitch_min_cd(),
-                                                 throttle_nudge,
+                                                 throttle_nudge + mingndspd_throttle_nudge,
                                                  tecs_hgt_afe(),
                                                  aerodynamic_load_factor,
                                                  g2.throttle_expo_auto,
                                                  max_climb_rate,
                                                  max_sink_rate,
                                                  throttle_min);
+    } else {
+        mingndspd_throttle_nudge_i = 0;
     }
 }
 

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1411,6 +1411,20 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow),
 #endif
 
+    // @Param: MIN_GNDSPD_I
+    // @DisplayName: Minimum ground speed throttle PID I term
+    // @Description: Minimum ground speed throttle PID I term
+    // @Range: 0 255
+    // @User: Advanced
+    AP_GROUPINFO("MIN_GNDSPD_I", 41, ParametersG2, mingndspd_i, 20),
+
+    // @Param: MIN_GNDSPD_D
+    // @DisplayName: Minimum ground speed throttle PID D term
+    // @Description: Minimum ground speed throttle PID D term
+    // @Range: 0 255
+    // @User: Advanced
+    AP_GROUPINFO("MIN_GNDSPD_D", 42, ParametersG2, mingndspd_d, 5),
+
     // @Param: SAT_FINISHTHRESH
     // @DisplayName: Number of microseconds of servo trim adjustment during 10s under which the auto trim is considered finished
     // @Description: Number of microseconds of servo trim adjustment during 10s under which the auto trim is considered finished

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -602,6 +602,9 @@ class ParametersG2 {
     AP_Int8 arming_mode_sw;
     AP_Int8 servos_auto_trim_finished_threshold;
 
+    AP_Float mingndspd_i;
+    AP_Float mingndspd_d;
+
     // plane throws diff
     AP_Float ailerons_diff;
     AP_Float elevator_diff;

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -317,6 +317,10 @@ class Plane : public AP_Vehicle {
     // last time we ran roll/pitch stabilization
     uint32_t last_stabilize_ms;
 
+    uint32_t update_altitute_last_us;
+    float mingndspd_throttle_nudge_i;
+    float mingndspd_last_error;
+
     // Failsafe
     struct {
         // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold

diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
@@ -184,6 +184,10 @@ class AP_TECS {
 
     bool is_using_synthetic_airspeed() const { return _use_synthetic_airspeed; }
 
+    bool is_using_airspeed() const {
+        return _ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed;
+    }
+
 private:
     // Last time update_50Hz was called
     uint64_t _update_50hz_last_usec;