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

FlightTaskFailsafe: comments and land with land speed if velocity in z is valid but altitude not

Browse Source
This commit is contained in:
Dennis Mannhart
2018-08-30 09:27:48 +02:00
committed by ChristophTobler
parent 3d250b3b83
commit 8714b216b0
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp
+6 -1
View File
@@ -41,7 +41,7 @@ bool FlightTaskFailsafe::activate()
bool ret = FlightTask::activate();
_position_setpoint = _position;
_velocity_setpoint *= 0.0f;
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.5f);
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.6f);
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
return ret;
@@ -50,19 +50,24 @@ bool FlightTaskFailsafe::activate()
bool FlightTaskFailsafe::update()
{
if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
// stay at current position setpoint
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
// don't move along xy
_position_setpoint(0) = _position_setpoint(1) = NAN;
_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
}
if (PX4_ISFINITE(_position(2))) {
// stay at current altitude setpoint
_velocity_setpoint(2) = 0.0f;
_thrust_setpoint(2) = NAN;
} else if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = MPC_LAND_SPEED.get();
_position_setpoint(2) = NAN;
_thrust_setpoint(2) = NAN;
}