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

FlightTask - When switching task, pass the last setpoints of the previous task to the new task

Browse Source 
This is done to allow proper initialization of the new FlightTask and
give it a chance to continue the setpoints without discontinuity. The
function checkSetpoints replaces the setpoints containing NANs with an
estimate of the state. The estimate is usually the current estimate of
the EKF or zero.
The transition FlightTask also provides an estimate of the current
acceleration to properly initialize the next FlightTask after
back-transition. This avoid having to initialize the accelerations to
zero knowing that the actual acceleration is usually far from zero.
This commit is contained in:
bresch
2019-07-02 16:11:44 +02:00
committed by Mathieu Bresciani
parent d24c415fd7
commit 1414f50cea
28 changed files with 151 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+43 -2
View File
@@ -22,7 +22,7 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
return true;
}
bool FlightTask::activate()
bool FlightTask::activate(vehicle_local_position_setpoint_s state_prev)
{
_resetSetpoints();
_setDefaultConstraints();
@@ -34,7 +34,7 @@ bool FlightTask::activate()
void FlightTask::reActivate()
{
activate();
activate(getPositionSetpoint());
}
bool FlightTask::updateInitialize()
@@ -47,6 +47,47 @@ bool FlightTask::updateInitialize()
return true;
}
void FlightTask::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
{
// If the position setpoint is unknown, set to the current postion
if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
// If the velocity setpoint is unknown, set to the current velocity
if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
// No jerk estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.jerk_x)) { setpoints.jerk_x = 0.f; }
if (!PX4_ISFINITE(setpoints.jerk_y)) { setpoints.jerk_y = 0.f; }
if (!PX4_ISFINITE(setpoints.jerk_z)) { setpoints.jerk_z = 0.f; }
if (!PX4_ISFINITE(setpoints.thrust[0])) { setpoints.thrust[0] = 0.f; }
if (!PX4_ISFINITE(setpoints.thrust[1])) { setpoints.thrust[1] = 0.f; }
if (!PX4_ISFINITE(setpoints.thrust[2])) { setpoints.thrust[2] = -0.5f; }
if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
if (!PX4_ISFINITE(setpoints.yawspeed)) { setpoints.yawspeed = 0.f; }
}
const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
{
/* fill position setpoint message */