Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
mc_pos_control: move failsafe logic into method
Browse files Browse the repository at this point in the history
Stifael authored and MaEtUgR committed Jun 27, 2018

Verified

This commit was signed with the committer’s verified signature. The key has expired.
addaleax Anna Henningsen
1 parent 923204a commit 300e6e2
Showing 1 changed file with 30 additions and 15 deletions.
45 changes: 30 additions & 15 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
Original file line number Diff line number Diff line change
@@ -222,6 +222,14 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara
*/
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()
{

0 comments on commit 300e6e2

Please sign in to comment.