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

AutoSmoothVel - Override checkTakeoff with task-specific logic and reactivate z axis with downward velocity to takeoff smoothly

Browse Source
This commit is contained in:
bresch
2019-05-22 22:05:38 +02:00
committed by Lorenz Meier
parent 7c7d980cf0
commit bb055fdaf3
2 changed files with 14 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+8 -4
View File
@@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate()
void FlightTaskAutoLineSmoothVel::reActivate()
{
// Don't reset during takeoff TODO: Find a proper solution
// The issue here is that with a small increment of velocity setpoint (generated by this flight task), the
// land detector doesn't detect takeoff and without takeoff detection, the
// flight task is always reset.
// On ground, reset acceleration and velocity to zero
for (int i = 0; i < 2; ++i) {
_trajectory[i].reset(0.f, 0.f, _position(i));
}
_trajectory[2].reset(0.f, 0.7f, _position(2));
}
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
@@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
_checkEkfResetCounters();
_want_takeoff = false;
if (PX4_ISFINITE(_position_setpoint(0)) &&
PX4_ISFINITE(_position_setpoint(1))) {
@@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
_velocity_setpoint(2) = vel_sp_z;
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
}