aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: smooth-out the end of takeoff pitch by reducing takeoff pitch min via TKOFF_PLIM_SEC

Browse Source 
@Description: This parameter reduces the pitch minimum limit of an auto-takeoff just a few seconds before it reaches the target altitude. This reduces overshoot by allowing the flight controller to start leveling off a few seconds before reaching the target height. When set to zero, the mission pitch min is enforced all the way to and through the target altitude, otherwise the pitch min slowly reduces to zero in the final segment. This is the pitch_min, not the demand. The flight controller should still be commanding to gain altitude to finish the takeoff but with this param it is not forcing it higher than it wants to be. (+1 squashed commits)
This commit is contained in:
Tom Pittenger 2016-03-09 14:22:53 -08:00
parent 431b3c7160
commit 2ba9a04bf5
6 changed files with 53 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -893,7 +893,7 @@ void Plane::update_alt()
flight_stage,
is_doing_auto_land,
distance_beyond_land_wp,
auto_state.takeoff_pitch_cd,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);

9
ArduPlane/Parameters.cpp
View File

@ -199,6 +199,15 @@ const AP_Param::Info Plane::var_info[] = {
// @User: User
GSCALAR(takeoff_throttle_slewrate, "TKOFF_THR_SLEW", 0),
// @Param: TKOFF_PLIM_SEC
// @DisplayName: Takeoff pitch limit reduction
// @Description: This parameter reduces the pitch minimum limit of an auto-takeoff just a few seconds before it reaches the target altitude. This reduces overshoot by allowing the flight controller to start leveling off a few seconds before reaching the target height. When set to zero, the mission pitch min is enforced all the way to and through the target altitude, otherwise the pitch min slowly reduces to zero in the final segment. This is the pitch_min, not the demand. The flight controller should still be commanding to gain altitude to finish the takeoff but with this param it is not forcing it higher than it wants to be.
// @Units: seconds
// @Range: 0 10
// @Increment: 0.5
// @User: Advanced
GSCALAR(takeoff_pitch_limit_reduction_sec, "TKOFF_PLIM_SEC", 2),
// @Param: LAND_THR_SLEW
// @DisplayName: Landing throttle slew rate
// @Description: This parameter sets the slew rate for the throttle during auto landing. When this is zero the THR_SLEWRATE parameter is used during landing. The value is a percentage throttle change per second, so a value of 20 means to advance the throttle over 5 seconds on landing. Values below 50 are not recommended as it may cause a stall when airspeed is low and you can not throttle up fast enough.

2
ArduPlane/Parameters.h
View File

@ -153,6 +153,7 @@ public:
k_param_fence_autoenable,
k_param_fence_ret_rally,
k_param_q_attitude_control,
k_param_takeoff_pitch_limit_reduction_sec,
// 110: Telemetry control
//
@ -495,6 +496,7 @@ public:
AP_Float takeoff_tdrag_speed1;
AP_Float takeoff_rotate_speed;
AP_Int8 takeoff_throttle_slewrate;
AP_Float takeoff_pitch_limit_reduction_sec;
AP_Int8 land_throttle_slewrate;
AP_Int8 level_roll_limit;
AP_Int8 flapin_channel;

4
ArduPlane/Plane.h
View File

@ -465,6 +465,9 @@ private:
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
int16_t takeoff_pitch_cd;
// Begin leveling out the enforced takeoff pitch angle min at this height to reduce/eliminate overshoot
int32_t height_below_takeoff_to_level_off_cm;
// the highest airspeed we have reached since entering AUTO. Used
// to control ground takeoff
float highest_airspeed;
@ -946,6 +949,7 @@ private:
void takeoff_calc_roll(void);
void takeoff_calc_pitch(void);
int8_t takeoff_tail_hold(void);
int16_t get_takeoff_pitch_min_cd(void);
void print_hit_enter();
void ahrs_update();
void update_speed_height(void);

1
ArduPlane/commands_logic.cpp
View File

@ -361,6 +361,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
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
// zero locked course

38
ArduPlane/takeoff.cpp
View File

@ -133,9 +133,10 @@ void Plane::takeoff_calc_pitch(void)
}
if (ahrs.airspeed_sensor_enabled()) {
int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd();
calc_nav_pitch();
if (nav_pitch_cd < auto_state.takeoff_pitch_cd) {
nav_pitch_cd = auto_state.takeoff_pitch_cd;
if (nav_pitch_cd < takeoff_pitch_min_cd) {
nav_pitch_cd = takeoff_pitch_min_cd;
}
} else {
nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd;
@ -143,6 +144,39 @@ void Plane::takeoff_calc_pitch(void)
}
}
/*
* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
*/
int16_t Plane::get_takeoff_pitch_min_cd(void)
{
if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) {
return auto_state.takeoff_pitch_cd;
}
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
int32_t remaining_height_to_target_cm = (auto_state.takeoff_altitude_rel_cm - relative_alt_cm);
// seconds to target alt method
if (g.takeoff_pitch_limit_reduction_sec > 0) {
// if height-below-target has been initialized then use it to create and apply a scaler to the pitch_min
if (auto_state.height_below_takeoff_to_level_off_cm != 0) {
float scalar = remaining_height_to_target_cm / (float)auto_state.height_below_takeoff_to_level_off_cm;
return auto_state.takeoff_pitch_cd * scalar;
}
// are we entering the region where we want to start leveling off before we reach takeoff alt?
float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
if (sec_to_target > 0 &&
relative_alt_cm >= 1000 &&
sec_to_target <= g.takeoff_pitch_limit_reduction_sec) {
// make a note of that altitude to use it as a start height for scaling
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100);
auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
}
}
return auto_state.takeoff_pitch_cd;
}
/*
return a tail hold percentage during initial takeoff for a tail
dragger