fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: refactor, move smooth takeoff velocity into a function

Browse Source 
# Conflicts:
#	src/modules/mc_pos_control/mc_pos_control_main.cpp
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent d5f90a1503
commit fabf214bca
1 changed files with 13 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+13 -5
View File
@@ -392,6 +392,8 @@ private:
bool manual_wants_takeoff();
void set_takeoff_velocity(float &vel_sp_z);
/**
* Shim for calling task_main from task_create.
*/
@@ -2515,11 +2517,7 @@ MulticopterPositionControl::calculate_velocity_setpoint()
/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
if (_in_smooth_takeoff) {
_in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += -_vel_sp(2) * _dt / _takeoff_ramp_time.get();
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
set_takeoff_velocity(_vel_sp(2));
}
/* make sure velocity setpoint is constrained in all directions (xyz) */
@@ -3238,6 +3236,16 @@ MulticopterPositionControl::task_main()
_control_task = -1;
}
void
MulticopterPositionControl::set_takeoff_velocity(float &vel_sp_z)
{
_in_smooth_takeoff = _takeoff_vel_limit < -vel_sp_z;
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += -vel_sp_z * _dt / _takeoff_ramp_time.get();
/* limit vertical velocity to the current ramp value */
vel_sp_z = math::max(vel_sp_z, -_takeoff_vel_limit);
}
int
MulticopterPositionControl::start()
{