diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e99ff76808..7285a93721 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -17,6 +17,9 @@ 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_HIGH_WIND = 32 # (1 << 5) +uint8 FAILURE_BATTERY = 64 # (1 << 6) +uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7) # HIL uint8 HIL_STATE_OFF = 0 diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 2799e2dab0..39c6de0496 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -66,6 +66,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic updateEscsStatus(vehicle_status); } + if (_param_fd_imb_prop_thr.get() > 0) { + updateImbalancedPropStatus(); + } + return _status != previous_status; } @@ -155,3 +159,58 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status) _status &= ~FAILURE_ARM_ESCS; } } + +void FailureDetector::updateImbalancedPropStatus() +{ + + if (_sensor_selection_sub.updated()) { + sensor_selection_s selection; + + if (_sensor_selection_sub.copy(&selection)) { + _selected_accel_device_id = selection.accel_device_id; + } + } + + const bool updated = _vehicle_imu_status_sub.updated(); // save before doing a copy + + // Find the imu_status instance corresponding to the selected accelerometer + vehicle_imu_status_s imu_status{}; + _vehicle_imu_status_sub.copy(&imu_status); + + if (imu_status.accel_device_id != _selected_accel_device_id) { + + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (!_vehicle_imu_status_sub.ChangeInstance(i)) { + continue; + } + + if (_vehicle_imu_status_sub.copy(&imu_status) + && (imu_status.accel_device_id == _selected_accel_device_id)) { + // instance found + break; + } + } + } + + if (updated) { + + if (_vehicle_imu_status_sub.copy(&imu_status)) { + + if ((imu_status.accel_device_id != 0) + && (imu_status.accel_device_id == _selected_accel_device_id)) { + const float dt = math::constrain((float)(imu_status.timestamp - _imu_status_timestamp_prev), 0.01f, 1.f); + _imbalanced_prop_lpf.setParameters(dt, _imbalanced_prop_lpf_time_constant); + + const float var_x = imu_status.var_accel[0]; + const float var_y = imu_status.var_accel[1]; + const float var_z = imu_status.var_accel[2]; + + const float metric = var_x + var_y - var_z; + const float metric_lpf = _imbalanced_prop_lpf.update(metric); + + const bool is_imbalanced = metric_lpf > _param_fd_imb_prop_thr.get(); + _status = (_status & ~FAILURE_IMBALANCED_PROP) | (is_imbalanced * FAILURE_IMBALANCED_PROP); + } + } + } +} diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index e158affa5f..eeed0726a1 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -43,6 +43,7 @@ #pragma once +#include #include #include #include @@ -50,9 +51,11 @@ // subscriptions #include +#include #include #include #include +#include #include #include #include @@ -63,7 +66,10 @@ typedef enum { FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH, FAILURE_ALT = vehicle_status_s::FAILURE_ALT, FAILURE_EXT = vehicle_status_s::FAILURE_EXT, - FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC + FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC, + FAILURE_HIGH_WIND = vehicle_status_s::FAILURE_HIGH_WIND, + FAILURE_BATTERY = vehicle_status_s::FAILURE_BATTERY, + FAILURE_IMBALANCED_PROP = vehicle_status_s::FAILURE_IMBALANCED_PROP } failure_detector_bitmak; using uORB::SubscriptionData; @@ -75,11 +81,13 @@ public: bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); uint8_t getStatus() const { return _status; } + float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); } private: void updateAttitudeStatus(); void updateExternalAtsStatus(); void updateEscsStatus(const vehicle_status_s &vehicle_status); + void updateImbalancedPropStatus(); uint8_t _status{FAILURE_NONE}; @@ -88,9 +96,16 @@ private: systemlib::Hysteresis _ext_ats_failure_hysteresis{false}; systemlib::Hysteresis _esc_failure_hysteresis{false}; + static constexpr float _imbalanced_prop_lpf_time_constant{5.f}; + AlphaFilter _imbalanced_prop_lpf{}; + 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)}; 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)}; DEFINE_PARAMETERS( (ParamInt) _param_fd_fail_p, @@ -99,6 +114,7 @@ private: (ParamFloat) _param_fd_fail_p_ttri, (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, - (ParamInt) _param_escs_en + (ParamInt) _param_escs_en, + (ParamInt) _param_fd_imb_prop_thr ) }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index 581a1a76ec..01444324f9 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.c +++ b/src/modules/commander/failure_detector/failure_detector_params.c @@ -141,3 +141,18 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900); * @group Failure Detector */ PARAM_DEFINE_INT32(FD_ESCS_EN, 1); + +/** + * Imbalanced propeller check threshold + * + * Value at which the imbalanced propeller metric (based on horizontal and + * vertical acceleration variance) triggers a failure + * + * Setting this value to 0 disables the feature. + * + * @min 0 + * @max 1000 + * @increment 1 + * @group Failure Detector + */ +PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);