diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e655f9f95e..8c9db64192 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1490,9 +1490,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } // update tecs - const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get()); - const float takeoff_pitch_min_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), - _param_fw_p_lim_min.get()); + const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); + const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()), + math::radians(_param_fw_p_lim_min.get())); if (_runway_takeoff.resetIntegrators()) { // reset integrals except yaw (which also counts for the wheel controller) @@ -1505,19 +1505,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, target_airspeed, - radians(takeoff_pitch_min_deg), - radians(takeoff_pitch_max_deg), + pitch_min, + pitch_max, _param_fw_thr_min.get(), _param_fw_thr_max.get(), false, - radians(takeoff_pitch_min_deg), + pitch_min, _param_sinkrate_target.get(), _param_fw_t_clmb_max.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); - _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust()); + _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); // apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF; diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index 091e63cb7e..455b78dd30 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -137,24 +137,32 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint) } } -float RunwayTakeoff::getThrottle(const hrt_abstime &time_now, float external_throttle_setpoint) +float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const { - float throttle = 0.0f; + float throttle = idle_throttle; switch (takeoff_state_) { case RunwayTakeoffState::THROTTLE_RAMP: - throttle = ((time_now - time_initialized_) / (param_rwto_ramp_time_.get() * 1_s)) * param_rwto_max_thr_.get(); - throttle = math::constrain(throttle, 0.0f, param_rwto_max_thr_.get()); + + throttle = interpolateValuesOverAbsoluteTime(idle_throttle, param_rwto_max_thr_.get(), time_initialized_, + param_rwto_ramp_time_.get()); + break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: throttle = param_rwto_max_thr_.get(); + break; - default: - const float interpolator = math::constrain((time_now - takeoff_time_ * 1_s) / (kThrottleHysteresisTime * 1_s), 0.0f, - 1.0f); - throttle = external_throttle_setpoint * interpolator + (1.0f - interpolator) * param_rwto_max_thr_.get(); + case RunwayTakeoffState::CLIMBOUT: + // ramp in throttle setpoint over takeoff rotation transition time + throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_, + param_rwto_rot_time_.get()); + + break; + + case RunwayTakeoffState::FLY: + throttle = external_throttle_setpoint; } return throttle; @@ -166,21 +174,33 @@ bool RunwayTakeoff::resetIntegrators() return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; } -float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) +float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const { - if (takeoff_state_ < RunwayTakeoffState::FLY) { - return min_pitch_in_climbout; + if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { + // constrain to the taxi pitch setpoint + return math::radians(param_rwto_psp_.get() - 0.01f); + + } else if (takeoff_state_ < RunwayTakeoffState::FLY) { + // ramp in the climbout pitch constraint over the rotation transition time + const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f); + return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_, + param_rwto_rot_time_.get()); } else { return min_pitch; } } -float RunwayTakeoff::getMaxPitch(const float max_pitch) +float RunwayTakeoff::getMaxPitch(const float max_pitch) const { - // use max pitch from parameter if set - if (takeoff_state_ < RunwayTakeoffState::FLY && param_rwto_max_pitch_.get() > kMinMaxPitch) { - return param_rwto_max_pitch_.get(); + if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { + // constrain to the taxi pitch setpoint + return math::radians(param_rwto_psp_.get() + 0.01f); + + } else if (takeoff_state_ < RunwayTakeoffState::FLY) { + // ramp in the climbout pitch constraint over the rotation transition time + const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f); + return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get()); } else { return max_pitch; @@ -194,4 +214,13 @@ void RunwayTakeoff::reset() takeoff_time_ = 0; } +float RunwayTakeoff::interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, + const hrt_abstime &start_time, const float interpolation_time) const +{ + const float seconds_since_start = hrt_elapsed_time(&start_time) / float(1_s); + const float interpolator = math::constrain(seconds_since_start / interpolation_time, 0.0f, 1.0f); + + return interpolator * end_value + (1.0f - interpolator) * start_value; +} + } // namespace runwaytakeoff diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index abb8d60516..532a1dd33b 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -140,27 +140,27 @@ public: /** * @brief Returns the throttle setpoint. * - * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway, then passes - * through the externally defined throttle setting. + * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway and then + * ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time * - * @param time_now Absolute time since system boot [us] + * @param idle_throttle normalized [0,1] * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1] * @return Throttle setpoint, normalized [0,1] */ - float getThrottle(const hrt_abstime &time_now, const float external_throttle_setpoint); + float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const; /** * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad] * @param min_pitch Externally commanded minimum pitch angle [rad] * @return Minimum pitch angle [rad] */ - float getMinPitch(const float min_pitch_in_climbout, const float min_pitch); + float getMinPitch(const float min_pitch_in_climbout, const float min_pitch) const; /** * @param max_pitch Externally commanded maximum pitch angle [rad] * @return Maximum pitch angle [rad] */ - float getMaxPitch(const float max_pitch); + float getMaxPitch(const float max_pitch) const; /** * @return Runway takeoff starting position in global frame (lat, lon) [deg] @@ -181,19 +181,19 @@ public: */ void reset(); + /** + * @brief Linearly interpolates between a start and end value over an absolute time span. + * + * @param start_value + * @param end_value + * @param start_time Absolute start time [us] + * @param interpolation_time The time span to interpolate over [s] + * @return interpolated value + */ + float interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, const hrt_abstime &start_time, + const float interpolation_time) const; + private: - /** - * Minimum allowed maximum pitch constraint (from parameter) for runway takeoff. [rad] - */ - static constexpr float kMinMaxPitch = 0.1f; - - /** - * Time from takeoff during which the takeoff throttle is transitioned to the externally commanded throttle. - * Used to avoid throttle jumps immediately on lift off when switching from the constant, parameterized, takeoff - * throttle to the airspeed/altitude controller's commanded throttle [s] - */ - static constexpr float kThrottleHysteresisTime = 2.0f; - /** * Current state of runway takeoff procedure */ @@ -232,9 +232,9 @@ private: (ParamInt) param_rwto_hdg_, (ParamFloat) param_rwto_max_thr_, (ParamFloat) param_rwto_psp_, - (ParamFloat) param_rwto_max_pitch_, (ParamFloat) param_rwto_ramp_time_, - (ParamFloat) param_rwto_rot_airspd_ + (ParamFloat) param_rwto_rot_airspd_, + (ParamFloat) param_rwto_rot_time_ ) }; diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c index 7da1693e6e..7f791a01c6 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c @@ -77,7 +77,7 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0); PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); /** - * Pitch setpoint during taxi / before takeoff airspeed is reached. + * Pitch setpoint during taxi / before takeoff rotation airspeed is reached. * * A taildragger with steerable wheel might need to pitch up * a little to keep its wheel on the ground before airspeed @@ -92,21 +92,6 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); */ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); -/** - * Max pitch during takeoff. - * - * Fixed-wing settings are used if set to 0. Note that there is also a minimum - * pitch of 10 degrees during takeoff, so this must be larger if set. - * - * @unit deg - * @min 0.0 - * @max 60.0 - * @decimal 1 - * @increment 0.5 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); - /** * Throttle ramp up time for runway takeoff * @@ -157,3 +142,16 @@ PARAM_DEFINE_INT32(RWTO_NUDGE, 1); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_ROT_AIRSPD, -1.0f); + +/** + * Takeoff rotation time + * + * This is the time desired to linearly ramp in takeoff pitch constraints during the takeoff rotation + * + * @unit s + * @min 0.1 + * @decimal 1 + * @increment 0.1 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_ROT_TIME, 1.0f);