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

FlightTask - Move checkSetpoints function in each FlightTask to be tailored to the specific needs and avoid checking all the setpoints

Browse Source
This commit is contained in:
bresch
2019-07-08 10:57:50 +02:00
committed by Mathieu Bresciani
parent 24811cf550
commit 37a9b90945
10 changed files with 75 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
-41
View File
@@ -47,47 +47,6 @@ 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 */