fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 16:33:34 +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-28 08:59:22 +02:00
committed by Lorenz Meier
parent 720b3307d8
commit 079a43238f
1 changed files with 18 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+18 -7
View File
@@ -150,6 +150,8 @@ private:
hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ 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 */ /** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500000; static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500000;
/**< number of tries before switching to a failsafe flight task */ /**< 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 * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
* to true, the failsafe will be initiated immediately. * 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: * Fill desired vehicle_trajectory_waypoint:
@@ -588,6 +591,9 @@ MulticopterPositionControl::run()
// set dt for control blocks // set dt for control blocks
setDt(dt); 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 // 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 // that turns the nose of the vehicle into the wind
if (_wv_controller != nullptr) { if (_wv_controller != nullptr) {
@@ -630,7 +636,7 @@ MulticopterPositionControl::run()
if (!_flight_tasks.update()) { if (!_flight_tasks.update()) {
// FAILSAFE // FAILSAFE
// Task was not able to update correctly. Do Failsafe. // Task was not able to update correctly. Do Failsafe.
failsafe(setpoint, _states, false); failsafe(setpoint, _states, false, !was_in_failsafe);
} else { } else {
setpoint = _flight_tasks.getPositionSetpoint(); setpoint = _flight_tasks.getPositionSetpoint();
@@ -640,13 +646,13 @@ MulticopterPositionControl::run()
if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) && if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
!(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) && !(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
!(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) { !(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 // Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
// of these setpoints are valid // of these setpoints are valid
if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) { 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 void
MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, 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); _failsafe_land_hysteresis.set_state_and_update(true);
@@ -1030,12 +1036,17 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
// descend downwards with landspeed. // descend downwards with landspeed.
setpoint.vz = _land_speed.get(); setpoint.vz = _land_speed.get();
setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; 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 { } else {
// Use the failsafe from the PositionController. // 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;
} }
} }