ArduPlane:make heading lock upon gnd speed consistent

Hwurzburg · Jan 24, 2025 · 94a0415 · 94a0415
1 parent 4e12b4e
commit 94a0415
Show file tree

Hide file tree

Showing 6 changed files with 13 additions and 23 deletions.
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -501,9 +501,6 @@ class Plane : public AP_Vehicle {
         // filtered sink rate for landing
         float sink_rate;
 
-        // time when we first pass min GPS speed on takeoff
-        uint32_t takeoff_speed_time_ms;
-
         // distance to next waypoint
         float wp_distance;
 

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -377,7 +377,6 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
     auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt;
     next_WP_loc.lat = home.lat + 10;
     next_WP_loc.lng = home.lng + 10;
-    auto_state.takeoff_speed_time_ms = 0;
     auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction.
     auto_state.height_below_takeoff_to_level_off_cm = 0;
     // Flag also used to override "on the ground" throttle disable
@@ -553,21 +552,15 @@ bool Plane::verify_takeoff()
     trust_ahrs_yaw |= ahrs.dcm_yaw_initialised();
 #endif
     if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) {
-        const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
-        if (auto_state.takeoff_speed_time_ms == 0 && 
-            gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
-            gps.ground_speed() > min_gps_speed &&
+        // once we reach sufficient speed for good GPS course
+        // estimation we save our current GPS ground course
+        // corrected for summed yaw to set the take off
+        // course. This keeps wings level until we are ready to
+        // rotate, and also allows us to cope with arbitrary
+        // compass errors for auto takeoff
+        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
+            gps.ground_speed() > GPS_GND_CRS_MIN_SPD &&
             hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
-            auto_state.takeoff_speed_time_ms = millis();
-        }
-        if (auto_state.takeoff_speed_time_ms != 0 &&
-            millis() - auto_state.takeoff_speed_time_ms >= 2000) {
-            // once we reach sufficient speed for good GPS course
-            // estimation we save our current GPS ground course
-            // corrected for summed yaw to set the take off
-            // course. This keeps wings level until we are ready to
-            // rotate, and also allows us to cope with arbitrary
-            // compass errors for auto takeoff
             float takeoff_course = wrap_PI(radians(gps.ground_course())) - steer_state.locked_course_err;
             takeoff_course = wrap_PI(takeoff_course);
             steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);

diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
@@ -10,7 +10,7 @@
 
 #define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats
 
-#define GPS_GND_CRS_MIN_SPD 5 // m/s, used to set when intial_direction.heading is captured in NAV_TAKEOFF and Mode TAKEOFF
+#define GPS_GND_CRS_MIN_SPD 5 // m/s, used to set when intial_direction.heading is captured or steer_state.hold_course_cd
 
 // failsafe
 // ----------------------

diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp
@@ -67,7 +67,7 @@ void ModeCruise::navigate()
         plane.channel_roll->get_control_in() == 0 &&
         plane.rudder_input() == 0 &&
         plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
-        plane.gps.ground_speed() >= 3 &&
+        plane.gps.ground_speed() >= GPS_GND_CRS_MIN_SPD &&
         moving_forwards &&
         lock_timer_ms == 0) {
         // user wants to lock the heading - start the timer

diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp
@@ -120,13 +120,13 @@ void ModeTakeoff::update()
 
             /*
               don't lock in the takeoff loiter location until we reach
-              a ground speed of AIRSPEED_MIN*0.3 to cope with aircraft
+              a ground speed of GPS_GND_CRS_MIN_SPD to cope with aircraft
               with no compass or poor compass. If flying in a very
               strong headwind then the is_flying() check above will
               eventually trigger
              */
             if (!plane.throttle_suppressed &&
-                groundspeed > plane.aparm.airspeed_min*0.3) {
+                groundspeed > GPS_GND_CRS_MIN_SPD) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
                                 alt, dist, direction);
                 plane.takeoff_state.start_time_ms = millis();

diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -104,7 +104,7 @@ bool Plane::suppress_throttle(void)
         return false;
     }
 
-    bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5);
+    bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= GPS_GND_CRS_MIN_SPD);
 
     if ((control_mode == &mode_auto &&
          auto_state.takeoff_complete == false) ||