From 968fc1d5fe11cee356f07a2be8756765a41a6599 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 8 Aug 2024 13:42:19 +0200 Subject: [PATCH] Plane: Enabled roll navigation in mode TAKEOFF during climb - Fixed lack of heading control during TAKEOFF climb. - Also now the TAKEOFF loiter waypoint is set by the bearing of the aircraft while on TKOFF_LVL_ALT, as in the last stable release, instead of TKOFF_ALT. --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 32 +++++++++++++++++++------------- 2 files changed, 20 insertions(+), 14 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 973478239b8ae3..156901f0330f12 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -482,7 +482,7 @@ void Plane::update_GPS_10Hz(void) */ void Plane::update_control_mode(void) { - if (control_mode != &mode_auto) { + if ((control_mode != &mode_auto) && (control_mode != &mode_takeoff)) { // hold_course is only used in takeoff and landing steer_state.hold_course_cd = -1; } diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 8a8fd94c66a932..ff3ffab7612f70 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -93,6 +93,7 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } else { gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering"); + start_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc; plane.next_WP_loc.alt += ((alt - altitude) *100); plane.next_WP_loc.offset_bearing(direction, dist); @@ -119,7 +120,7 @@ void ModeTakeoff::update() alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); takeoff_mode_setup = true; - + plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. } } } @@ -127,24 +128,29 @@ void ModeTakeoff::update() if (plane.check_takeoff_timeout()) { plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); takeoff_mode_setup = false; - } - - // we finish the initial level takeoff if we climb past - // TKOFF_LVL_ALT or we pass the target location. The check for - // target location prevents us flying forever if we can't climb - // reset the loiter waypoint target to be correct bearing and dist - // from starting location in case original yaw used to set it was off due to EKF - // reset or compass interference from max throttle + + // We update the waypoint to follow once we're past TKOFF_LVL_ALT. + // This will correct any yaw estimation errors (caused by EKF reset + // or compass interference from max throttle), since it's using position bearing. const float altitude_cm = plane.current_loc.alt - start_loc.alt; - if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && - (altitude_cm >= (alt*100 - 200) || - start_loc.get_distance(plane.current_loc) >= dist)) { - // reset the target loiter waypoint using current yaw which should be close to correct starting heading + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF + && plane.steer_state.hold_course_cd == -1 // This is the enter-once flag. + && altitude_cm >= level_alt + ) { const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01; plane.next_WP_loc = start_loc; plane.next_WP_loc.offset_bearing(direction, dist); plane.next_WP_loc.alt += alt*100.0; + plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. + } + + // We finish the initial level takeoff if we climb past + // TKOFF_ALT or we pass the target location. The check for + // target location prevents us flying forever if we can't climb. + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && + (altitude_cm >= (alt*100 - 200) || + start_loc.get_distance(plane.current_loc) >= dist)) { plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); }