Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow for higher roll limits on takeoff

Browse Source 
only restrict to LEVEL_ROLL_LIMIT below 5m. Above that start to allow
larger roll limits, reaching the full roll limit at 15m
This commit is contained in:
Andrew Tridgell 2015-09-17 15:35:37 +10:00
parent 004c5b8416
commit d019f83bee
4 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Attitude.cpp
View File

@ -593,6 +593,7 @@ bool Plane::suppress_throttle(void)
if (auto_takeoff_check()) { if (auto_takeoff_check()) {
// we're in auto takeoff // we're in auto takeoff
throttle_suppressed = false; throttle_suppressed = false;
auto_state.baro_takeoff_alt = barometer.get_altitude();
return false; return false;
} }
// keep throttle suppressed // keep throttle suppressed

3
ArduPlane/Plane.h
View File

@ -486,6 +486,9 @@ private:
// time stamp of when we start flying while in auto mode in milliseconds // time stamp of when we start flying while in auto mode in milliseconds
uint32_t started_flying_in_auto_ms; uint32_t started_flying_in_auto_ms;
// barometric altitude at start of takeoff
float baro_takeoff_alt;
} auto_state; } auto_state;
struct { struct {

1
ArduPlane/commands_logic.cpp
View File

@ -334,6 +334,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
// zero locked course // zero locked course
steer_state.locked_course_err = 0; steer_state.locked_course_err = 0;
steer_state.hold_course_cd = -1; steer_state.hold_course_cd = -1;
auto_state.baro_takeoff_alt = barometer.get_altitude();
} }
void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd) void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)

19
ArduPlane/takeoff.cpp
View File

@ -98,9 +98,22 @@ void Plane::takeoff_calc_roll(void)
calc_nav_roll(); calc_nav_roll();
// during takeoff use the level flight roll limit to // during takeoff use the level flight roll limit to prevent large
// prevent large course corrections // wing strike. Slowly allow for more roll as we get higher above
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); // the takeoff altitude
float roll_limit = roll_limit_cd*0.01f;
float baro_alt = barometer.get_altitude();
// below 5m use the LEVEL_ROLL_LIMIT
const float lim1 = 5;
// at 15m allow for full roll
const float lim2 = 15;
if (baro_alt < auto_state.baro_takeoff_alt+lim1) {
roll_limit = g.level_roll_limit;
} else if (baro_alt < auto_state.baro_takeoff_alt+lim2) {
float proportion = (baro_alt - (auto_state.baro_takeoff_alt+lim1)) / (lim2 - lim1);
roll_limit = (1-proportion) * g.level_roll_limit + proportion * roll_limit;
}
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit*100UL, roll_limit*100UL);
} }