From 6ce6ef8fffb83ba69847dcdcb1d65e9f3ff27f22 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Mon, 5 Aug 2024 17:30:56 +0200 Subject: [PATCH] Plane: Takeoff improvements - TAKEOFF and AUTO flight modes now should have identical takeoff - Prevent behaviour switching past climb altitude in TAKEOFF mode. - Refactor set_pitch_min/max methods. Max was already there, now renamed. Min is newly introduced. behaviour. - Remove enforcement of min takeoff throttle logic from servos.cpp. It is now handled only by takeoff.cpp. - Take TKOFF_LVL_ALT into consideration in AUTO as well. - Fixed pitch setpoint when TKOFF_ROTATE_SPD>0. - Roll navigation in mode TAKEOFF during climb should now work again. - 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. - Using TRIM_THROTTLE in takeoffs, when TKOFF_THR_MIN==0 --- ArduPlane/ArduPlane.cpp | 6 +- ArduPlane/Plane.h | 6 +- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/mode_auto.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 62 +++++++++-------- ArduPlane/quadplane.cpp | 6 +- ArduPlane/servos.cpp | 26 ++------ ArduPlane/takeoff.cpp | 126 ++++++++++++++++++++++++----------- 8 files changed, 140 insertions(+), 96 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 137a895f2e..a3775e1bbb 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -493,10 +493,14 @@ 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; } + // refresh the throttle limits, to avoid using stale values + // they will be updated once takeoff_calc_throttle is called + takeoff_state.throttle_lim_max = 100.0f; + takeoff_state.throttle_lim_min = -100.0f; update_fly_forward(); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 70ef5fe7a9..66df996ec6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -446,6 +446,10 @@ private: uint32_t accel_event_ms; uint32_t start_time_ms; bool waiting_for_rudder_neutral; + float throttle_lim_max; + float throttle_lim_min; + uint32_t throttle_max_timer_ms; + // Good candidate for keeping the initial time for TKOFF_THR_MAX_T. } takeoff_state; // ground steering controller state @@ -1131,7 +1135,7 @@ private: bool auto_takeoff_check(void); void takeoff_calc_roll(void); void takeoff_calc_pitch(void); - void takeoff_calc_throttle(const bool use_max_throttle=false); + void takeoff_calc_throttle(); int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2f3b6344ba..90c3e02480 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -393,7 +393,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) set_next_WP(cmd.content.location); // configure abort altitude and pitch - // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m + // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 30m if (cmd.p1 > 0) { auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100; } else if (auto_state.takeoff_altitude_rel_cm <= 0) { diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index b0d0a0585d..287ce0e727 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -89,7 +89,7 @@ void ModeAuto::update() (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); - plane.calc_throttle(); + plane.takeoff_calc_throttle(); } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { plane.calc_nav_roll(); plane.calc_nav_pitch(); diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index cf51814571..5f93cc4729 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -81,6 +81,7 @@ void ModeTakeoff::update() const float alt = target_alt; const float dist = target_dist; if (!takeoff_mode_setup) { + plane.auto_state.takeoff_altitude_rel_cm = alt * 100; const uint16_t altitude = plane.relative_ground_altitude(false,true); const float direction = degrees(ahrs.get_yaw()); // see if we will skip takeoff as already flying @@ -92,11 +93,12 @@ 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); takeoff_mode_setup = true; - plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); } // not flying so do a full takeoff sequence } else { @@ -117,8 +119,9 @@ void ModeTakeoff::update() gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); + plane.takeoff_state.throttle_max_timer_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. } } } @@ -126,48 +129,49 @@ 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 or we + // pass the target location. The check for target location prevents us + // flying towards a wrong location if we can't climb. + // 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 >= level_alt*100 || - 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 * 100.0f) || start_loc.get_distance(plane.current_loc) >= dist) + ) { 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); } if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { - //below TAKOFF_LVL_ALT + //below TKOFF_ALT plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); - plane.takeoff_calc_throttle(true); + plane.takeoff_calc_throttle(); } else { - if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering #if AP_FENCE_ENABLED - if (!have_autoenabled_fences) { - plane.fence.auto_enable_fence_after_takeoff(); - have_autoenabled_fences = true; - } -#endif - plane.calc_nav_roll(); - plane.calc_nav_pitch(); - plane.calc_throttle(); - } else { // still climbing to TAKEOFF_ALT; may be loitering - plane.takeoff_calc_throttle(); - plane.takeoff_calc_roll(); - plane.takeoff_calc_pitch(); + if (!have_autoenabled_fences) { + plane.fence.auto_enable_fence_after_takeoff(); + have_autoenabled_fences = true; } +#endif + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); //check if in long failsafe due to being in initial TAKEOFF stage; if it is, recall long failsafe now to get fs action via events call if (plane.long_failsafe_pending) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a3c27cc4fd..bdaa8a9915 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3401,7 +3401,8 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) return false; } transition->restart(); - plane.TECS_controller.set_pitch_max_limit(transition_pitch_max); + plane.TECS_controller.set_pitch_max(transition_pitch_max); + plane.TECS_controller.set_pitch_min(-transition_pitch_max); // todo: why are you doing this, I want to delete it. set_alt_target_current(); @@ -4551,7 +4552,8 @@ void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_ } // set a single loop pitch limit in TECS - plane.TECS_controller.set_pitch_max_limit(max_pitch); + plane.TECS_controller.set_pitch_max(max_pitch); + plane.TECS_controller.set_pitch_min(-max_pitch); // ensure pitch is constrained to limit nav_pitch_cd = constrain_int32(nav_pitch_cd, -max_pitch*100.0, max_pitch*100.0); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 9ef0353f29..49e3e07b46 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -532,6 +532,7 @@ float Plane::apply_throttle_limits(float throttle_in) min_throttle = 0; } + // Handle throttle limits for takeoff conditions. // Query the conditions where TKOFF_THR_MAX applies. const bool use_takeoff_throttle = (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || @@ -539,27 +540,9 @@ float Plane::apply_throttle_limits(float throttle_in) // Handle throttle limits for takeoff conditions. if (use_takeoff_throttle) { - if (aparm.takeoff_throttle_max != 0) { - // Replace max throttle with the takeoff max throttle setting. - // This is typically done to protect against long intervals of large power draw. - // Or (in contrast) to give some extra throttle during the initial climb. - max_throttle = aparm.takeoff_throttle_max.get(); - } - // Do not allow min throttle to go below a lower threshold. - // This is typically done to protect against premature stalls close to the ground. - const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); - if (!use_throttle_range || !ahrs.using_airspeed_sensor()) { - // Use a constant max throttle throughout the takeoff or when airspeed readings are not available. - if (aparm.takeoff_throttle_max.get() == 0) { - min_throttle = MAX(min_throttle, aparm.throttle_max.get()); - } else { - min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); - } - } else if (use_throttle_range) { // Use a throttle range through the takeoff. - if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1. - min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get()); - } - } + // Read from takeoff_state + max_throttle = takeoff_state.throttle_lim_max; + min_throttle = takeoff_state.throttle_lim_min; } else if (landing.is_flaring()) { // Allow throttle cutoff when flaring. // This is to allow the aircraft to bleed speed faster and land with a shut off thruster. @@ -600,6 +583,7 @@ float Plane::apply_throttle_limits(float throttle_in) min_throttle = MIN(min_throttle, max_throttle); // Let TECS know about the updated throttle limits. + // These will be taken into account on the next iteration. TECS_controller.set_throttle_min(0.01f*min_throttle); TECS_controller.set_throttle_max(0.01f*max_throttle); return constrain_float(throttle_in, min_throttle, max_throttle); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 98c59acfcf..dda412d5ee 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void) takeoff_state.launchTimerStarted = false; takeoff_state.last_tkoff_arm_time = 0; takeoff_state.start_time_ms = now; + takeoff_state.throttle_max_timer_ms = now; steer_state.locked_course_err = 0; // use current heading without any error offset return true; } @@ -179,66 +180,111 @@ void Plane::takeoff_calc_roll(void) */ void Plane::takeoff_calc_pitch(void) { - if (auto_state.highest_airspeed < g.takeoff_rotate_speed) { - // we have not reached rotate speed, use the specified takeoff target pitch angle - nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch); - return; + // First see if TKOFF_ROTATE_SPD applies. + // This will set the pitch for the first portion of the takeoff, up until cruise speed is reached. + if (g.takeoff_rotate_speed > 0) { + // A non-zero rotate speed is recommended for ground takeoffs. + if (auto_state.highest_airspeed < g.takeoff_rotate_speed) { + // We have not reached rotate speed, use the specified takeoff target pitch angle. + nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch); + TECS_controller.set_pitch_min(0.01f*nav_pitch_cd); + TECS_controller.set_pitch_max(0.01f*nav_pitch_cd); + return; + } else if (gps.ground_speed() <= (float)aparm.airspeed_cruise) { + // If rotate speed applied, gradually transition from TKOFF_GND_PITCH to the climb angle. + // This is recommended for ground takeoffs, so delay rotation until ground speed indicates adequate airspeed. + const uint16_t min_pitch_cd = 500; // Set a minimum of 5 deg climb angle. + nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd; + nav_pitch_cd = constrain_int32(nav_pitch_cd, min_pitch_cd, auto_state.takeoff_pitch_cd); + TECS_controller.set_pitch_min(0.01f*nav_pitch_cd); + TECS_controller.set_pitch_max(0.01f*nav_pitch_cd); + return; + } } + // We are now past the rotation. + // Initialize pitch limits for TECS. + int16_t pitch_min_cd = get_takeoff_pitch_min_cd(); + bool pitch_clipped_max = false; + + // If we're using an airspeed sensor, we consult TECS. if (ahrs.using_airspeed_sensor()) { - int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd(); calc_nav_pitch(); - if (nav_pitch_cd < takeoff_pitch_min_cd) { - nav_pitch_cd = takeoff_pitch_min_cd; + // At any rate, we don't want to go lower than the minimum pitch bound. + if (nav_pitch_cd < pitch_min_cd) { + nav_pitch_cd = pitch_min_cd; } } else { - if (g.takeoff_rotate_speed > 0) { - // Rise off ground takeoff so delay rotation until ground speed indicates adequate airspeed - nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd; - nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); - } else { - // Doing hand or catapult launch so need at least 5 deg pitch to prevent initial height loss - nav_pitch_cd = MAX(auto_state.takeoff_pitch_cd, 500); - } + // If not, we will use the minimum allowed angle. + nav_pitch_cd = pitch_min_cd; + + pitch_clipped_max = true; } + // Check if we have trouble with roll control. if (aparm.stall_prevention != 0) { - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF || - control_mode == &mode_takeoff) { - // during takeoff we want to prioritise roll control over - // pitch. Apply a reduction in pitch demand if our roll is - // significantly off. The aim of this change is to - // increase the robustness of hand launches, particularly - // in cross-winds. If we start to roll over then we reduce - // pitch demand until the roll recovers - float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90)); - float reduction = sq(cosf(roll_error_rad)); - nav_pitch_cd *= reduction; + // during takeoff we want to prioritise roll control over + // pitch. Apply a reduction in pitch demand if our roll is + // significantly off. The aim of this change is to + // increase the robustness of hand launches, particularly + // in cross-winds. If we start to roll over then we reduce + // pitch demand until the roll recovers + float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90)); + float reduction = sq(cosf(roll_error_rad)); + nav_pitch_cd *= reduction; + + if (nav_pitch_cd < pitch_min_cd) { + pitch_min_cd = nav_pitch_cd; } } + // Notify TECS about the external pitch setting, for the next iteration. + TECS_controller.set_pitch_min(0.01f*pitch_min_cd); + if (pitch_clipped_max) {TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);} } /* - * Set the throttle limits to run at during a takeoff. + * Calculate the throttle limits to run at during a takeoff. + * These limits are meant to be used exclusively by Plane::apply_throttle_limits(). */ -void Plane::takeoff_calc_throttle(const bool use_max_throttle) { - // This setting will take effect at the next run of TECS::update_pitch_throttle(). - - // Set the maximum throttle limit. +void Plane::takeoff_calc_throttle() { + // Initialize the maximum throttle limit. if (aparm.takeoff_throttle_max != 0) { - TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max); + takeoff_state.throttle_lim_max = aparm.takeoff_throttle_max; + } else { + takeoff_state.throttle_lim_max = aparm.throttle_max; } - // Set the minimum throttle limit. - const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); - if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. - float min_throttle = (aparm.takeoff_throttle_max != 0) ? 0.01f*aparm.takeoff_throttle_max : 0.01f*aparm.throttle_max; - TECS_controller.set_throttle_min(min_throttle); - } else { // TKOFF_MODE == 1, allow for a throttle range. - if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN. - TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min); + // Initialize the minimum throttle limit. + if (aparm.takeoff_throttle_min != 0) { + takeoff_state.throttle_lim_min = aparm.takeoff_throttle_min; + } else { + takeoff_state.throttle_lim_min = aparm.throttle_cruise; + } + + // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff. + // It only applies if the timer has been started externally. + if (takeoff_state.throttle_max_timer_ms != 0) { + const uint32_t dt = AP_HAL::millis() - takeoff_state.throttle_max_timer_ms; + if (dt*0.001 < aparm.takeoff_throttle_max_t) { + takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max; + } else { + // Reset the timer for future use. + takeoff_state.throttle_max_timer_ms = 0; } } + + // Enact the TKOFF_OPTIONS logic. + const float current_baro_alt = barometer.get_altitude(); + const bool below_lvl_alt = current_baro_alt < auto_state.baro_takeoff_alt + mode_takeoff.level_alt; + // Set the minimum throttle limit. + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + if (!use_throttle_range // We don't want to employ a throttle range. + || !ahrs.using_airspeed_sensor() // We don't have an airspeed sensor. + || below_lvl_alt // We are below TKOFF_LVL_ALT. + ) { // Traditional takeoff throttle limit. + takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max; + } + calc_throttle(); }