Merge remote-tracking branch 'px4/new_state_machine_drton' into fmuv2_bringup_new_state_machine_drton

Conflicts:
	src/drivers/blinkm/blinkm.cpp
	src/drivers/px4io/px4io.cpp
	src/modules/commander/state_machine_helper.c
	src/modules/px4iofirmware/protocol.h
	src/modules/px4iofirmware/registers.c
	src/modules/systemlib/systemlib.h
	src/systemcmds/reboot/reboot.c

src/modules/px4iofirmware/controls.c

@@ -300,6 +300,8 @@ controls_tick() {
 		} else {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		}
+	} else {
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 	}
 }
 

src/modules/px4iofirmware/dsm.c

@@ -129,9 +129,9 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_send_pulses:
 		for (int i = 0; i < pulses; i++) {
 			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(50);
+			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(50);
+			up_udelay(25);
 		}
 		break;
 	case dsm_bind_reinit_uart:

src/modules/px4iofirmware/mixer.cpp

@@ -59,6 +59,12 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US		1000000
+ #define ESC_RAMP_TIME_US		2000000
+
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -68,6 +74,17 @@ extern "C" {
 
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+	ESC_OFF,
+	ESC_INIT,
+	ESC_RAMP,
+	ESC_ON
+};
+static esc_state_e esc_state;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
-		 not a valid mixer. */
+		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
@@ -167,6 +183,48 @@ mixer_tick(void)
 		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
+		uint16_t ramp_promille;
+
+		/* update esc init state, but only if we are truely armed and not just PWM enabled */
+		if (mixer_servos_armed && should_arm) {
+
+			switch (esc_state) {
+
+				/* after arming, some ESCs need an initalization period, count the time from here */
+				case ESC_OFF:
+					esc_init_time = hrt_absolute_time();
+					esc_state = ESC_INIT;
+				break;
+
+				/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+				case ESC_INIT:
+					if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+						esc_state = ESC_RAMP;
+					}
+				break;
+
+				/* then ramp until the min speed is reached */
+				case ESC_RAMP:
+					if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+						esc_state = ESC_ON;
+					}
+				break;
+
+				case ESC_ON:
+				default:
+
+				break;
+			}
+		} else {
+			esc_state = ESC_OFF;
+		}
+
+		/* do the calculations during the ramp for all at once */
+		if(esc_state == ESC_RAMP) {
+			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+		}
+
+
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
@@ -176,9 +234,27 @@ mixer_tick(void)
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
+			switch (esc_state) {
+				case ESC_INIT:
+					r_page_servos[i] = (outputs[i] * 600 + 1500);
+				break;
 
+				case ESC_RAMP:
+					r_page_servos[i] = (outputs[i]
+					 * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+					 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+				break;
+
+				case ESC_ON:
+					r_page_servos[i] = (outputs[i]
+					 * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+					 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+				break;
+
+				case ESC_OFF:
+				default:
+				break;
+			}
 		}
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
@@ -193,30 +269,46 @@ mixer_tick(void)
 	 * XXX correct behaviour for failsafe may require an additional case
 	 * here.
 	 */
-	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+						     )
 	);
 
-	if (should_arm && !mixer_servos_armed) {
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+		isr_debug(5, "> PWM enabled");
 
-	} else if (!should_arm && mixer_servos_armed) {
+	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+		isr_debug(5, "> PWM disabled");
+
 	}
 
-	if (mixer_servos_armed) {
+	if (mixer_servos_armed && should_arm) {
 		/* update the servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
+
+	} else if (mixer_servos_armed && should_always_enable_pwm) {
+		/* set the idle servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servo_idle[i]);
 	}
 }
 
@@ -265,9 +357,8 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	/* do not allow a mixer change while outputs armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 		return;
 	}
 
@@ -344,6 +435,7 @@ mixer_set_failsafe()
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
 		return;

src/modules/px4iofirmware/protocol.h

@@ -94,7 +94,7 @@
 #define PX4IO_P_STATUS_CPULOAD			1
 
 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
@@ -106,7 +106,8 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -157,6 +158,7 @@
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
 #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
@@ -209,6 +211,15 @@ enum {							/* DSM bind states */
 #define PX4IO_PAGE_TEST			127
 #define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
 
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
 /**
  * As-needed mixer data upload.
  *

src/modules/px4iofirmware/px4io.h

@@ -78,6 +78,9 @@ extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 
 /*
  * Register aliases.

src/modules/px4iofirmware/registers.c

@@ -158,7 +158,9 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
+					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -196,6 +198,30 @@ uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
  */
 uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
 
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t		r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
 int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
@@ -259,6 +285,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 		break;
 
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_min[offset] = 900;
+
+			else if (*values > 1200)
+				r_page_servo_control_min[offset] = 1200;
+			else if (*values < 900)
+				r_page_servo_control_min[offset] = 900;
+			else
+				r_page_servo_control_min[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+	
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_max[offset] = 2100;
+
+			else if (*values > 2100)
+				r_page_servo_control_max[offset] = 2100;
+			else if (*values < 1800)
+				r_page_servo_control_max[offset] = 1800;
+			else
+				r_page_servo_control_max[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+	case PX4IO_PAGE_IDLE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_idle[offset] = 0;
+
+			else if (*values < 900)
+				r_page_servo_idle[offset] = 900;
+			else if (*values > 2100)
+				r_page_servo_idle[offset] = 2100;
+			else
+				r_page_servo_idle[offset] = *values;
+
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
 		mixer_handle_text(values, num_values * sizeof(*values));
@@ -331,9 +426,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
-			}
+
+			// XXX do not reset IO's safety state by FMU for now
+			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			// }
 
 			r_setup_arming = value;
 
@@ -397,9 +494,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_RC_CONFIG: {
 
-		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		/* do not allow a RC config change while outputs armed */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 			break;
 		}
 
@@ -659,6 +755,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_FAILSAFE_PWM:
 		SELECT_PAGE(r_page_servo_failsafe);
 		break;
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+		SELECT_PAGE(r_page_servo_control_min);
+		break;
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+		SELECT_PAGE(r_page_servo_control_max);
+		break;
+	case PX4IO_PAGE_IDLE_PWM:
+		SELECT_PAGE(r_page_servo_idle);
+		break;
 
 	default:
 		return -1;

src/modules/px4iofirmware/safety.c

@@ -110,7 +110,7 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* switch to armed state */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
-	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;