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

mc_pos_control: replace takeoff velocity ramp with thrust ramp 

The velocity ramp had problems with:
- different vehicle tunings resulted in the start value of the resulting
thrust ramp staring either higher and lower than zero thrust.
lower -> delay of beginning
higher -> small jump at beginning
- when a task set position and velocity at the same time during takeoff
(which AutoSmoothVel does) it resulted in a velocity setpoint
jump at the end of the ramp because the additional velocity
setpoint correction from the position controller was not considered.

The thrust ramp should now be very deterministic:
- always start at zero
- always end at the curreant thrust setpoint output
of the complete position controller
This commit is contained in:
Matthias Grob 2019-05-15 08:15:38 +02:00 committed by Lorenz Meier
parent bb055fdaf3
commit da533a7b1d
1 changed files with 50 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -104,8 +104,7 @@ public:
private:
// smooth takeoff vertical velocity ramp state
bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */
bool _position_triggered_takeoff = false; /**< true if takeoff was triggered by a position setpoint */
float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */
float _takeoff_ramp_thrust = 0.f; /**< current value of the smooth takeoff ramp */
float _takeoff_reference_z = 0.f; /**< z-position when takeoff ramp was initiated */
orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
@ -236,7 +235,7 @@ private:
* @param z_sp position setpoint in the z-Direction
* @param vz_constraint velocity to ramp to when there's only a position setpoint
*/
void update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint);
void update_takeoff_ramp(const bool want_takeoff, const float thr_sp);
/**
* Adjust the setpoint during landing.
@ -644,43 +643,6 @@ MulticopterPositionControl::run()
// check if all local states are valid and map accordingly
set_vehicle_states(setpoint.vz);
// do smooth takeoff after delay if there's a valid vertical velocity or position
if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) {
update_takeoff_ramp(constraints.want_takeoff, setpoint.vz, setpoint.z, constraints.speed_up);
}
// disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards
if (_in_takeoff_ramp) {
// during smooth takeoff, constrain speed to ramp state
constraints.speed_up = _takeoff_ramp_velocity;
// altitude above reference takeoff
const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z);
// disable yaw control when close to ground
if (alt_above_tko <= ALTITUDE_THRESHOLD) {
setpoint.yawspeed = NAN;
// if there is a valid yaw estimate, just set setpoint to yaw
if (PX4_ISFINITE(_states.yaw)) {
setpoint.yaw = _states.yaw;
}
// limit tilt during smooth takeoff when still close to ground
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}
}
if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) {
// Keep throttle low when landed and NOT in smooth takeoff
reset_setpoint_to_nan(setpoint);
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
setpoint.yaw = _states.yaw;
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}
// limit altitude only if local position is valid
if (PX4_ISFINITE(_states.position(2))) {
limit_altitude(setpoint);
@ -719,6 +681,46 @@ MulticopterPositionControl::run()
local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
_control.getThrustSetpoint().copyTo(local_pos_sp.thrust);
// do smooth takeoff after delay if there's a valid vertical velocity or position
if (_spoolup_time_hysteresis.get_state()) {
update_takeoff_ramp(constraints.want_takeoff, local_pos_sp.thrust[2]);
}
// disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards
if (_in_takeoff_ramp) {
// altitude above reference takeoff
const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z);
// disable yaw control when close to ground
if (alt_above_tko <= ALTITUDE_THRESHOLD) {
setpoint.yawspeed = NAN;
// if there is a valid yaw estimate, just set setpoint to yaw
if (PX4_ISFINITE(_states.yaw)) {
setpoint.yaw = _states.yaw;
}
// limit tilt during smooth takeoff when still close to ground
constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
}
}
if (_in_takeoff_ramp) {
local_pos_sp.thrust[2] = math::max(local_pos_sp.thrust[2], _takeoff_ramp_thrust);
//local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.f;
_control.resetIntegralXY();
_control.resetIntegralZ();
}
if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) {
// Keep throttle low when landed and NOT in smooth takeoff
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
setpoint.yaw = _states.yaw;
// 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.
@ -977,40 +979,29 @@ MulticopterPositionControl::start_flight_task()
}
void
MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint)
MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float thr_sp)
{
const float takeoff_ramp_initialization = -0.7f;
// check if takeoff is triggered
if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) {
// takeoff was triggered, start velocity ramp
_takeoff_ramp_velocity = takeoff_ramp_initialization;
_takeoff_ramp_thrust = 0.0f;
_in_takeoff_ramp = true;
_takeoff_reference_z = _states.position(2);
}
// if in smooth takeoff limit upwards velocity setpoint to a smooth ramp
// if in smooth takeoff limit upwards thrust setpoint to a smooth ramp
if (_in_takeoff_ramp) {
float takeoff_desired_velocity = -vz_sp;
// if there's only a position setpoint we go up with the configured takeoff speed
if (!PX4_ISFINITE(vz_sp) && PX4_ISFINITE(z_sp)) {
takeoff_desired_velocity = vz_constraint;
}
float takeoff_desired_thrust = thr_sp;
if (_param_mpc_tko_ramp_t.get() > _dt) {
_takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get();
_takeoff_ramp_thrust += takeoff_desired_thrust * _dt / _param_mpc_tko_ramp_t.get();
} else {
// no ramp time, directly jump to desired velocity
_takeoff_ramp_velocity = takeoff_desired_velocity;
_takeoff_ramp_thrust = takeoff_desired_thrust;
}
// make sure we cannot overshoot the desired setpoint with the ramp
_takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity);
// smooth takeoff is finished once desired velocity setpoint is reached
_in_takeoff_ramp = (_takeoff_ramp_velocity < takeoff_desired_velocity);
// smooth takeoff is finished once desired thrust setpoint is reached
_in_takeoff_ramp = (_takeoff_ramp_thrust > takeoff_desired_thrust);
}
}