Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit fac3e1c

Browse files
MaEtUgRLorenzMeier
MaEtUgR
authored and
LorenzMeier
committed
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.
1 parent 90c6fea commit fac3e1c

File tree

3 files changed

+58
-45
lines changed

3 files changed

+58
-45
lines changed

‎src/modules/mc_pos_control/Takeoff/Takeoff.cpp

Lines changed: 18 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -36,10 +36,16 @@
3636
*/
3737

3838
#include "Takeoff.hpp"
39-
#include <float.h>
39+
#include <mathlib/mathlib.h>
40+
41+
void Takeoff::generateInitialValue(const float hover_thrust, float velocity_p_gain)
42+
{
43+
velocity_p_gain = math::max(velocity_p_gain, 0.01f);
44+
_takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain;
45+
}
4046

4147
void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
42-
const float takeoff_desired_thrust, const bool skip_takeoff)
48+
const float takeoff_desired_vz, const bool skip_takeoff)
4349
{
4450
_spoolup_time_hysteresis.set_state_and_update(armed);
4551

@@ -63,14 +69,14 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool
6369
case TakeoffState::ready_for_takeoff:
6470
if (want_takeoff) {
6571
_takeoff_state = TakeoffState::rampup;
66-
_takeoff_ramp_thrust = 0.0f;
72+
_takeoff_ramp_vz = _takeoff_ramp_vz_init;
6773

6874
} else {
6975
break;
7076
}
7177

7278
case TakeoffState::rampup:
73-
if (_takeoff_ramp_thrust <= takeoff_desired_thrust) {
79+
if (_takeoff_ramp_vz >= takeoff_desired_vz) {
7480
_takeoff_state = TakeoffState::flight;
7581

7682
} else {
@@ -98,24 +104,24 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool
98104
}
99105
}
100106

101-
float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt)
107+
float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz)
102108
{
103109
if (_takeoff_state < TakeoffState::rampup) {
104-
return 0.f;
110+
return _takeoff_ramp_vz_init;
105111
}
106112

107113
if (_takeoff_state == TakeoffState::rampup) {
108-
if (_takeoff_ramp_time > FLT_EPSILON) {
109-
_takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time;
114+
if (_takeoff_ramp_time > dt) {
115+
_takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time;
110116

111117
} else {
112-
_takeoff_ramp_thrust = takeoff_desired_thrust;
118+
_takeoff_ramp_vz = takeoff_desired_vz;
113119
}
114120

115-
if (_takeoff_ramp_thrust > takeoff_desired_thrust) {
116-
return _takeoff_ramp_thrust;
121+
if (_takeoff_ramp_vz < takeoff_desired_vz) {
122+
return _takeoff_ramp_vz;
117123
}
118124
}
119125

120-
return takeoff_desired_thrust;
126+
return takeoff_desired_vz;
121127
}

‎src/modules/mc_pos_control/Takeoff/Takeoff.hpp

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -55,29 +55,31 @@ enum class TakeoffState {
5555
class Takeoff
5656
{
5757
public:
58-
5958
Takeoff() = default;
6059
~Takeoff() = default;
6160

61+
// initialize parameters
62+
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
63+
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
64+
void generateInitialValue(const float hover_thrust, const float velocity_p_gain);
65+
6266
/**
6367
* Update the state for the takeoff.
6468
* @param setpoint a vehicle_local_position_setpoint_s structure
6569
* @return true if setpoint has updated correctly
6670
*/
6771
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
68-
const float takeoff_desired_thrust, const bool skip_takeoff);
69-
float updateThrustRamp(const float dt, const float takeoff_desired_thrust);
72+
const float takeoff_desired_vz, const bool skip_takeoff);
73+
float updateRamp(const float dt, const float takeoff_desired_vz);
7074

71-
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
72-
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
7375
TakeoffState getTakeoffState() { return _takeoff_state; }
7476

75-
// TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time
76-
systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
77-
7877
private:
7978
TakeoffState _takeoff_state = TakeoffState::disarmed;
8079

8180
float _takeoff_ramp_time = 0.f;
82-
float _takeoff_ramp_thrust = 0.f;
81+
float _takeoff_ramp_vz_init = 0.f;
82+
float _takeoff_ramp_vz = 0.f;
83+
84+
systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
8385
};

‎src/modules/mc_pos_control/mc_pos_control_main.cpp

Lines changed: 29 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,9 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
151151
(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
152152
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
153153
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
154-
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */
154+
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
155+
(ParamFloat<px4::params::MPC_THR_HOVER>)_param_mpc_thr_hover,
156+
(ParamFloat<px4::params::MPC_Z_VEL_P>)_param_mpc_z_vel_p
155157
);
156158

157159
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -358,6 +360,7 @@ MulticopterPositionControl::parameters_update(bool force)
358360
// set trigger time for takeoff delay
359361
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
360362
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
363+
_takeoff.generateInitialValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get());
361364

362365
if (_wv_controller != nullptr) {
363366
_wv_controller->update_parameters();
@@ -582,7 +585,7 @@ MulticopterPositionControl::run()
582585
}
583586

584587
// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
585-
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, -1.f, !_control_mode.flag_control_climb_rate_enabled);
588+
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled);
586589

587590
// takeoff delay for motors to reach idle speed
588591
if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) {
@@ -634,6 +637,30 @@ MulticopterPositionControl::run()
634637
// check if all local states are valid and map accordingly
635638
set_vehicle_states(setpoint.vz);
636639

640+
// handle smooth takeoff
641+
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled);
642+
constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);
643+
644+
if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
645+
// we are not flying yet and need to avoid any corrections
646+
reset_setpoint_to_nan(setpoint);
647+
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
648+
// set yaw-sp to current yaw
649+
// TODO: we need a clean way to disable yaw control
650+
setpoint.yaw = _states.yaw;
651+
setpoint.yawspeed = 0.f;
652+
// prevent any integrator windup
653+
_control.resetIntegralXY();
654+
_control.resetIntegralZ();
655+
// reactivate the task which will reset the setpoint to current state
656+
_flight_tasks.reActivate();
657+
}
658+
659+
if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
660+
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
661+
}
662+
663+
637664
// limit altitude only if local position is valid
638665
if (PX4_ISFINITE(_states.position(2))) {
639666
limit_altitude(setpoint);
@@ -672,28 +699,6 @@ MulticopterPositionControl::run()
672699
local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
673700
_control.getThrustSetpoint().copyTo(local_pos_sp.thrust);
674701

675-
// handle smooth takeoff
676-
_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);
677-
local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt);
678-
679-
if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
680-
// we are not flying yet and need to avoid any corrections
681-
// set the horizontal thrust to zero
682-
local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.0f;
683-
// set yaw-sp to current yaw to avoid any corrections
684-
// TODO: we need a clean way to disable yaw control
685-
local_pos_sp.yaw = _states.yaw;
686-
local_pos_sp.yawspeed = 0.f;
687-
// prevent any integrator windup
688-
_control.resetIntegralXY();
689-
_control.resetIntegralZ();
690-
}
691-
692-
if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
693-
// reactivate the task which will reset the setpoint to current state
694-
_flight_tasks.reActivate();
695-
}
696-
697702
// Publish local position setpoint
698703
// This message will be used by other modules (such as Landdetector) to determine
699704
// vehicle intention.

0 commit comments

Comments
 (0)