diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index e7c3f3da30..ed64ed056e 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -593,6 +593,7 @@ bool Plane::suppress_throttle(void) if (auto_takeoff_check()) { // we're in auto takeoff throttle_suppressed = false; + auto_state.baro_takeoff_alt = barometer.get_altitude(); return false; } // keep throttle suppressed diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6f056514f3..51e7afa01a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -486,6 +486,9 @@ private: // time stamp of when we start flying while in auto mode in milliseconds uint32_t started_flying_in_auto_ms; + + // barometric altitude at start of takeoff + float baro_takeoff_alt; } auto_state; struct { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index cd0431cf41..5d67f4ff94 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -334,6 +334,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) // zero locked course steer_state.locked_course_err = 0; 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) diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index fdf92c8756..5bcbc60a08 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -98,9 +98,22 @@ void Plane::takeoff_calc_roll(void) calc_nav_roll(); - // during takeoff use the level flight roll limit to - // prevent large course corrections - nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); + // during takeoff use the level flight roll limit to prevent large + // wing strike. Slowly allow for more roll as we get higher above + // 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); }