fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 07:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

smooth takeoff - Support smooth takeoff triggered by jerk setpoint

Browse Source
This commit is contained in:
bresch
2019-02-14 12:24:35 +01:00
committed by Beat Küng
parent 0d170a0eaf
commit 8f584a1496
3 changed files with 20 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+9 -2
View File
@@ -49,10 +49,12 @@ bool FlightTaskManualPositionSmoothVel::activate()
void FlightTaskManualPositionSmoothVel::reActivate()
{
reset(Axes::XY);
// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
// using the generated jerk, reset the z derivatives to zero
reset(Axes::XYZ, true);
}
void FlightTaskManualPositionSmoothVel::reset(Axes axes)
void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero)
{
int count;
@@ -75,6 +77,11 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes)
_smoothing[i].reset(0.f, _velocity(i), _position(i));
}
// Set the z derivatives to zero
if (force_z_zero) {
_smoothing[2].reset(0.f, 0.f, _position(2));
}
_position_lock_xy_active = false;
_position_lock_z_active = false;
_position_setpoint_xy_locked(0) = NAN;