From 619812cbdd071bdf5141b5e09711784fb73498e6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 27 May 2024 18:04:50 +0200 Subject: [PATCH] CA testing hacks, external failure message, sitl tuning, disabling of rate controller saturation Signed-off-by: Silvan Fuhrer --- .../1040_gazebo-classic_standard_vtol | 35 ++++++--- msg/CMakeLists.txt | 1 + msg/FailureDetectorExtServo.msg | 5 ++ msg/FailureDetectorStatus.msg | 2 + src/lib/mixer_module/mixer_module.cpp | 38 +++++++++- src/lib/mixer_module/mixer_module.hpp | 8 +- src/lib/mixer_module/params.c | 20 +++++ src/lib/rate_control/rate_control.cpp | 4 + src/modules/commander/Commander.cpp | 76 +++++++++++++++++++ src/modules/commander/Commander.hpp | 11 ++- src/modules/commander/commander_params.c | 28 +++++++ .../control_allocator/ControlAllocator.cpp | 35 +++++++++ .../control_allocator/ControlAllocator.hpp | 1 + 13 files changed, 251 insertions(+), 13 deletions(-) create mode 100644 msg/FailureDetectorExtServo.msg diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol index 0e25b56330..39981d328f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol @@ -15,17 +15,17 @@ param set-default FD_ACT_MOT_TOUT 500 param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 1 param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 param set-default CA_ROTOR3_KM -0.05 param set-default CA_ROTOR4_AX 1 param set-default CA_ROTOR4_AZ 0 @@ -33,9 +33,9 @@ param set-default CA_ROTOR4_PX 0.2 param set-default CA_SV_CS_COUNT 3 param set-default CA_SV_CS0_TYPE 1 -param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TRQ_R -1 param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TRQ_R 1 param set-default CA_SV_CS2_TYPE 3 param set-default CA_SV_CS2_TRQ_P 1 param set-default PWM_MAIN_FUNC1 101 @@ -79,3 +79,18 @@ param set-default VT_FWD_THRUST_SC 1 param set-default VT_F_TRANS_THR 0.75 param set-default VT_TYPE 2 +param set-default CA_FW_DTHR_SC_R 1 +param set-default CA_FAILURE_MODE 1 + +# Two ways to lock servo1 and servo2, either in allocation (will automatically notify about the failure), in in the output mixer +# Only enable one of the two options + +# output mixer (NO automatic failure notification) +# param set-default OUT_SRV_FAIL_IPT 1 +# param set-default OUT_SRV_FAIL_NR 2 + +# allocation (automatic failure notification) +param set-default COM_SRV_FAIL_IPT 1 +param set-default COM_SRV_FAIL_NR 2 + +param set-default CA_FW_DTHR_AIRMD 1 # has to be enabled otheriwse the MC motors have no differential thrust capability diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 53ec65cf9a..4d16df98a5 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -93,6 +93,7 @@ set(msg_files Event.msg FigureEightStatus.msg FailsafeFlags.msg + FailureDetectorExtServo.msg FailureDetectorStatus.msg FlightPhaseEstimation.msg FollowTarget.msg diff --git a/msg/FailureDetectorExtServo.msg b/msg/FailureDetectorExtServo.msg new file mode 100644 index 0000000000..e356a6cad5 --- /dev/null +++ b/msg/FailureDetectorExtServo.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +bool fd_servo + +uint16 servo_failure_mask # Bit-mask with servo indices, indicating critical servo failures diff --git a/msg/FailureDetectorStatus.msg b/msg/FailureDetectorStatus.msg index 5aa9cb18cb..ef94398a7f 100644 --- a/msg/FailureDetectorStatus.msg +++ b/msg/FailureDetectorStatus.msg @@ -14,3 +14,5 @@ bool fd_servo 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 uint16 servo_failure_mask # Bit-mask with servo indices, indicating critical servo failures + +uint16 servo_to_center_mask # HACK to test allocation without some servos diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index ee75bcf9fa..0060eef95b 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -417,6 +417,28 @@ bool MixingOutput::update() _throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode; } + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_sp_sub.update(&manual_control_setpoint)) { + + switch (_param_out_srv_fail_ipt.get()) { + case 0: + // disable + _servo_locking_injection_active = false; + break; + + case 1: + // yaw stick above 60% to enable + _servo_locking_injection_active = manual_control_setpoint.yaw > 0.6f; + break; + + case 2: + // aux1 above 60% to enable + _servo_locking_injection_active = manual_control_setpoint.aux1 > 0.6f; + break; + } + } + // only used for sitl with lockstep bool has_updates = _subscription_callback && _subscription_callback->updated(); @@ -442,7 +464,21 @@ bool MixingOutput::update() all_disabled = false; if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) { - outputs[i] = _functions[i]->value(_function_assignment[i]); + + float factor = 1.f; + + if (_servo_locking_injection_active) { + if (_function_assignment[i] == OutputFunction::Servo1 && (_param_out_srv_fail_nr.get() == 0 + || _param_out_srv_fail_nr.get() == 2)) { + factor = 0.f; + + } else if (_function_assignment[i] == OutputFunction::Servo2 && (_param_out_srv_fail_nr.get() == 1 + || _param_out_srv_fail_nr.get() == 2)) { + factor = 0.f; + } + } + + outputs[i] = _functions[i]->value(_function_assignment[i]) * factor; } else { outputs[i] = NAN; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 3deaa54aa4..ca82b42708 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -59,6 +59,7 @@ #include #include #include +#include using namespace time_literals; @@ -258,6 +259,9 @@ private: uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; + bool _servo_locking_injection_active{false}; + uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs)}; actuator_armed_s _armed{}; @@ -296,6 +300,8 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode, ///< multicopter air-mode (ParamFloat) _param_mot_slew_max, - (ParamFloat) _param_thr_mdl_fac ///< thrust to motor control signal modelling factor + (ParamFloat) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor + (ParamInt) _param_out_srv_fail_ipt, + (ParamInt) _param_out_srv_fail_nr ) }; diff --git a/src/lib/mixer_module/params.c b/src/lib/mixer_module/params.c index e54a25eca0..796b5e99bc 100644 --- a/src/lib/mixer_module/params.c +++ b/src/lib/mixer_module/params.c @@ -16,3 +16,23 @@ * @group Mixer Output */ PARAM_DEFINE_INT32(MC_AIRMODE, 0); + +/** + * Manual control source to inject servo failure + * + * @group Mixer Output + * @value 0 Disabled + * @value 1 yaw stick + * @value 2 aux1 + */ +PARAM_DEFINE_INT32(OUT_SRV_FAIL_IPT, 0); + +/** + * Index of the servos to fail + * + * @group Mixer Output + * @value 0 Lock servo 1 + * @value 1 Lock servo 2 + * @value 2 Lock servo 1 and 2 + */ +PARAM_DEFINE_INT32(OUT_SRV_FAIL_NR, 0); diff --git a/src/lib/rate_control/rate_control.cpp b/src/lib/rate_control/rate_control.cpp index baa4c8e7b5..7aa3ed111c 100644 --- a/src/lib/rate_control/rate_control.cpp +++ b/src/lib/rate_control/rate_control.cpp @@ -52,6 +52,10 @@ void RateControl::setSaturationStatus(const Vector3 &saturation_positive, { _control_allocator_saturation_positive = saturation_positive; _control_allocator_saturation_negative = saturation_negative; + + const Vector3 saturation_disabled(false, false, false); + _control_allocator_saturation_positive = saturation_disabled; + _control_allocator_saturation_negative = saturation_disabled; } void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c4e3d331e4..fdef75b363 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -74,6 +74,8 @@ #include #include +using namespace time_literals; + typedef enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -1907,6 +1909,77 @@ void Commander::run() _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); + // HACK: inject aileron failure (stuck to center) through here. Assume servo 0 is an aileron. + manual_control_setpoint_s manual_control_setpoint; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + bool aileron_failure_injected = false; + bool servo_failure_detected = false; + int failed_servo_bitmask = 0; + + if (!_param_com_srv_fail_cl.get()) { + + if (_param_com_srv_fail_ipt.get() == 1) { + + aileron_failure_injected = manual_control_setpoint.yaw > 0.6f; + + } else if (_param_com_srv_fail_ipt.get() == 2) { + aileron_failure_injected = manual_control_setpoint.aux1 > 0.6f; + } + + if (aileron_failure_injected) { + if (_time_failure_injected == 0) { + _time_failure_injected = hrt_absolute_time(); + } + + servo_failure_detected = hrt_elapsed_time(&_time_failure_injected) > 1_s; // trigger detection 2s after injection + + } else { + _time_failure_injected = 0; + servo_failure_detected = false; + } + + if (_param_com_srv_fail_nr.get() == 0) { + failed_servo_bitmask = 1 << 0; + + } else if (_param_com_srv_fail_nr.get() == 1) { + failed_servo_bitmask = 1 << 1; + + } else if (_param_com_srv_fail_nr.get() == 2) { + failed_servo_bitmask = (1 << 0) + (1 << 1); + } + + } else { + _time_failure_injected = 0; // reset open loop variable + aileron_failure_injected = false; + + if (_failure_detector_ext_servo.updated()) { + _failure_detector_ext_servo.update(); + _time_last_ext_fail_topic = now; + + } + + if ((_time_last_ext_fail_topic > 0UL) && ((now - _time_last_ext_fail_topic) < 500_ms)) { + // aileron_failure_injected = _failure_detector_ext_servo.get().fd_servo; + servo_failure_detected = _failure_detector_ext_servo.get().fd_servo; + failed_servo_bitmask = _failure_detector_ext_servo.get().servo_failure_mask; + } + } + + // Inform operator about detected servo failures + if (servo_failure_detected && failed_servo_bitmask > _reported_servo_fail) { + _reported_servo_fail = failed_servo_bitmask; + + if ((_reported_servo_fail == (1 << 0)) || (_reported_servo_fail == (1 << 1))) { + events::send(events::ID("single_aileron_failure"), events::Log::Critical, + "Actuator failure detected: single aileron fault"); + + } else if (_reported_servo_fail == ((1 << 1) + (1 << 0))) { + events::send(events::ID("double_aileron_failure"), events::Log::Critical, + "Actuator failure detected: double aileron fault"); + } + } + // failure_detector_status publish failure_detector_status_s fd_status{}; fd_status.fd_roll = _failure_detector.getStatusFlags().roll; @@ -1917,8 +1990,11 @@ void Commander::run() 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.fd_servo = servo_failure_detected; fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); fd_status.motor_failure_mask = _failure_detector.getMotorFailures(); + fd_status.servo_failure_mask = servo_failure_detected ? failed_servo_bitmask : 0; + fd_status.servo_to_center_mask = aileron_failure_injected ? failed_servo_bitmask : 0; fd_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(fd_status); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 9431381d8c..1b54549775 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -280,6 +281,10 @@ private: bool _have_taken_off_since_arming{false}; bool _status_changed{true}; + hrt_abstime _time_failure_injected{0}; + hrt_abstime _time_last_ext_fail_topic{0U}; + uint16_t _reported_servo_fail{UINT16_C(0)}; + vehicle_land_detected_s _vehicle_land_detected{}; // commander publications @@ -308,6 +313,7 @@ private: uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::SubscriptionData _failure_detector_ext_servo{ORB_ID(failure_detector_ext_servo)}; // Publications uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; @@ -349,6 +355,9 @@ private: (ParamInt) _param_com_rc_override, (ParamInt) _param_flight_uuid, (ParamInt) _param_takeoff_finished_action, - (ParamFloat) _param_com_cpu_max + (ParamFloat) _param_com_cpu_max, + (ParamInt) _param_com_srv_fail_ipt, + (ParamInt) _param_com_srv_fail_nr, + (ParamBool) _param_com_srv_fail_cl ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8f2c3d51ce..950bf281a6 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1044,3 +1044,31 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); * @increment 1 */ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3); + +/** + * Manual control source to inject servo failure + * + * @group Commander + * @value 0 Disabled + * @value 1 yaw stick + * @value 2 aux1 + */ +PARAM_DEFINE_INT32(COM_SRV_FAIL_IPT, 0); + +/** + * Index of the servos to fail + * + * @group Commander + * @value 0 Lock servo 0 + * @value 1 Lock servo 1 + * @value 2 Lock servo 1 and 2 + */ +PARAM_DEFINE_INT32(COM_SRV_FAIL_NR, 0); + +/** + * Flag if closed loop servo detection and allocation shall be used. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_SRV_FAIL_CL, 0); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 131670a230..5c9f197f4b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -644,6 +644,22 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso } } + // handle servo failure injection + if (_handled_servo_center_mask) { + + if (_handled_servo_center_mask & 1) { + // set the first servo to 0 by setting the min/max to 0 (center) + minimum[1](0) = 0.f; + maximum[1](0) = 0.f; + } + + if (_handled_servo_center_mask & 2) { + // set the second servo to 0 by setting the min/max to 0 (center) + minimum[1](1) = 0.f; + maximum[1](1) = 0.f; + } + } + for (int i = 0; i < _num_control_allocation; ++i) { _control_allocation[i]->setActuatorMin(minimum[i]); _control_allocation[i]->setActuatorMax(maximum[i]); @@ -818,6 +834,25 @@ ControlAllocator::check_for_actuator_activation_update() const bool status_updated = _failure_detector_status_sub.update(&failure_detector_status); + // hack: set the servos in the failed bitmask to 0 + if (status_updated) { + if (failure_detector_status.servo_to_center_mask) { + if (!_handled_servo_center_mask) { + PX4_WARN("Servo(s) to center failure injected"); + _handled_servo_center_mask = failure_detector_status.servo_to_center_mask; + activation_updated = true; + + } + + } else { + if (_handled_servo_center_mask) { + PX4_INFO("Restoring servo(s)"); + _handled_servo_center_mask = 0; + activation_updated = true; + } + } + } + if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE && status_updated) { if (failure_detector_status.fd_motor) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 4609f9943f..b03cc142c1 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -201,6 +201,7 @@ private: uint16_t _handled_servo_failure_bitmask{0}; uint16_t _handled_motor_disabled_bitmask{0}; + uint16_t _handled_servo_center_mask{0}; perf_counter_t _loop_perf; /**< loop duration performance counter */