diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index cb2d684b890..ad234362b69 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -171,7 +171,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic failure_detector_status_u status_prev = _status; if (vehicle_control_mode.flag_control_attitude_enabled) { - updateAttitudeStatus(); + updateAttitudeStatus(vehicle_status); if (_param_fd_ext_ats_en.get()) { updateExternalAtsStatus(); @@ -206,15 +206,31 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic return _status.value != status_prev.value; } -void FailureDetector::updateAttitudeStatus() +void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status) { vehicle_attitude_s attitude; if (_vehicle_attitude_sub.update(&attitude)) { const matrix::Eulerf euler(matrix::Quatf(attitude.q)); - const float roll(euler.phi()); - const float pitch(euler.theta()); + float roll(euler.phi()); + float pitch(euler.theta()); + + // special handling for tailsitter + if (vehicle_status.is_vtol_tailsitter) { + if (vehicle_status.in_transition_mode) { + // disable attitude check during tailsitter transition + roll = 0.f; + pitch = 0.f; + + } else if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // in FW flight rotate the attitude by 90° around pitch (level FW flight = 0° pitch) + const matrix::Eulerf euler_rotated = matrix::Eulerf(matrix::Quatf(attitude.q) * matrix::Quatf(matrix::Eulerf(0.f, + M_PI_2_F, 0.f))); + roll = euler_rotated.phi(); + pitch = euler_rotated.theta(); + } + } const float max_roll_deg = _param_fd_fail_r.get(); const float max_pitch_deg = _param_fd_fail_p.get(); diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index ba66c44e3a6..61edc82e646 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -107,7 +107,7 @@ public: uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; } private: - void updateAttitudeStatus(); + void updateAttitudeStatus(const vehicle_status_s &vehicle_status); void updateExternalAtsStatus(); 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);