From fb71e7587c76ac883928870a23789e6b182d46b9 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Fri, 26 Nov 2021 16:50:40 +0100 Subject: [PATCH] failure_detector: add motor/ESC failure detection --- msg/failure_detector_status.msg | 2 + msg/vehicle_status.msg | 22 +-- msg/vehicle_status_flags.msg | 1 - src/drivers/osd/atxxxx/atxxxx.cpp | 4 - src/drivers/rc_input/crsf_telemetry.cpp | 4 - src/lib/circuit_breaker/circuit_breaker.h | 1 - .../circuit_breaker/circuit_breaker_params.c | 16 -- src/modules/commander/Commander.cpp | 116 ++++++++------ src/modules/commander/Commander.hpp | 17 +- src/modules/commander/commander_params.c | 60 ++------ .../failure_detector/FailureDetector.cpp | 145 ++++++++++++++++-- .../failure_detector/FailureDetector.hpp | 26 +++- .../failure_detector_params.c | 56 +++++++ .../commander/state_machine_helper.cpp | 50 ++---- .../FixedwingAttitudeControl.cpp | 7 +- .../FixedwingPositionControl.cpp | 6 - src/modules/mavlink/mavlink_messages.cpp | 5 - src/modules/mavlink/streams/HEARTBEAT.hpp | 3 - src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 2 +- .../mavlink/streams/UTM_GLOBAL_POSITION.hpp | 1 - src/modules/navigator/CMakeLists.txt | 1 - src/modules/navigator/enginefailure.cpp | 140 ----------------- src/modules/navigator/enginefailure.h | 73 --------- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 17 +- 25 files changed, 338 insertions(+), 441 deletions(-) delete mode 100644 src/modules/navigator/enginefailure.cpp delete mode 100644 src/modules/navigator/enginefailure.h diff --git a/msg/failure_detector_status.msg b/msg/failure_detector_status.msg index 3466d2dc96..923ceb36da 100644 --- a/msg/failure_detector_status.msg +++ b/msg/failure_detector_status.msg @@ -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 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 919de6bdc3..3a415ea50d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 6b8343ff95..0d36707314 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -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 diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index 730c7a871d..b504b943c7 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -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; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 3c3b2271a5..4cec3f8294 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -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; diff --git a/src/lib/circuit_breaker/circuit_breaker.h b/src/lib/circuit_breaker/circuit_breaker.h index 1e19e8d2f3..4029f2f7e9 100644 --- a/src/lib/circuit_breaker/circuit_breaker.h +++ b/src/lib/circuit_breaker/circuit_breaker.h @@ -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 diff --git a/src/lib/circuit_breaker/circuit_breaker_params.c b/src/lib/circuit_breaker/circuit_breaker_params.c index 1eccec0034..f19d6a33d7 100644 --- a/src/lib/circuit_breaker/circuit_breaker_params.c +++ b/src/lib/circuit_breaker/circuit_breaker_params.c @@ -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 * diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 96fa2705bd..d84e18bef1 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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(events::ID("commander_esc_offline"), events::Log::Critical, "ESC{1} offline. {2}", index + 1, action); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6a7a1ca6d1..5397d87c15 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include @@ -240,9 +239,7 @@ private: (ParamFloat) _param_com_lkdown_tko, // Engine failure - (ParamFloat) _param_ef_throttle_thres, - (ParamFloat) _param_ef_current2throttle_thres, - (ParamFloat) _param_ef_time_thres, + (ParamInt) _param_com_actuator_failure_act, (ParamBool) _param_arm_without_gps, (ParamBool) _param_arm_mission_required, @@ -256,7 +253,6 @@ private: (ParamInt) _param_cbrk_supply_chk, (ParamInt) _param_cbrk_usb_chk, (ParamInt) _param_cbrk_airspd_chk, - (ParamInt) _param_cbrk_enginefail, (ParamInt) _param_cbrk_flightterm, (ParamInt) _param_cbrk_velposerr, (ParamInt) _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)}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2553dff4a5..b16c5cf65d 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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. * diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index e57d4a940b..14b98a4484 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -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; + } +} diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 5798c6dd9f..c55fefb169 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -51,6 +51,7 @@ // subscriptions #include +#include #include #include #include @@ -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) _param_fd_fail_p, @@ -117,6 +129,12 @@ private: (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, (ParamInt) _param_escs_en, - (ParamInt) _param_fd_imb_prop_thr + (ParamInt) _param_fd_imb_prop_thr, + + // Actuator failure + (ParamBool) _param_fd_actuator_en, + (ParamFloat) _param_fd_motor_throttle_thres, + (ParamFloat) _param_fd_motor_current2throttle_thres, + (ParamInt) _param_fd_motor_time_thres ) }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index 01444324f9..60ee007477 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.c +++ b/src/modules/commander/failure_detector/failure_detector_params.c @@ -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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 71efd6b83a..002e71d6db 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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 { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 45d05f8a4c..245a0da160 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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() && diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 493a19d9a5..cafd944d20 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fdef95417d..b974495099 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index ddc6884557..4da54af5a9 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -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; } diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index a46e4ef0bf..a7312d29a9 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -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; } diff --git a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp index f441bd6f69..2cf17b69f2 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -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 diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 9075f34926..5da49df860 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -48,7 +48,6 @@ px4_add_module( precland.cpp mission_feasibility_checker.cpp geofence.cpp - enginefailure.cpp follow_target.cpp vtol_takeoff.cpp DEPENDS diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp deleted file mode 100644 index 9977799542..0000000000 --- a/src/modules/navigator/enginefailure.cpp +++ /dev/null @@ -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 -*/ -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#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; - } -} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h deleted file mode 100644 index 8ba17d8246..0000000000 --- a/src/modules/navigator/enginefailure.h +++ /dev/null @@ -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 - */ - -#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(); - -}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8553f13146..aeffa50fda 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -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, 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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1be889b9d8..615cb045fd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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;