Feature/First iteration on MC failsafe

.gitignore

@@ -77,6 +77,7 @@ vectorcontrol/
 
 # CLion ignores
 .idea
+cmake-build-*/
 
 # gcov code coverage
 coverage-html/

msg/actuator_armed.msg

@@ -3,5 +3,6 @@ bool armed		# Set to true if system is armed
 bool prearmed		# Set to true if the actuator safety is disabled but motors are not armed
 bool ready_to_arm	# Set to true if system is ready to be armed
 bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
+bool manual_lockdown    # Set to true if manual throttle kill switch is engaged
 bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
 bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics

src/drivers/ardrone_interface/ardrone_interface.c

@@ -333,7 +333,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */
-			if (armed.armed && !armed.lockdown) {
+			if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
 				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
 
 			} else {

src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp

@@ -373,7 +373,7 @@ void task_main(int argc, char *argv[])
 				       pwm,
 				       &_pwm_limit);
 
-			if (_armed.lockdown) {
+			if (_armed.lockdown || _armed.manual_lockdown) {
 				send_outputs_pwm(disarmed_pwm);
 
 			} else {

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -88,6 +88,8 @@ class PWMSim : public device::CDev
 class PWMSim : public device::VDev
 #endif
 {
+	const uint32_t PWM_SIM_DISARMED_MAGIC = 900;
+	const uint32_t PWM_SIM_FAILSAFE_MAGIC = 600;
 public:
 	enum Mode {
 		MODE_2PWM,
@@ -128,6 +130,8 @@ class PWMSim : public device::VDev
 
 	volatile bool	_task_should_exit;
 	static bool	_armed;
+	static bool	_lockdown;
+	static bool	_failsafe;
 
 	MixerGroup	*_mixers;
 
@@ -170,6 +174,8 @@ PWMSim	*g_pwm_sim;
 } // namespace
 
 bool PWMSim::_armed = false;
+bool PWMSim::_lockdown = false;
+bool PWMSim::_failsafe = false;
 
 PWMSim::PWMSim() :
 #ifdef __PX4_NUTTX
@@ -508,10 +514,23 @@ PWMSim::task_main()
 					 * This will be clearly visible on the servo status and will limit the risk of accidentally
 					 * spinning motors. It would be deadly in flight.
 					 */
-					outputs.output[i] = 900;
+					outputs.output[i] = PWM_SIM_DISARMED_MAGIC;
 				}
 			}
 
+			/* overwrite outputs in case of force_failsafe */
+			if (_failsafe) {
+				for (size_t i = 0; i < num_outputs; i++) {
+					outputs.output[i] = PWM_SIM_FAILSAFE_MAGIC;
+				}
+			}
+
+			/* overwrite outputs in case of lockdown */
+			if (_lockdown) {
+				for (size_t i = 0; i < num_outputs; i++) {
+					outputs.output[i] = 0.0;
+				}
+			}
 
 			/* and publish for anyone that cares to see */
 			orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
@@ -526,6 +545,8 @@ PWMSim::task_main()
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
 			/* do not obey the lockdown value, as lockdown is for PWMSim */
 			_armed = aa.armed;
+			_failsafe = aa.force_failsafe;
+			_lockdown = aa.lockdown || aa.manual_lockdown;
 		}
 	}
 

src/drivers/px4fmu/fmu.cpp

@@ -1192,8 +1192,15 @@ PX4FMU::cycle()
 				       _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
 
 
+			/* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */
+			if (_armed.force_failsafe) {
+				for (size_t i = 0; i < num_outputs; i++) {
+					pwm_limited[i] = _failsafe_pwm[i];
+				}
+			}
+
 			/* overwrite outputs in case of lockdown with disarmed PWM values */
-			if (_armed.lockdown) {
+			if (_armed.lockdown || _armed.manual_lockdown) {
 				for (size_t i = 0; i < num_outputs; i++) {
 					pwm_limited[i] = _disarmed_pwm[i];
 				}

src/drivers/px4io/px4io.cpp

@@ -1454,11 +1454,11 @@ PX4IO::io_set_arming_state()
 
 		_armed = armed.armed;
 
-		if (armed.lockdown && !_lockdown_override) {
+		if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) {
 			set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
 			_lockdown_override = true;
 
-		} else if (!armed.lockdown && _lockdown_override) {
+		} else if (!(armed.lockdown || armed.manual_lockdown) && _lockdown_override) {
 			clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
 			_lockdown_override = false;
 		}

src/modules/commander/commander.cpp

@@ -1665,8 +1665,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	transition_result_t arming_ret;
 
-	int32_t datalink_loss_enabled = 0;
-	int32_t rc_loss_enabled = 0;
+	int32_t datalink_loss_act = 0;
+	int32_t rc_loss_act = 0;
 	int32_t datalink_loss_timeout = 10;
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
@@ -1753,8 +1753,8 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			/* Safety parameters */
-			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
-			param_get(_param_enable_rc_loss, &rc_loss_enabled);
+			param_get(_param_enable_datalink_loss, &datalink_loss_act);
+			param_get(_param_enable_rc_loss, &rc_loss_act);
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
 			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
 			param_get(_param_rc_in_off, &rc_in_off);
@@ -2685,15 +2685,15 @@ int commander_thread_main(int argc, char *argv[])
 			/* check throttle kill switch */
 			if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
 				/* set lockdown flag */
-				if (!armed.lockdown) {
+				if (!armed.manual_lockdown) {
 					mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH ENGAGED");
 				}
-				armed.lockdown = true;
+				armed.manual_lockdown = true;
 			} else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-				if (armed.lockdown) {
+				if (armed.manual_lockdown) {
 					mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH OFF");
 				}
-				armed.lockdown = false;
+				armed.manual_lockdown = false;
 			}
 			/* no else case: do not change lockdown flag in unconfigured case */
 
@@ -2909,18 +2909,20 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(&status,
-						       &internal_state,
-						       &mavlink_log_pub,
-						       (datalink_loss_enabled > 0),
-						       _mission_result.finished,
-						       _mission_result.stay_in_failsafe,
-						       &status_flags,
-						       land_detector.landed,
-						       (rc_loss_enabled > 0),
-						       offboard_loss_act,
-						       offboard_loss_rc_act);
-
-		if (status.failsafe != failsafe_old) {
+											   &armed,
+											   &internal_state,
+											   &mavlink_log_pub,
+											   (link_loss_actions_t)datalink_loss_act,
+											   _mission_result.finished,
+											   _mission_result.stay_in_failsafe,
+											   &status_flags,
+											   land_detector.landed,
+											   (link_loss_actions_t)rc_loss_act,
+											   offboard_loss_act,
+											   offboard_loss_rc_act);
+
+		if (status.failsafe != failsafe_old)
+		{
 			status_changed = true;
 
 			if (status.failsafe) {
@@ -2980,9 +2982,11 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
 			   (battery.warning == battery_status_s::BATTERY_WARNING_LOW)) {
-			/* play tune on battery warning or failsafe */
+			/* play tune on battery warning */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
+		} else if (status.failsafe) {
+			tune_failsafe(true);
 		} else {
 			set_tune(TONE_STOP_TUNE);
 		}
@@ -3162,19 +3166,30 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
 	if (actuator_armed->armed) {
-		/* armed, solid */
-		led_on(LED_BLUE);
+		if (status.failsafe) {
+			led_off(LED_BLUE);
+			if (leds_counter % 5 == 0) {
+				led_toggle(LED_GREEN);
+			}
+		} else {
+			led_off(LED_GREEN);
+
+			/* armed, solid */
+			led_on(LED_BLUE);
+		}
 
 	} else if (actuator_armed->ready_to_arm) {
+		led_off(LED_BLUE);
 		/* ready to arm, blink at 1Hz */
 		if (leds_counter % 20 == 0) {
-			led_toggle(LED_BLUE);
+			led_toggle(LED_GREEN);
 		}
 
 	} else {
+		led_off(LED_BLUE);
 		/* not ready to arm, blink at 10Hz */
 		if (leds_counter % 2 == 0) {
-			led_toggle(LED_BLUE);
+			led_toggle(LED_GREEN);
 		}
 	}
 

src/modules/commander/commander_helper.cpp

@@ -243,6 +243,17 @@ void tune_negative(bool use_buzzer)
 	}
 }
 
+void tune_failsafe(bool use_buzzer)
+{
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_PURPLE);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
+	if (use_buzzer) {
+		set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
+	}
+}
+
 int blink_msg_state()
 {
 	if (blink_msg_end == 0) {

src/modules/commander/commander_helper.h

@@ -64,6 +64,7 @@ void tune_mission_fail(bool use_buzzer);
 void tune_positive(bool use_buzzer);
 void tune_neutral(bool use_buzzer);
 void tune_negative(bool use_buzzer);
+void tune_failsafe(bool use_buzzer);
 
 int blink_msg_state();
 

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -477,9 +477,9 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 
 bool StateMachineHelperTest::isSafeTest(void)
 {
-	struct vehicle_status_s current_state;
-	struct safety_s safety;
-	struct actuator_armed_s armed;
+	struct vehicle_status_s current_state = {};
+	struct safety_s safety = {};
+	struct actuator_armed_s armed = {};
 
 	armed.armed = false;
 	armed.lockdown = false;