diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 16cbb04e45..90e3601e69 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,7 +47,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -61,6 +60,7 @@ RunwayTakeoff::RunwayTakeoff() : _climbout(false), _start_sp{}, _target_sp{}, + _throttle_ramp_time(2 * 1e6), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -87,7 +87,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); - _climbout = true; + _climbout = true; // this is true until climbout is finished } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -95,7 +95,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: - if (hrt_elapsed_time(&_initialized_time) > 1e6) { + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } @@ -131,12 +131,21 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } +/* + * Returns true as long as we're below navigation altitude + */ bool RunwayTakeoff::controlYaw() { // keep controlling yaw directly until we start navigation return _state < RunwayTakeoffState::CLIMBOUT; } +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { @@ -146,6 +155,9 @@ float RunwayTakeoff::getPitch(float tecsPitch) return tecsPitch; } +/* + * Returns the roll setpoint to use. + */ float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 @@ -163,16 +175,25 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return navigatorRoll; } +/* + * Returns the yaw setpoint to use. + */ float RunwayTakeoff::getYaw(float navigatorYaw) { return navigatorYaw; } +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ float RunwayTakeoff::getThrottle(float tecsThrottle) { switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { - float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * _takeoff_throttle.get(); return throttle < _takeoff_throttle.get() ? throttle : @@ -193,9 +214,16 @@ bool RunwayTakeoff::resetIntegrators() return _state < RunwayTakeoffState::TAKEOFF; } +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) { - if (_climbout) { + if (_state < RunwayTakeoffState::FLY) { return math::max(sp_min, climbout_min); } @@ -204,9 +232,15 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { return _max_takeoff_pitch.get(); } @@ -215,6 +249,9 @@ float RunwayTakeoff::getMaxPitch(float max) } } +/* + * Returns the "previous" (start) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getPrevWP() { math::Vector<2> prev_wp; @@ -223,6 +260,9 @@ math::Vector<2> RunwayTakeoff::getPrevWP() return prev_wp; } +/* + * Returns the "current" (target) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) { if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2913b2ad64..cf0ab455d8 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,7 +50,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -105,6 +104,7 @@ private: bool _climbout; struct position_setpoint_s _start_sp; struct position_setpoint_s _target_sp; + unsigned _throttle_ramp_time; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled;