dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Matthias Grob 2019-05-17 08:17:24 +01:00 committed by Lorenz Meier
parent 90c6fea408
commit fac3e1c3f9
3 changed files with 58 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
View File

@ -36,10 +36,16 @@
*/
#include "Takeoff.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
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;
}

20
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
View File

@ -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 */
};

53
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -151,7 +151,9 @@ private:
(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_HOVER>)_param_mpc_thr_hover,
(ParamFloat<px4::params::MPC_Z_VEL_P>)_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.