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

mc_pos_control: print Failsafe message only once when entering failsafe

Browse Source
This commit is contained in:
Beat Küng 2018-09-27 13:17:00 +02:00 committed by Lorenz Meier
parent 720b3307d8
commit 079a43238f
1 changed files with 18 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -150,6 +150,8 @@ private:
hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */
/** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500000;
/**< number of tries before switching to a failsafe flight task */
@ -246,7 +248,8 @@ private:
* setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
* to true, the failsafe will be initiated immediately.
*/
void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force);
void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force,
const bool warn);
/**
* Fill desired vehicle_trajectory_waypoint:
@ -588,6 +591,9 @@ MulticopterPositionControl::run()
// set dt for control blocks
setDt(dt);
const bool was_in_failsafe = _in_failsafe;
_in_failsafe = false;
// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
// that turns the nose of the vehicle into the wind
if (_wv_controller != nullptr) {
@ -630,7 +636,7 @@ MulticopterPositionControl::run()
if (!_flight_tasks.update()) {
// FAILSAFE
// Task was not able to update correctly. Do Failsafe.
failsafe(setpoint, _states, false);
failsafe(setpoint, _states, false, !was_in_failsafe);
} else {
setpoint = _flight_tasks.getPositionSetpoint();
@ -640,13 +646,13 @@ MulticopterPositionControl::run()
if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
!(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
!(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
failsafe(setpoint, _states, true);
failsafe(setpoint, _states, true, !was_in_failsafe);
}
// Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
// of these setpoints are valid
if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) {
failsafe(setpoint, _states, true);
failsafe(setpoint, _states, true, !was_in_failsafe);
}
}
@ -1010,7 +1016,7 @@ MulticopterPositionControl::limit_thrust_during_landing(matrix::Vector3f &thr_sp
void
MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
const bool force)
const bool force, const bool warn)
{
_failsafe_land_hysteresis.set_state_and_update(true);
@ -1030,12 +1036,17 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
// 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.");
if (warn) {
PX4_WARN("Failsafe: Descend with land-speed.");
}
} else {
// Use the failsafe from the PositionController.
warn_rate_limited("Failsafe: Descend with just attitude control.");
if (warn) {
PX4_WARN("Failsafe: Descend with just attitude control.");
}
}
_in_failsafe = true;
}
}