The goal of this patch is to allow compatibility with GR12/GR16/GR24 graupner receiver by :

- allowing decoding of SUMD frame with failsafe bit set
- updating stack failsafe state with the sumd failsafe info

Refer to #5817 issue

src/drivers/px4fmu/fmu.cpp

@@ -1415,6 +1415,7 @@ PX4FMU::cycle()
 			if (newBytes > 0) {
 				// parse new data
 				uint8_t sumd_rssi, rx_count;
+				bool sumd_failsafe;
 
 				rc_updated = false;
 
@@ -1422,14 +1423,14 @@ PX4FMU::cycle()
 					/* set updated flag if one complete packet was parsed */
 					sumd_rssi = RC_INPUT_RSSI_MAX;
 					rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
-									&raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+									&raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
 				}
 
 				if (rc_updated) {
 					// we have a new SUMD frame. Publish it.
 					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
 					fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp,
-						   false, false, frame_drops, sumd_rssi);
+						   false, sumd_failsafe, frame_drops, sumd_rssi);
 					_rc_scan_locked = true;
 				}
 			}

src/lib/rc/rc_tests/RCTest.cpp

@@ -302,9 +302,10 @@ bool RCTest::sumdTest(void)
 		uint8_t rx_count;
 		uint16_t channel_count;
 		uint16_t channels[32];
+		bool sumd_failsafe;
 
 
-		if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
+		if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32, &sumd_failsafe)) {
 			//PX4_INFO("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
 
 			for (unsigned i = 0; i < channel_count; i++) {

src/lib/rc/sumd.c

@@ -110,7 +110,7 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value)
 }
 
 int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
-		uint16_t max_chan_count)
+		uint16_t max_chan_count, bool *failsafe)
 {
 
 	int ret = 1;
@@ -143,7 +143,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe
 		break;
 
 	case SUMD_DECODE_STATE_GOT_HEADER:
-		if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) {
+		if (byte == SUMD_ID_SUMD || byte == SUMD_ID_FAILSAFE || byte == SUMD_ID_SUMH) {
 			_rxpacket.status = byte;
 
 			if (byte == SUMD_ID_SUMH) {
@@ -311,6 +311,9 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe
 
 			*rssi = 100;
 
+			/* failsafe flag */
+			*failsafe = (_rxpacket.status == SUMD_ID_FAILSAFE);
+
 			/* received Channels */
 			if ((uint16_t)_rxpacket.length > max_chan_count) {
 				_rxpacket.length = (uint8_t) max_chan_count;

src/lib/rc/sumd.h

@@ -95,14 +95,15 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value);
  * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
  * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
  * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
+ * @param failsafe pointer to a boolean where the decoded failsafe flag is written back to
  * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
  */
 /*
 __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
-				 uint16_t *channels, uint16_t max_chan_count);
+				 uint16_t *channels, uint16_t max_chan_count, bool *failsafe);
 */
 __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
-			 uint16_t *channels, uint16_t max_chan_count);
+			 uint16_t *channels, uint16_t max_chan_count, bool *failsafe);
 
 
 __END_DECLS

src/modules/px4iofirmware/controls.c

@@ -125,6 +125,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	/* get data from FD and attempt to parse with SUMD libs */
 	uint8_t sumd_rssi, sumd_rx_count;
 	uint16_t sumd_channel_count = 0;
+	bool sumd_failsafe_state;
 
 	*sumd_updated = false;
 
@@ -132,7 +133,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		/* set updated flag if one complete packet was parsed */
 		sumd_rssi = RC_INPUT_RSSI_MAX;
 		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
-						    &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+						    &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
 	}
 
 	if (*sumd_updated) {
@@ -142,7 +143,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+		if (sumd_failsafe_state) {
+			r_raw_rc_flags |= (PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+		}
 	}
 
 	return (*dsm_updated | *st24_updated | *sumd_updated);