diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-04-21 19:39:08 +0200 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-04-21 19:39:08 +0200 |
commit | 39f6e13c18414ae92e898640c62574d94171ae2d (patch) | |
tree | 8a60bbf45834a933065091adab1bae4fdada25d9 /src/modules | |
parent | a0ad5ec42b659fbf083a489c368f9a90c29444a6 (diff) | |
parent | 09ae879b8262f5ae9b0cd23f48854d4730172ae2 (diff) | |
download | px4-firmware-39f6e13c18414ae92e898640c62574d94171ae2d.tar.gz px4-firmware-39f6e13c18414ae92e898640c62574d94171ae2d.tar.bz2 px4-firmware-39f6e13c18414ae92e898640c62574d94171ae2d.zip |
Merge pull request #2061 from PX4/rssi_cleanup
IO RSSI handling: Fix RSSI for all protocols.
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/controls.c | 29 |
1 files changed, 22 insertions, 7 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e04ffc940..ac004f212 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool if (*st24_updated) { + /* ensure ADC RSSI is disabled */ + r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); + *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -116,14 +119,14 @@ 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; + 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)); } if (*sumd_updated) { - *rssi = sumd_rssi; + /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; @@ -187,12 +190,20 @@ controls_tick() { /* use 1:1 scaling on 3.3V ADC input */ unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 */ - rssi = mV / 13; + /* scale to 0..253 and lowpass */ + rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (rssi > RC_INPUT_RSSI_MAX) { + rssi = RC_INPUT_RSSI_MAX; + } } } #endif + /* zero RSSI if signal is lost */ + if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) { + rssi = 0; + } + perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); @@ -215,22 +226,26 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - rssi = 255; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - rssi = 100; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - rssi = 0; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + rssi = sbus_rssi; + } + } perf_end(c_gather_sbus); |