dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

failure_detector: add motor/ESC failure detection

This commit is contained in:
Alessandro Simovic 2021-11-26 16:50:40 +01:00 committed by Beat Küng
parent dfd934fbdb
commit fb71e7587c
25 changed files with 338 additions and 441 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/failure_detector_status.msg
View File

@ -8,5 +8,7 @@ bool fd_ext
bool fd_arm_escs
bool fd_battery
bool fd_imbalanced_prop
bool fd_motor
float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed)
uint16 motor_failure_mask # Bit-mask with motor indices, indicating critical motor failures

22
msg/vehicle_status.msg
View File

@ -11,14 +11,15 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
uint8 ARMING_STATE_MAX = 6
# FailureDetector status
uint8 FAILURE_NONE = 0
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ARM_ESC = 16 # (1 << 4)
uint8 FAILURE_BATTERY = 32 # (1 << 5)
uint8 FAILURE_IMBALANCED_PROP = 64 # (1 << 6)
uint16 FAILURE_NONE = 0
uint16 FAILURE_ROLL = 1 # (1 << 0)
uint16 FAILURE_PITCH = 2 # (1 << 1)
uint16 FAILURE_ALT = 4 # (1 << 2)
uint16 FAILURE_EXT = 8 # (1 << 3)
uint16 FAILURE_ARM_ESC = 16 # (1 << 4)
uint16 FAILURE_BATTERY = 32 # (1 << 5)
uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6)
uint16 FAILURE_MOTOR = 128 # (1 << 7)
# HIL
uint8 HIL_STATE_OFF = 0
@ -31,7 +32,7 @@ uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure
uint8 NAVIGATION_STATE_UNUSED3 = 8 # Free slot
uint8 NAVIGATION_STATE_UNUSED = 9 # Free slot
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
uint8 NAVIGATION_STATE_UNUSED1 = 11 # Free slot
@ -85,11 +86,10 @@ uint8 data_link_lost_counter # counts unique data link lost events
bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
bool engine_failure # Set to true if an engine failure is detected
bool mission_failure # Set to true if mission could not continue/finish
bool geofence_violated
uint8 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
uint16 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
# see SYS_STATUS mavlink message for the following
# lower 32 bits are for the base flags, higher 32 bits are or the extended flags

1
msg/vehicle_status_flags.msg
View File

@ -28,7 +28,6 @@ bool flight_terminated
bool circuit_breaker_engaged_power_check
bool circuit_breaker_engaged_airspd_check
bool circuit_breaker_engaged_enginefailure_check
bool circuit_breaker_flight_termination_disabled
bool circuit_breaker_engaged_usb_check
bool circuit_breaker_engaged_posfailure_check # set to true when the position valid checks have been disabled

4
src/drivers/osd/atxxxx/atxxxx.cpp
View File

@ -408,10 +408,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
flight_mode = "AUTO";
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
flight_mode = "FAILURE";
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
flight_mode = "ACRO";
break;

4
src/drivers/rc_input/crsf_telemetry.cpp
View File

@ -162,10 +162,6 @@ bool CRSFTelemetry::send_flight_mode()
flight_mode = "Auto";
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
flight_mode = "Failure";
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
flight_mode = "Acro";
break;

1
src/lib/circuit_breaker/circuit_breaker.h
View File

@ -56,7 +56,6 @@
#define CBRK_IO_SAFETY_KEY 22027
#define CBRK_AIRSPD_CHK_KEY 162128
#define CBRK_FLIGHTTERM_KEY 121212
#define CBRK_ENGINEFAIL_KEY 284953
#define CBRK_USB_CHK_KEY 197848
#define CBRK_VELPOSERR_KEY 201607
#define CBRK_VTOLARMING_KEY 159753

16
src/lib/circuit_breaker/circuit_breaker_params.c
View File

@ -115,22 +115,6 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
*/
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
/**
* Circuit breaker for engine failure detection
*
* Setting this parameter to 284953 will disable the engine failure detection.
* If the aircraft is in engine failure mode the engine failure flag will be
* set to healthy
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @reboot_required true
* @min 0
* @max 284953
* @category Developer
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
/**
* Circuit breaker for disabling buzzer
*

116
src/modules/commander/Commander.cpp
View File

@ -2734,49 +2734,6 @@ Commander::run()
avoidance_check();
// engine failure detection
// TODO: move out of commander
if (_actuator_controls_sub.updated()) {
/* Check engine failure
* only for fixed wing for now
*/
if (!_status_flags.circuit_breaker_engaged_enginefailure_check &&
_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol && _armed.armed) {
actuator_controls_s actuator_controls{};
_actuator_controls_sub.copy(&actuator_controls);
const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
const float current2throttle = _battery_current / throttle;
if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get()))
|| _status.engine_failure) {
const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f;
/* potential failure, measure time */
if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get())
&& !_status.engine_failure) {
_status.engine_failure = true;
_status_changed = true;
PX4_ERR("Engine Failure");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status);
}
}
} else {
/* no failure reset flag */
_timestamp_engine_healthy = hrt_absolute_time();
if (_status.engine_failure) {
_status.engine_failure = false;
_status_changed = true;
}
}
}
/* check if we are disarmed and there is a better mode to wait in */
if (!_armed.armed) {
/* if there is no radio control but GPS lock the user might want to fly using
@ -2822,6 +2779,8 @@ Commander::run()
/* Check for failure detector status */
if (_failure_detector.update(_status, _vehicle_control_mode)) {
const bool motor_failure_changed = ((_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) !=
_failure_detector.getStatus().flags.motor;
_status.failure_detector_status = _failure_detector.getStatus().value;
auto fd_status_flags = _failure_detector.getStatusFlags();
_status_changed = true;
@ -2884,6 +2843,67 @@ Commander::run()
(imbalanced_propeller_action_t)_param_com_imb_prop_act.get());
}
}
// One-time actions based on motor failure
if (motor_failure_changed) {
if (fd_status_flags.motor) {
mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t");
events::send(events::ID("commander_motor_failure"), events::Log::Emergency,
"Motor failure! Land immediately");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status);
} else {
mavlink_log_critical(&_mavlink_log_pub, "Motor recovered\t");
events::send(events::ID("commander_motor_recovered"), events::Log::Warning,
"Motor recovered, landing still advised");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, true, _status);
}
}
if (fd_status_flags.motor) {
switch ((ActuatorFailureActions)_param_com_actuator_failure_act.get()) {
case ActuatorFailureActions::AUTO_LOITER:
mavlink_log_critical(&_mavlink_log_pub, "Loitering due to actuator failure\t");
events::send(events::ID("commander_act_failure_loiter"), events::Log::Warning,
"Loitering due to actuator failure");
main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
_status_changed = true;
break;
case ActuatorFailureActions::AUTO_LAND:
mavlink_log_critical(&_mavlink_log_pub, "Landing due to actuator failure\t");
events::send(events::ID("commander_act_failure_land"), events::Log::Warning,
"Landing due to actuator failure");
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state);
_status_changed = true;
break;
case ActuatorFailureActions::AUTO_RTL:
mavlink_log_critical(&_mavlink_log_pub, "Returning home due to actuator failure\t");
events::send(events::ID("commander_act_failure_rtl"), events::Log::Warning,
"Returning home due to actuator failure");
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state);
_status_changed = true;
break;
case ActuatorFailureActions::TERMINATE:
if (!_armed.manual_lockdown) {
mavlink_log_critical(&_mavlink_log_pub, "Flight termination due to actuator failure\t");
events::send(events::ID("commander_act_failure_term"), events::Log::Warning,
"Flight termination due to actuator failure");
_status_changed = true;
_armed.manual_lockdown = true;
send_parachute_command();
}
break;
default:
// nothing to do here
break;
}
}
}
// Check wind speed actions if either high wind warning or high wind RTL is enabled
@ -3070,7 +3090,9 @@ Commander::run()
fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
_failure_detector_status_pub.publish(fd_status);
}
@ -3178,8 +3200,6 @@ Commander::get_circuit_breaker_params()
CBRK_USB_CHK_KEY);
_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(),
CBRK_AIRSPD_CHK_KEY);
_status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled_by_val(_param_cbrk_enginefail.get(),
CBRK_ENGINEFAIL_KEY);
_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(),
CBRK_FLIGHTTERM_KEY);
_status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_velposerr.get(),
@ -3420,7 +3440,6 @@ Commander::update_control_mode()
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
@ -3832,8 +3851,6 @@ void Commander::battery_status_check()
hrt_abstime oldest_update = hrt_absolute_time();
float worst_battery_time_s{NAN};
_battery_current = 0.0f;
for (auto &battery_sub : _battery_status_subs) {
int index = battery_sub.get_instance();
battery_status_s battery;
@ -3919,8 +3936,6 @@ void Commander::battery_status_check()
worst_battery_time_s = battery.time_remaining_s;
}
// Sum up current from all batteries.
_battery_current += battery.current_filtered_a;
}
}
@ -4386,6 +4401,7 @@ void Commander::esc_status_check()
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
events::px4::enums::suggested_action_t::none;
// TODO: use esc_status.esc[index].actuator_function as index after SYS_CTRL_ALLOC becomes default
events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
events::Log::Critical, "ESC{1} offline. {2}", index + 1, action);
}

17
src/modules/commander/Commander.hpp
View File

@ -64,7 +64,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/action_request.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
@ -240,9 +239,7 @@ private:
(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
// Engine failure
(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,
(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
@ -256,7 +253,6 @@ private:
(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
@ -283,6 +279,14 @@ private:
OFFBOARD_MODE_BIT = (1 << 1),
};
enum class ActuatorFailureActions {
DISABLED = 0,
AUTO_LOITER = 1,
AUTO_LAND = 2,
AUTO_RTL = 3,
TERMINATE = 4,
};
/* Decouple update interval and hysteresis counters, all depends on intervals */
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
@ -341,7 +345,6 @@ private:
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _battery_failsafe_timestamp{0};
float _battery_current{0.0f};
uint8_t _last_connected_batteries{0};
uint32_t _last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
uint16_t _last_battery_fault[battery_status_s::MAX_INSTANCES] {};
@ -368,7 +371,6 @@ private:
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
hrt_abstime _timestamp_engine_healthy{0}; ///< absolute time when engine was healty
hrt_abstime _overload_start{0}; ///< time when CPU overload started
hrt_abstime _led_armed_state_toggle{0};
@ -405,7 +407,6 @@ private:
// Subscriptions
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};

60
src/modules/commander/commander_params.c
View File

@ -128,49 +128,6 @@ PARAM_DEFINE_INT32(COM_HLDL_LOSS_T, 120);
*/
PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0);
/**
* Engine Failure Throttle Threshold
*
* Engine failure triggers only above this throttle value
*
* @group Commander
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
/**
* Engine Failure Current/Throttle Threshold
*
* Engine failure triggers only below this current value
*
* @group Commander
* @min 0.0
* @max 50.0
* @unit A/%
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
/**
* Engine Failure Time Threshold
*
* Engine failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time
*
* @group Commander
* @unit s
* @min 0.0
* @max 60.0
* @decimal 1
* @increment 1
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
/**
* RC loss time threshold
*
@ -893,6 +850,23 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
*/
PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
/**
* Set the actuator failure failsafe mode
*
* Note: actuator failure needs to be enabled and configured via FD_ACT_*
* parameters.
*
* @min 0
* @max 3
* @value 0 Disabled
* @value 1 Hold mode
* @value 2 Land mode
* @value 3 Return mode
* @value 4 Terminate
* @group Commander
*/
PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0);
/**
* Flag to enable obstacle avoidance.
*

145
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -65,8 +65,17 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
_status.flags.ext = false;
}
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status);
// esc_status subscriber is shared between subroutines
esc_status_s esc_status;
if (_esc_status_sub.update(&esc_status)) {
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status, esc_status);
}
if (_param_fd_actuator_en.get()) {
updateMotorStatus(vehicle_status, esc_status);
}
}
if (_param_fd_imb_prop_thr.get() > 0) {
@ -80,7 +89,7 @@ void FailureDetector::updateAttitudeStatus()
{
vehicle_attitude_s attitude;
if (_vehicule_attitude_sub.update(&attitude)) {
if (_vehicle_attitude_sub.update(&attitude)) {
const matrix::Eulerf euler(matrix::Quatf(attitude.q));
const float roll(euler.phi());
@ -127,22 +136,26 @@ void FailureDetector::updateExternalAtsStatus()
}
}
void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
hrt_abstime time_now = hrt_absolute_time();
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
esc_status_s esc_status;
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);
if (_esc_status_sub.update(&esc_status)) {
int all_escs_armed = (1 << esc_status.esc_count) - 1;
bool is_esc_failure = !is_all_escs_armed;
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
for (int i = 0; i < limited_esc_count; i++) {
is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
}
if (_esc_failure_hysteresis.get_state()) {
_status.flags.arm_escs = true;
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now);
if (_esc_failure_hysteresis.get_state()) {
_status.flags.arm_escs = true;
}
} else {
@ -209,3 +222,111 @@ void FailureDetector::updateImbalancedPropStatus()
}
}
}
void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
// What need to be checked:
//
// 1. ESC telemetry disappears completely -> dead ESC or power loss on that ESC
// 2. ESC failures like overvoltage, overcurrent etc. But DShot driver for example is not populating the field 'esc_report.failures'
// 3. Motor current too low. Compare drawn motor current to expected value from a parameter
// -- ESC voltage does not really make sense and is highly dependent on the setup
// First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC
// Then check
const hrt_abstime time_now = hrt_absolute_time();
// Only check while armed
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);
// Check individual ESC reports
for (int esc_status_idx = 0; esc_status_idx < limited_esc_count; esc_status_idx++) {
const esc_report_s &cur_esc_report = esc_status.esc[esc_status_idx];
// Map the esc status index to the actuator function index
const unsigned i_esc = cur_esc_report.actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
if (i_esc >= actuator_motors_s::NUM_CONTROLS) {
continue;
}
// Check if ESC telemetry was available and valid at some point. This is a prerequisite for the failure detection.
if (!(_motor_failure_esc_valid_current_mask & (1 << i_esc)) && cur_esc_report.esc_current > 0.0f) {
_motor_failure_esc_valid_current_mask |= (1 << i_esc);
}
// Check for telemetry timeout
const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
const bool esc_timed_out = telemetry_age > 100_ms; // TODO: magic number
const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc);
const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);
if (esc_was_valid && esc_timed_out && !esc_timeout_currently_flagged) {
// Set flag
_motor_failure_esc_timed_out_mask |= (1 << i_esc);
} else if (!esc_timed_out && esc_timeout_currently_flagged) {
// Reset flag
_motor_failure_esc_timed_out_mask &= ~(1 << i_esc);
}
// Check if ESC current is too low
float esc_throttle = 0.f;
if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
esc_throttle = fabsf(actuator_motors.control[i_esc]);
}
const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
_param_fd_motor_current2throttle_thres.get();
if (throttle_above_threshold && current_too_low && !esc_timed_out) {
if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
_motor_failure_undercurrent_start_time[i_esc] = time_now;
}
} else {
if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
}
}
if (_motor_failure_undercurrent_start_time[i_esc] != 0
&& (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
&& (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
// Set flag
_motor_failure_esc_under_current_mask |= (1 << i_esc);
} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
}
bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
if (critical_esc_failure && !(_status.flags.motor)) {
// Add motor failure flag to bitfield
_status.flags.motor = true;
} else if (!critical_esc_failure && _status.flags.motor) {
// Reset motor failure flag
_status.flags.motor = false;
}
} else { // Disarmed
// reset ESC bitfield
for (int i_esc = 0; i_esc < actuator_motors_s::NUM_CONTROLS; i_esc++) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
}
_motor_failure_esc_under_current_mask = 0;
_status.flags.motor = false;
}
}

26
src/modules/commander/failure_detector/FailureDetector.hpp
View File

@ -51,6 +51,7 @@
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
@ -69,6 +70,7 @@ union failure_detector_status_u {
uint16_t arm_escs : 1;
uint16_t battery : 1;
uint16_t imbalanced_prop : 1;
uint16_t motor : 1;
} flags;
uint16_t value {0};
};
@ -79,16 +81,19 @@ class FailureDetector : public ModuleParams
{
public:
FailureDetector(ModuleParams *parent);
~FailureDetector() = default;
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
const failure_detector_status_u &getStatus() const { return _status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
private:
void updateAttitudeStatus();
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status);
void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateImbalancedPropStatus();
failure_detector_status_u _status{};
@ -103,11 +108,18 @@ private:
uint32_t _selected_accel_device_id{0};
hrt_abstime _imu_status_timestamp_prev{0};
uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
// Motor failure check
uint8_t _motor_failure_esc_valid_current_mask{}; // ESC 1-8, true if ESC telemetry was valid at some point
uint8_t _motor_failure_esc_timed_out_mask{}; // ESC telemetry no longer available -> failure
uint8_t _motor_failure_esc_under_current_mask{}; // ESC drawing too little current -> failure
hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@ -117,6 +129,12 @@ private:
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
// Actuator failure
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_actuator_en,
(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_motor_throttle_thres,
(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_motor_current2throttle_thres,
(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_motor_time_thres
)
};

56
src/modules/commander/failure_detector/failure_detector_params.c
View File

@ -156,3 +156,59 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
/**
* Enable Actuator Failure check
*
* If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
* level is being consumed.
* Otherwise this indicates an motor failure.
*
* @boolean
* @reboot_required true
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ACT_EN, 1);
/**
* Motor Failure Throttle Threshold
*
* Motor failure triggers only above this throttle value.
*
* @group Failure Detector
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
/**
* Motor Failure Current/Throttle Threshold
*
* Motor failure triggers only below this current value
*
* @group Failure Detector
* @min 0.0
* @max 50.0
* @unit A/%
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 2.0f);
/**
* Motor Failure Time Threshold
*
* Motor failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time.
*
* @group Failure Detector
* @unit ms
* @min 10
* @max 10000
* @increment 100
*/
PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 100);

50
src/modules/commander/state_machine_helper.cpp
View File

@ -383,9 +383,6 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags.vtol_transition_failure) {
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
@ -434,10 +431,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status_flags.vtol_transition_failure) {
@ -475,10 +469,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
/* require global position and home, also go into failsafe on an engine failure */
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@ -488,12 +479,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
/* require global position and home */
// require global position and home
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
@ -503,15 +491,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
break;
case commander_state_s::MAIN_STATE_ORBIT:
if (status.engine_failure) {
// failsafe: on engine failure
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
// Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
@ -548,12 +528,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
/* require local position */
// require local position
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status_flags.vtol_transition_failure) {
@ -590,12 +567,10 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
case commander_state_s::MAIN_STATE_AUTO_LAND:
/* require local position */
// require local position
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
@ -606,12 +581,9 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
/* must be rotary wing plus same requirements as normal landing */
// must be rotary wing plus same requirements as normal landing
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {

7
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -581,9 +581,10 @@ void FixedwingAttitudeControl::Run()
_wheel_ctrl.reset_integrator();
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* throttle passed through if it is finite */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])) ?
_att_sp.thrust_body[0] :
0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&

6
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -2682,12 +2682,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
_reinitialize_tecs = false;
}
if (_vehicle_status.engine_failure) {
/* Force the slow downwards spiral */
pitch_min_rad = radians(-1.0f);
pitch_max_rad = radians(5.0f);
}
/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);

5
src/modules/mavlink/mavlink_messages.cpp
View File

@ -260,11 +260,6 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;

3
src/modules/mavlink/streams/HEARTBEAT.hpp
View File

@ -131,9 +131,6 @@ private:
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
system_status = MAV_STATE_FLIGHT_TERMINATION;
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
system_status = MAV_STATE_EMERGENCY;
} else if (vehicle_status_flags.calibration_enabled) {
system_status = MAV_STATE_CALIBRATING;
}

2
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -437,7 +437,7 @@ private:
msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
}
if (status.engine_failure) {
if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) {
msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
}

1
src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp
View File

@ -141,7 +141,6 @@ private:
bool vehicle_in_auto_mode = (vehicle_status.timestamp > 0)
&& (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER

1
src/modules/navigator/CMakeLists.txt
View File

@ -48,7 +48,6 @@ px4_add_module(
precland.cpp
mission_feasibility_checker.cpp
geofence.cpp
enginefailure.cpp
follow_target.cpp
vtol_takeoff.cpp
DEPENDS

140
src/modules/navigator/enginefailure.cpp
View File

@ -1,140 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* @file enginefailure.cpp
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <lib/geo/geo.h>
#include <navigator/navigation.h>
#include <px4_platform_common/events.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include "navigator.h"
#include "enginefailure.h"
EngineFailure::EngineFailure(Navigator *navigator) :
MissionBlock(navigator),
_ef_state(EF_STATE_NONE)
{
}
void
EngineFailure::on_inactive()
{
_ef_state = EF_STATE_NONE;
}
void
EngineFailure::on_activation()
{
_ef_state = EF_STATE_NONE;
advance_ef();
set_ef_item();
}
void
EngineFailure::on_active()
{
if (is_mission_item_reached()) {
advance_ef();
set_ef_item();
}
}
void
EngineFailure::set_ef_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous = pos_sp_triplet->current;
_navigator->set_can_loiter_at_sp(false);
switch (_ef_state) {
case EF_STATE_LOITERDOWN: {
//XXX create mission item at ground (below?) here
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
//XXX setting altitude to a very low value, evaluate other options
_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
default:
break;
}
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
void
EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down\t");
events::send(events::ID("enginefailure_loitering"), events::Log::Emergency, "Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
break;
}
}

73
src/modules/navigator/enginefailure.h
View File

@ -1,73 +0,0 @@
/***************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file enginefailure.h
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class EngineFailure : public MissionBlock
{
public:
EngineFailure(Navigator *navigator);
~EngineFailure() = default;
void on_inactive() override;
void on_activation() override;
void on_active() override;
private:
enum EFState {
EF_STATE_NONE = 0,
EF_STATE_LOITERDOWN = 1,
} _ef_state{EF_STATE_NONE};
/**
* Set the DLL item
*/
void set_ef_item();
/**
* Move to next EF item
*/
void advance_ef();
};

4
src/modules/navigator/navigator.h
View File

@ -41,7 +41,6 @@
#pragma once
#include "enginefailure.h"
#include "follow_target.h"
#include "geofence.h"
#include "land.h"
@ -86,7 +85,7 @@ using namespace time_literals;
/**
* Number of navigation modes that need on_active/on_inactive calls
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 9
#define NAVIGATOR_MODE_ARRAY_SIZE 8
class Navigator : public ModuleBase<Navigator>, public ModuleParams
{
@ -393,7 +392,6 @@ private:
Land _land; /**< class for handling land commands */
PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
FollowTarget _follow_target;
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */

17
src/modules/navigator/navigator_main.cpp
View File

@ -78,19 +78,17 @@ Navigator::Navigator() :
_land(this),
_precland(this),
_rtl(this),
_engineFailure(this),
_follow_target(this)
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_engineFailure;
_navigation_mode_array[4] = &_takeoff;
_navigation_mode_array[5] = &_land;
_navigation_mode_array[6] = &_precland;
_navigation_mode_array[7] = &_vtol_takeoff;
_navigation_mode_array[8] = &_follow_target;
_navigation_mode_array[3] = &_takeoff;
_navigation_mode_array[4] = &_land;
_navigation_mode_array[5] = &_precland;
_navigation_mode_array[6] = &_vtol_takeoff;
_navigation_mode_array[7] = &_follow_target;
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
_handle_reverse_delay = param_find("VT_B_REV_DEL");
@ -697,11 +695,6 @@ void Navigator::run()
_precland.set_mode(PrecLandMode::Required);
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_follow_target;