px4io firmware: Actually accept failsafe values, check their range. Add an error return code for commands that were rejected on reboot and force safety instead of failing silently with an OK value

1 files changed, 15 insertions, 2 deletions

diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index fd7c6081f..a43bb65a4 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -160,6 +160,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#else
+	/* this is unused, but we will pad it to be safe */
+	[PX4IO_P_SETUP_RELAYS]			= 0,
 #endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
@@ -562,12 +565,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
 			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 				// don't allow reboot while armed
-				break;
+				return -1;
 			}
 
 			// check the magic value
 			if (value != PX4IO_REBOOT_BL_MAGIC)
-				break;
+				return -1;
 
                         // we schedule a reboot rather than rebooting
                         // immediately to allow the IO board to ACK
@@ -582,6 +585,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
 			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			} else {
+				return -1;
+			}
+			break;
+
+		case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
+			if (value > 650 && value < 2350) {
+				r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
+			} else {
+				return -1;
 			}
 			break;