From 027919302d9537e4787f1384690a019eb937e458 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 10:40:42 +0200 Subject: [PATCH 1/4] IO RSSI handling: Fix RSSI for all protocols. --- src/modules/px4iofirmware/controls.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e04ffc94003e..4ef27718cf3c 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,8 +190,8 @@ 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 / 13) * 0.01f); } } #endif @@ -215,17 +218,23 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - rssi = 255; + unsigned sbus_rssi = 254; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - rssi = 100; + sbus_rssi = 100; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + rssi = sbus_rssi; + } + if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + /* set RSSI to 0 if the decoder senses complete drop, independent of the ADC value */ rssi = 0; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); From 4440c6383c3ad9030db7de4b828a7c4a1627c100 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 10:48:40 +0200 Subject: [PATCH 2/4] IO RSSI handling: Make 0-RSSI value consistent for all input sources --- src/modules/px4iofirmware/controls.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 4ef27718cf3c..c9702c2ec6e5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -196,6 +196,11 @@ controls_tick() { } #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); @@ -227,19 +232,17 @@ controls_tick() { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } - /* set RSSI to an emulated value if ADC RSSI is off */ - if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { - rssi = sbus_rssi; - } - if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - /* set RSSI to 0 if the decoder senses complete drop, independent of the ADC value */ - 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); From d544ac09550814550e4fc07a8197f8fffb44d98d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 17:45:29 +0200 Subject: [PATCH 3/4] Sumd: Better magic number for RSSI --- src/lib/rc/sumd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index a98c986bb7cc..cea7790ec188 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -269,7 +269,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - *rssi = 255; + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { From 09ae879b8262f5ae9b0cd23f48854d4730172ae2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 17:46:21 +0200 Subject: [PATCH 4/4] RC input: Replace magic numbers with better numbers, cap output to 0-100 --- src/drivers/drv_rc_input.h | 2 +- src/modules/px4iofirmware/controls.c | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index d44728a7127e..a24d8814feaa 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -65,7 +65,7 @@ /** * Maximum RSSI value */ -#define RC_INPUT_RSSI_MAX 255 +#define RC_INPUT_RSSI_MAX 100 /** * @addtogroup topics diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index c9702c2ec6e5..ac004f21272d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -191,7 +191,10 @@ controls_tick() { unsigned mV = counts * 3300 / 4096; /* scale to 0..253 and lowpass */ - rssi = (rssi * 0.99f) + ((mV / 13) * 0.01f); + rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (rssi > RC_INPUT_RSSI_MAX) { + rssi = RC_INPUT_RSSI_MAX; + } } } #endif @@ -223,11 +226,11 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - unsigned sbus_rssi = 254; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - sbus_rssi = 100; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); }