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

mc_pos_control: refactor smooth takeoff names 

The comments and variable names were partly misleading.
I grouped all members, hopefully gave them more
understandable names and adjusted the comments.
This commit is contained in:
Matthias Grob 2019-05-11 13:34:59 +02:00 committed by Lorenz Meier
parent 6d5d09c231
commit bc77302fc9
1 changed files with 49 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -102,8 +102,11 @@ public:
int print_status() override;
private:
bool _in_smooth_takeoff = false; /**< true if takeoff ramp is applied */
// smooth takeoff vertical velocity ramp state
bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */
bool _velocity_triggered_takeoff = false; /**< true if takeoff was triggered by a velocity setpoint */
float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */
float _takeoff_reference_z; /**< z-position when takeoff ramp was initiated */
orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */
@ -122,11 +125,6 @@ private:
int _task_failure_count{0}; /**< counter for task failures */
float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */
float _takeoff_reference_z; /**< Z-position when takeoff was initiated */
bool _smooth_velocity_takeoff =
false; /**< Smooth velocity takeoff can be initiated either through position or velocity setpoint */
vehicle_status_s _vehicle_status{}; /**< vehicle status */
/**< vehicle-land-detection: initialze to landed */
vehicle_land_detected_s _vehicle_land_detected = {
@ -233,13 +231,13 @@ private:
void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj);
/**
* Update smooth takeoff setpoint ramp to bring the vehicle off the ground without step.
* Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step
* @param z_sp position setpoint in the z-Direction
* @param vz_sp velocity setpoint in the z-Direction
* @param jz_sp jerk setpoint in the z-Direction
* @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly
*/
void update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance);
void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground);
/**
* Adjust the setpoint during landing.
@ -649,14 +647,14 @@ MulticopterPositionControl::run()
// 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_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground);
update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground);
}
// disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards
if (_in_smooth_takeoff) {
if (_in_takeoff_ramp) {
// during smooth takeoff, constrain speed to takeoff speed
constraints.speed_up = _takeoff_speed;
// 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);
@ -675,7 +673,7 @@ MulticopterPositionControl::run()
}
}
if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) {
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;
@ -734,7 +732,7 @@ MulticopterPositionControl::run()
// Part of landing logic: if ground-contact/maybe landed was detected, turn off
// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
// Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.
if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) {
if (!_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) {
limit_thrust_during_landing(local_pos_sp);
}
@ -980,61 +978,60 @@ MulticopterPositionControl::start_flight_task()
}
void
MulticopterPositionControl::update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance)
MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground)
{
// Check for smooth takeoff
if (_vehicle_land_detected.landed && !_in_smooth_takeoff) {
// Vehicle is still landed and no takeoff was initiated yet.
// Adjust for different takeoff cases.
// The minimum takeoff altitude needs to be at least 20cm above minimum distance or, if valid, above minimum distance
// above ground.
float min_altitude = PX4_ISFINITE(min_ground_clearance) ? (min_ground_clearance + 0.05f) :
0.2f;
// check if takeoff is triggered
if (_vehicle_land_detected.landed && !_in_takeoff_ramp) {
// vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered
// minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator
float min_altitude = 0.2f;
if (PX4_ISFINITE(min_distance_to_ground)) {
min_altitude = min_distance_to_ground + 0.05f;
}
// takeoff was initiated through velocity setpoint
_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
_smooth_velocity_takeoff |= jerk_triggered_takeoff;
// upwards velocity setpoint larger than a minimal speed
_velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
// upwards jerk setpoint
const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
// position setpoint above the minimum altitude
const bool position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude));
if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) {
// There is a position setpoint above current position or velocity setpoint larger than
// takeoff speed. Enable smooth takeoff.
_in_smooth_takeoff = true;
_takeoff_speed = 0.f;
_velocity_triggered_takeoff |= jerk_triggered_takeoff;
if (_velocity_triggered_takeoff || position_triggered_takeoff) {
// takeoff was triggered, start velocity ramp
_takeoff_ramp_velocity = 0.f;
_in_takeoff_ramp = true;
_takeoff_reference_z = _states.position(2);
}
}
// If in smooth takeoff, adjust setpoints based on what is valid:
// 1. position setpoint is valid -> go with takeoffspeed to specific altitude
// 2. position setpoint not valid but velocity setpoint valid: ramp up velocity
if (_in_smooth_takeoff) {
float desired_tko_speed = -vz_sp;
// if in smooth takeoff limit upwards velocity setpoint to a smooth ramp
if (_in_takeoff_ramp) {
float takeoff_desired_velocity = -vz_sp;
// If there is a valid position setpoint, then set the desired speed to the takeoff speed.
if (!_smooth_velocity_takeoff) {
desired_tko_speed = _param_mpc_tko_speed.get();
// if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter
if (!_velocity_triggered_takeoff) {
takeoff_desired_velocity = _param_mpc_tko_speed.get();
}
// Ramp up takeoff speed.
// ramp up vrtical velocity in TKO_RAMP_T seconds
if (_param_mpc_tko_ramp_t.get() > _dt) {
_takeoff_speed += desired_tko_speed * _dt / _param_mpc_tko_ramp_t.get();
_takeoff_ramp_velocity += takeoff_desired_velocity * _dt / _param_mpc_tko_ramp_t.get();
} else {
// No ramp, directly go to desired takeoff speed
_takeoff_speed = desired_tko_speed;
// no ramp, directly go to desired takeoff speed
_takeoff_ramp_velocity = takeoff_desired_velocity;
}
_takeoff_speed = math::min(_takeoff_speed, _param_mpc_tko_speed.get());
_takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, _param_mpc_tko_speed.get());
// Smooth takeoff is achieved once desired altitude/velocity setpoint is reached.
if (!_smooth_velocity_takeoff) {
_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());
// smooth takeoff is achieved once desired altitude/velocity setpoint is reached
if (!_velocity_triggered_takeoff) {
_in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());
} else {
// Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
_in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed;
// make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
_in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp) || _vehicle_land_detected.landed;
}
}
}