From 72156d9cd6edf467887ac6d1c33c7208031f1f05 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 10 Jan 2016 09:28:46 -0700 Subject: [PATCH] add macro to control RC input inverter remove redundant variable --- src/drivers/boards/px4fmu-v4/board_config.h | 2 + src/drivers/px4fmu/fmu.cpp | 47 ++++++++++----------- 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 0655868c57..03afcda0b4 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -228,8 +228,10 @@ __BEGIN_DECLS #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) /* for R07, this signal is active low */ #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, 1-_s); /* for R12, this signal is active high */ //#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +//#define INVERT_RC_INPUT(_s) stm32_gpiowrite(GPIO_SBUS_INV, _s); #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a20814966..2a7978f347 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -209,8 +209,8 @@ private: PX4FMU operator=(const PX4FMU &); void fill_rc_in(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool sbus_frame_drop, bool sbus_failsafe, - unsigned sbus_frame_drops); + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -626,8 +626,8 @@ PX4FMU::cycle_trampoline(void *arg) void PX4FMU::fill_rc_in(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool sbus_frame_drop, bool sbus_failsafe, - unsigned sbus_frame_drops) + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi = -1) { // fill rc_in struct for publishing _rc_in.channel_count = raw_rc_count; @@ -643,11 +643,15 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count, _rc_in.timestamp_publication = now; _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; _rc_in.rc_ppm_frame_length = 0; - _rc_in.rssi = - (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); - _rc_in.rc_failsafe = sbus_failsafe; + + if (rssi == -1) { + _rc_in.rssi = + (!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2); + } + + _rc_in.rc_failsafe = failsafe; _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = sbus_frame_drops; + _rc_in.rc_lost_frame_count = frame_drops; _rc_in.rc_total_frame_count = 0; } @@ -860,8 +864,7 @@ PX4FMU::cycle() bool sbus_failsafe, sbus_frame_drop; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; uint16_t raw_rc_count; - unsigned sbus_frame_drops; - unsigned dsm_frame_drops; + unsigned frame_drops; bool dsm_11_bit; @@ -878,11 +881,9 @@ PX4FMU::cycle() case RC_SCAN_SBUS: if (rc_scan_begin == 0) { rc_scan_begin = now; -// // Configure serial port for SBUS + // Configure serial port for SBUS sbus_config(_rcs_fd, false); -// /* for PixRacer R07, this signal is active low */ -// /* for PixRacer R12, this signal is active high */ - stm32_gpiowrite(GPIO_SBUS_INV, 0); + INVERT_RC_INPUT(true); } else if (now - rc_scan_last_lock < rc_scan_max || now - rc_scan_begin < rc_scan_max) { @@ -890,12 +891,12 @@ PX4FMU::cycle() // parse new data if (newBytes > 0) { rc_updated = sbus_parse(now, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, - &sbus_frame_drop, &sbus_frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + &sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new SBUS frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, - sbus_frame_drop, sbus_failsafe, sbus_frame_drops); + sbus_frame_drop, sbus_failsafe, frame_drops); rc_scan_last_lock = now; rc_scan_locked = true; } @@ -915,9 +916,7 @@ PX4FMU::cycle() rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); -// /* for PixRacer R07, this signal is active low */ -// /* for PixRacer R12, this signal is active high */ - stm32_gpiowrite(GPIO_SBUS_INV, 1); + INVERT_RC_INPUT(false); } else if (now - rc_scan_last_lock < rc_scan_max || now - rc_scan_begin < rc_scan_max) { @@ -925,12 +924,12 @@ PX4FMU::cycle() if (newBytes > 0) { // parse new data rc_updated = dsm_parse(now, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &dsm_frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new DSM frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, - false, false, dsm_frame_drops); + false, false, frame_drops); rc_scan_last_lock = now; rc_scan_locked = true; } @@ -950,9 +949,7 @@ PX4FMU::cycle() rc_scan_begin = now; // // Configure serial port for DSM dsm_config(_rcs_fd); -// /* for PixRacer R07, this signal is active low */ -// /* for PixRacer R12, this signal is active high */ - stm32_gpiowrite(GPIO_SBUS_INV, 1); + INVERT_RC_INPUT(false); } else if (now - rc_scan_last_lock < rc_scan_max || now - rc_scan_begin < rc_scan_max) { @@ -973,7 +970,7 @@ PX4FMU::cycle() if (rc_updated) { // we have a new ST24 frame. Publish it. fill_rc_in(raw_rc_count, raw_rc_values, now, - false, false, dsm_frame_drops); + false, false, frame_drops, st24_rssi); rc_scan_last_lock = now; rc_scan_locked = true; }