Safelink protocol support in IO firmware structures, not hooked up yet.

3 files changed, 24 insertions, 0 deletions

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..0006b137c 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -111,6 +111,7 @@
 #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_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_SAFELINK_FS	(1 << 13) /* SAFELINK failsave is active */
 
 #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 */
@@ -161,6 +162,10 @@
 #define PX4IO_PAGE_PWM_INFO		7
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
+#define PX4IO_PAGE_SAFELINK			8	/**< redundancy link page */
+#define PX4IO_P_SAFELINK_COUNT			0	/**< number of inputs */
+#define PX4IO_P_SAFELINK_BASE			1	/**< index of safelink actuator zero */
+
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
@@ -168,6 +173,8 @@
 #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
 #define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
 #define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_SAFELINK_IN	(1 << 4) /* enable SAFELINK input parsing on S.BUS port */
+#define PX4IO_P_SETUP_FEATURES_SAFELINK_OUT	(1 << 5) /* enable SAFELINK output on S.BUS port */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 342db4b66..0a159f7f5 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -77,6 +77,7 @@ extern uint16_t			r_page_servos[];	/* PX4IO_PAGE_SERVOS */
 extern uint16_t			r_page_raw_rc_input[];	/* PX4IO_PAGE_RAW_RC_INPUT */
 extern uint16_t			r_page_rc_input[];	/* PX4IO_PAGE_RC_INPUT */
 extern uint16_t			r_page_adc[];		/* PX4IO_PAGE_RAW_ADC_INPUT */
+extern uint16_t			r_page_safelink[];	/* PX4IO_PAGE_SAFELINK */
 
 extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
@@ -110,6 +111,8 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #endif
 
 #define r_control_values	(&r_page_controls[0])
+#define r_safelink_values	(&r_page_safelink[PX4IO_P_SAFELINK_BASE])
+#define r_safelink_count	r_page_safelink[PX4IO_P_SAFELINK_COUNT]
 
 /*
  * System state structure.
@@ -222,6 +225,10 @@ extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsa
 extern void	sbus1_output(uint16_t *values, uint16_t num_values);
 extern void	sbus2_output(uint16_t *values, uint16_t num_values);
 
+extern int	safelink_init(int fd);
+extern void	safelink_output(uint16_t *values, uint16_t num_values);
+extern bool	safelink_input(uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels);
+
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 1335f52e1..6c183bbf9 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -142,6 +142,16 @@ uint16_t		r_page_rc_input[] = {
 uint16_t		r_page_scratch[32];
 
 /**
+ * PAGE 8
+ *
+ * Inputs from redundant autopilot
+ */
+uint16_t		r_page_safelink[] = {
+	[PX4IO_P_SAFELINK_COUNT]		= 0,
+	[PX4IO_P_SAFELINK_BASE ... (PX4IO_P_SAFELINK_BASE + PX4IO_SERVO_COUNT)] = 0
+};
+
+/**
  * PAGE 100
  *
  * Setup registers