diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 1a038528fd..2f80c8e2cb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1358,18 +1358,21 @@ PX4FMU::cycle() if (newBytes > 0) { // parse new data - uint8_t st24_rssi, rx_count; + uint8_t st24_rssi, lost_count; rc_updated = false; for (unsigned i = 0; i < (unsigned)newBytes; i++) { /* set updated flag if one complete packet was parsed */ st24_rssi = RC_INPUT_RSSI_MAX; - rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count, + rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); } - if (rc_updated) { + // The st24 will keep outputting RC channels and RSSI even if RC has been lost. + // The only way to detect RC loss is therefore to look at the lost_count. + + if (rc_updated && lost_count == 0) { // we have a new ST24 frame. Publish it. _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp, diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index ca27ed012b..37d59db9ac 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -103,10 +103,9 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, +int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) { - int ret = 1; switch (_decode_state) { @@ -175,8 +174,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case ST24_PACKET_TYPE_CHANNELDATA12: { ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; - *rssi = d->rssi; - *rx_count = d->packet_count; + // Scale from 0..255 to 100%. + *rssi = d->rssi * (100.0f / 255.0f); + *lost_count = d->lost_count; /* this can lead to rounding of the strides */ *channel_count = (max_chan_count < 12) ? max_chan_count : 12; @@ -203,8 +203,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case ST24_PACKET_TYPE_CHANNELDATA24: { ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; - *rssi = d->rssi; - *rx_count = d->packet_count; + // Scale from 0..255 to 100%. + *rssi = d->rssi * (100.0f / 255.0f); + *lost_count = d->lost_count; /* this can lead to rounding of the strides */ *channel_count = (max_chan_count < 24) ? max_chan_count : 24; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 0cf732824b..5328bb8cce 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -73,7 +73,7 @@ typedef struct { typedef struct { uint16_t t; ///< packet counter or clock uint8_t rssi; ///< signal strength - uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t lost_count; ///< Number of UART packets sent since reception of last RF frame (100 frame means RC timeout of 1s) uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) } ChannelData12; @@ -84,7 +84,7 @@ typedef struct { typedef struct { uint16_t t; ///< packet counter or clock uint8_t rssi; ///< signal strength - uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t lost_count; ///< Number of UART packets sent since reception of last RF frame (100 frame means RC timeout of 1s) uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) } ChannelData24; @@ -152,12 +152,12 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * * @param byte current char to read * @param rssi pointer to a byte where the RSSI value is written back to - * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to + * @param lost_count pointer to a byte where the receive count of packets since last wireless frame is written back to ( > 0 if RC is lost) * @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 * @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 st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, +__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); __END_DECLS diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 91476c95f6..4c14b6094f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -96,7 +96,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool perf_end(c_gather_dsm); /* get data from FD and attempt to parse with DSM and ST24 libs */ - uint8_t st24_rssi, rx_count; + uint8_t st24_rssi, lost_count; uint16_t st24_channel_count = 0; *st24_updated = false; @@ -104,11 +104,11 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ st24_rssi = RC_INPUT_RSSI_MAX; - *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, + *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } - if (*st24_updated) { + if (*st24_updated && lost_count == 0) { /* ensure ADC RSSI is disabled */ r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);