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

mc_pos_control: refactor smooth takeoff call 

There were two rather confusing function calls one to check
if smooth takeoff needs to be ran and one that updates it.
I combined them and documented the interface correctly
making the parameters non-reference const floats.
This commit is contained in:
Matthias Grob 2019-05-11 10:54:16 +02:00 committed by Lorenz Meier
parent 3413df19e8
commit 6d5d09c231
1 changed files with 10 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -233,19 +233,13 @@ private:
void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj);
/**
* Checks if smooth takeoff is initiated.
* @param position_setpoint_z the position setpoint in the z-Direction
* @param velocity setpoint_z the velocity setpoint in the z-Direction
* Update smooth takeoff setpoint 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 check_for_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z,
const float &jerk_sp, const vehicle_constraints_s &constraints);
/**
* Check if smooth takeoff has ended and updates accordingly.
* @param position_setpoint_z the position setpoint in the z-Direction
* @param velocity setpoint_z the velocity setpoint in the z-Direction
*/
void update_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z);
void update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance);
/**
* Adjust the setpoint during landing.
@ -655,8 +649,7 @@ 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))) {
check_for_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints);
update_smooth_takeoff(setpoint.z, setpoint.vz);
update_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground);
}
// disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards
@ -987,8 +980,7 @@ MulticopterPositionControl::start_flight_task()
}
void
MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const float &vz_sp,
const float &jerk_sp, const vehicle_constraints_s &constraints)
MulticopterPositionControl::update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance)
{
// Check for smooth takeoff
if (_vehicle_land_detected.landed && !_in_smooth_takeoff) {
@ -996,12 +988,12 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl
// 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(constraints.min_distance_to_ground) ? (constraints.min_distance_to_ground + 0.05f) :
float min_altitude = PX4_ISFINITE(min_ground_clearance) ? (min_ground_clearance + 0.05f) :
0.2f;
// takeoff was initiated through velocity setpoint
_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
bool jerk_triggered_takeoff = PX4_ISFINITE(jerk_sp) && jerk_sp < -FLT_EPSILON;
bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
_smooth_velocity_takeoff |= jerk_triggered_takeoff;
if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) {
@ -1013,11 +1005,7 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl
}
}
}
void
MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float &vz_sp)
{
// 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