Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
George Zogopoulos 2024-08-05 17:30:56 +02:00 committed by Andrew Tridgell
parent 880ebbcdad
commit 6ce6ef8fff
8 changed files with 140 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.cpp
View File

@ -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();

6
ArduPlane/Plane.h
View File

@ -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);

2
ArduPlane/commands_logic.cpp
View File

@ -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) {

2
ArduPlane/mode_auto.cpp
View File

@ -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();

62
ArduPlane/mode_takeoff.cpp
View File

@ -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) {

6
ArduPlane/quadplane.cpp
View File

@ -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);

26
ArduPlane/servos.cpp
View File

@ -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);

126
ArduPlane/takeoff.cpp
View File

@ -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();
}