Merge pull request #1399 from PX4/safety

Enable IO safety parameter to disable safety on boot

src/drivers/px4io/px4io.cpp

@@ -817,6 +817,11 @@ PX4IO::init()
 
 	}
 
+	/* set safety to off if circuit breaker enabled */
+	if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
+		(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
+	}
+
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
 

src/modules/commander/state_machine_helper.cpp

@@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		}
 
 		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
-			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
 			// XXX do not make this fatal yet
 		}
 	}

src/modules/px4iofirmware/mixer.cpp

@@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0;
 int
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while safety off */
+	/* do not allow a mixer change while safety off and FMU armed */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		return 1;
 	}
 
-	/* abort if we're in the mixer */
+	/* disable mixing, will be enabled once load is complete */
+	r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
+
+	/* abort if we're in the mixer - the caller is expected to retry */
 	if (in_mixer) {
 		return 1;
 	}
@@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length)
 
 	isr_debug(2, "mix txt %u", length);
 
-	if (length < sizeof(px4io_mixdata))
+	if (length < sizeof(px4io_mixdata)) {
 		return 0;
+	}
 
-	unsigned	text_length = length - sizeof(px4io_mixdata);
+	unsigned text_length = length - sizeof(px4io_mixdata);
 
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
 		isr_debug(2, "reset");
 
-		/* FIRST mark the mixer as invalid */
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 		/* THEN actually delete it */
 		mixer_group.reset();
 		mixer_text_length = 0;
@@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length)
 	case F2I_MIXER_ACTION_APPEND:
 		isr_debug(2, "append %d", length);
 
-		/* disable mixing during the update */
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;

src/modules/px4iofirmware/registers.c

@@ -407,10 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+		/* do not change the mixer if FMU is armed and IO's safety is off
-				    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+		 * this state defines an active system. This check is done in the
-			return mixer_handle_text(values, num_values * sizeof(*values));
+		 * text handling function.
-		}
+		 */
+		return mixer_handle_text(values, num_values * sizeof(*values));
 		break;
 
 	default:
@@ -583,8 +584,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_REBOOT_BL:
-			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+			// do not reboot if FMU is armed and IO's safety is off
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			// this state defines an active system.
+			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
+			    (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 				// don't allow reboot while armed
 				break;
 			}
@@ -631,9 +634,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		/**
 		 * do not allow a RC config change while outputs armed
+		 * = FMU is armed and IO's safety is off
+		 * this state defines an active system.
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			    (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 			break;
 		}