diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 37c9dd3743..77abc78ab1 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -36,10 +36,16 @@ */ #include "Takeoff.hpp" -#include +#include + +void Takeoff::generateInitialValue(const float hover_thrust, float velocity_p_gain) +{ + velocity_p_gain = math::max(velocity_p_gain, 0.01f); + _takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain; +} void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_thrust, const bool skip_takeoff) + const float takeoff_desired_vz, const bool skip_takeoff) { _spoolup_time_hysteresis.set_state_and_update(armed); @@ -63,14 +69,14 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool case TakeoffState::ready_for_takeoff: if (want_takeoff) { _takeoff_state = TakeoffState::rampup; - _takeoff_ramp_thrust = 0.0f; + _takeoff_ramp_vz = _takeoff_ramp_vz_init; } else { break; } case TakeoffState::rampup: - if (_takeoff_ramp_thrust <= takeoff_desired_thrust) { + if (_takeoff_ramp_vz >= takeoff_desired_vz) { _takeoff_state = TakeoffState::flight; } else { @@ -98,24 +104,24 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool } } -float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt) +float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz) { if (_takeoff_state < TakeoffState::rampup) { - return 0.f; + return _takeoff_ramp_vz_init; } if (_takeoff_state == TakeoffState::rampup) { - if (_takeoff_ramp_time > FLT_EPSILON) { - _takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time; + if (_takeoff_ramp_time > dt) { + _takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time; } else { - _takeoff_ramp_thrust = takeoff_desired_thrust; + _takeoff_ramp_vz = takeoff_desired_vz; } - if (_takeoff_ramp_thrust > takeoff_desired_thrust) { - return _takeoff_ramp_thrust; + if (_takeoff_ramp_vz < takeoff_desired_vz) { + return _takeoff_ramp_vz; } } - return takeoff_desired_thrust; + return takeoff_desired_vz; } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 6d07f2983b..957562e1b7 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -55,29 +55,31 @@ enum class TakeoffState { class Takeoff { public: - Takeoff() = default; ~Takeoff() = default; + // initialize parameters + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } + void generateInitialValue(const float hover_thrust, const float velocity_p_gain); + /** * Update the state for the takeoff. * @param setpoint a vehicle_local_position_setpoint_s structure * @return true if setpoint has updated correctly */ void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_thrust, const bool skip_takeoff); - float updateThrustRamp(const float dt, const float takeoff_desired_thrust); + const float takeoff_desired_vz, const bool skip_takeoff); + float updateRamp(const float dt, const float takeoff_desired_vz); - void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } - void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } TakeoffState getTakeoffState() { return _takeoff_state; } - // TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time - systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ - private: TakeoffState _takeoff_state = TakeoffState::disarmed; float _takeoff_ramp_time = 0.f; - float _takeoff_ramp_thrust = 0.f; + float _takeoff_ramp_vz_init = 0.f; + float _takeoff_ramp_vz = 0.f; + + systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ }; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5d7316c8c8..82690e4b04 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -151,7 +151,9 @@ private: (ParamInt) _param_mpc_auto_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ - (ParamFloat) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat)_param_mpc_thr_hover, + (ParamFloat)_param_mpc_z_vel_p ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -358,6 +360,7 @@ MulticopterPositionControl::parameters_update(bool force) // set trigger time for takeoff delay _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); + _takeoff.generateInitialValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get()); if (_wv_controller != nullptr) { _wv_controller->update_parameters(); @@ -582,7 +585,7 @@ MulticopterPositionControl::run() } // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, -1.f, !_control_mode.flag_control_climb_rate_enabled); + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled); // takeoff delay for motors to reach idle speed if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { @@ -634,6 +637,30 @@ MulticopterPositionControl::run() // check if all local states are valid and map accordingly set_vehicle_states(setpoint.vz); + // handle smooth takeoff + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled); + constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); + + if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { + // we are not flying yet and need to avoid any corrections + reset_setpoint_to_nan(setpoint); + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + // set yaw-sp to current yaw + // TODO: we need a clean way to disable yaw control + setpoint.yaw = _states.yaw; + setpoint.yawspeed = 0.f; + // prevent any integrator windup + _control.resetIntegralXY(); + _control.resetIntegralZ(); + // reactivate the task which will reset the setpoint to current state + _flight_tasks.reActivate(); + } + + if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { + constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); + } + + // limit altitude only if local position is valid if (PX4_ISFINITE(_states.position(2))) { limit_altitude(setpoint); @@ -672,28 +699,6 @@ MulticopterPositionControl::run() local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; _control.getThrustSetpoint().copyTo(local_pos_sp.thrust); - // handle smooth takeoff - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled); - local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt); - - if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { - // we are not flying yet and need to avoid any corrections - // set the horizontal thrust to zero - local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.0f; - // set yaw-sp to current yaw to avoid any corrections - // TODO: we need a clean way to disable yaw control - local_pos_sp.yaw = _states.yaw; - local_pos_sp.yawspeed = 0.f; - // prevent any integrator windup - _control.resetIntegralXY(); - _control.resetIntegralZ(); - } - - if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { - // reactivate the task which will reset the setpoint to current state - _flight_tasks.reActivate(); - } - // Publish local position setpoint // This message will be used by other modules (such as Landdetector) to determine // vehicle intention.