diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 53d35c231b..b0d68380d9 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/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; } } diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 8155d6eea1..9569bc9a73 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/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++) { diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index 1d99f1d433..0c829cee2c 100644 --- a/src/lib/rc/sumd.c +++ b/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; diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index 03d7364186..a9ac05ccee 100644 --- a/src/lib/rc/sumd.h +++ b/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 diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 4c14b6094f..72ea8445ec 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/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);