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

mc_pos_control: move failsafe logic into method

Browse Source
This commit is contained in:
Dennis Mannhart
2018-06-27 10:18:04 +02:00
committed by Lorenz Meier
parent ac2c49218d
commit 260284571d
1 changed files with 30 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+30 -15
View File
@@ -222,6 +222,14 @@ private:
*/
void start_flight_task();
/**
* Failsafe.
* If flighttask fails for whatever reason, then do failsafe. This could
* occur if the commander fails to switch to a mode in case of invalid states or
* setpoints.
*/
void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states);
/**
* Shim for calling task_main from task_create.
*/
@@ -542,21 +550,7 @@ MulticopterPositionControl::task_main()
if (!_flight_tasks.update()) {
// FAILSAFE
// Task was not able to update correctly. Do Failsafe.
setpoint.x = setpoint.y = setpoint.z = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
if (PX4_ISFINITE(_states.velocity(2))) {
// We have a valid velocity in D-direction.
// descend downwards with landspeed.
setpoint.vz = _land_speed.get();
setpoint.thrust[0] = setpoint.thrust[1] = 0.0f;
warn_rate_limited("Failsafe: Descend with land-speed.");
} else {
// Use the failsafe from the PositionController.
warn_rate_limited("Failsafe: Descend with just attitude control.");
}
failsafe(setpoint, _states);
} else {
setpoint = _flight_tasks.getPositionSetpoint();
@@ -864,6 +858,27 @@ MulticopterPositionControl::limit_thrust_during_landing(matrix::Vector3f &thr_sp
}
}
void
MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states)
{
setpoint.x = setpoint.y = setpoint.z = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
if (PX4_ISFINITE(_states.velocity(2))) {
// We have a valid velocity in D-direction.
// descend downwards with landspeed.
setpoint.vz = _land_speed.get();
setpoint.thrust[0] = setpoint.thrust[1] = 0.0f;
warn_rate_limited("Failsafe: Descend with land-speed.");
} else {
// Use the failsafe from the PositionController.
warn_rate_limited("Failsafe: Descend with just attitude control.");
}
}
void
MulticopterPositionControl::publish_attitude()
{