CA testing hacks, external failure message, sitl tuning, disabling of rate controller saturation

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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

msg/CMakeLists.txt

@@ -93,6 +93,7 @@ set(msg_files
 	Event.msg
 	FigureEightStatus.msg
 	FailsafeFlags.msg
+	FailureDetectorExtServo.msg
 	FailureDetectorStatus.msg
 	FlightPhaseEstimation.msg
 	FollowTarget.msg

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

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

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;

src/lib/mixer_module/mixer_module.hpp

@@ -59,6 +59,7 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/manual_control_setpoint.h>
 
 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<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
 
 	actuator_armed_s _armed{};
@@ -296,6 +300,8 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,   ///< multicopter air-mode
 		(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
-		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac ///< thrust to motor control signal modelling factor
+		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
+		(ParamInt<px4::params::OUT_SRV_FAIL_IPT>) _param_out_srv_fail_ipt,
+		(ParamInt<px4::params::OUT_SRV_FAIL_NR>) _param_out_srv_fail_nr
 	)
 };

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);

src/lib/rate_control/rate_control.cpp

@@ -52,6 +52,10 @@ void RateControl::setSaturationStatus(const Vector3<bool> &saturation_positive,
 {
 	_control_allocator_saturation_positive = saturation_positive;
 	_control_allocator_saturation_negative = saturation_negative;
+
+	const Vector3<bool> 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)

src/modules/commander/Commander.cpp

@@ -74,6 +74,8 @@
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/tune_control.h>
 
+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);
 		}

src/modules/commander/Commander.hpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/failure_detector_ext_servo.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
@@ -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_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::SubscriptionData<failure_detector_ext_servo_s> 	_failure_detector_ext_servo{ORB_ID(failure_detector_ext_servo)};
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
@@ -349,6 +355,9 @@ private:
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action,
-		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
+		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max,
+		(ParamInt<px4::params::COM_SRV_FAIL_IPT>)   _param_com_srv_fail_ipt,
+		(ParamInt<px4::params::COM_SRV_FAIL_NR>)    _param_com_srv_fail_nr,
+		(ParamBool<px4::params::COM_SRV_FAIL_CL>)   _param_com_srv_fail_cl
 	)
 };

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);

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) {

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 */