From fac3e1c3f9e30c911208d799a999e3a413e6843c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 17 May 2019 08:17:24 +0100 Subject: [PATCH] mc_pos_control: switch back to velocity ramp But fix the two crucial problems: - When to begin the ramp? There's a calculation now for the velocity ramp initial value such that the resulting thrust is zero at the beginning. - When to end the ramp? The ramp is applied to the upwards velocity constraint and it just ramps from the initial value to the velocity constraint which is applied during flight. Slower/going down is always possible. --- .../mc_pos_control/Takeoff/Takeoff.cpp | 30 ++++++----- .../mc_pos_control/Takeoff/Takeoff.hpp | 20 +++---- .../mc_pos_control/mc_pos_control_main.cpp | 53 ++++++++++--------- 3 files changed, 58 insertions(+), 45 deletions(-) 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.