From ff3a014971f83f15f4884e584a2f58ee979f23ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 7 Nov 2012 02:47:01 -0800 Subject: Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs. --- apps/px4io/mixer.c | 199 ++++------------------------------------------------- 1 file changed, 14 insertions(+), 185 deletions(-) (limited to 'apps/px4io/mixer.c') diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 471965fd7..b614f3fa4 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Control channel input/output mixer and failsafe. + * @file mixer.c + * + * Control channel input/output mixer and failsafe. */ #include @@ -43,8 +45,6 @@ #include #include #include -#include -#include #include #include @@ -55,23 +55,12 @@ #include "px4io.h" -/* - * Count of periodic calls in which we have no data. - */ -static unsigned mixer_input_drops; -#define MIXER_INPUT_DROP_LIMIT 10 - /* * Count of periodic calls in which we have no FMU input. */ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Serial port fd for serial RX protocols - */ -static int rx_port = -1; - /* * HRT periodic call used to check for control input data. */ @@ -106,8 +95,7 @@ struct mixer { int mixer_init(void) { - /* open the serial port */ - rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); @@ -115,37 +103,6 @@ mixer_init(void) return 0; } -void -mixer_set_serial_mode(uint8_t serial_mode) -{ - - if (serial_mode == system_state.serial_rx_mode) - return; - - struct termios t; - tcgetattr(rx_port, &t); - - switch (serial_mode) { - case RX_MODE_PPM_ONLY: - break; - case RX_MODE_SPEKTRUM_6: - case RX_MODE_SPEKTRUM_7: - /* 115200, no parity, one stop bit */ - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - break; - case RX_MODE_FUTABA_SBUS: - /* 100000, even parity, two stop bits */ - cfsetspeed(&t, 100000); - t.c_cflag |= (CSTOPB | PARENB); - break; - default: - return; - } - - tcsetattr(rx_port, TCSANOW, &t); - system_state.serial_rx_mode = serial_mode; -} static void mixer_tick(void *arg) @@ -234,148 +191,20 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) } } -static bool -mixer_get_spektrum_input(void) -{ - static uint8_t buf[16]; - static unsigned count; - - /* always read as much data as we can into the buffer */ - if (count >= sizeof(buf)) - count = 0; - ssize_t result = read(rx_port, buf, sizeof(buf) - count); - /* no data or an error */ - if (result <= 0) - return false; - count += result; - - /* if there are more than two bytes in the buffer, check for sync */ - if (count >= 2) { - if ((buf[0] != 0x3) || (buf[1] != 0x1)) { - /* not in sync; look for a possible sync marker */ - for (unsigned i = 1; i < count; i++) { - if (buf[i] == 0x3) { - /* could be a frame marker; move buffer bytes */ - count -= i; - memmove(buf, buf + i, count); - break; - } - } - } - } - if (count < sizeof(buf)) - return false; - - /* we got a frame; decode it */ - const uint16_t *channels = (const uint16_t *)&buf[2]; - - /* - * Channel assignment for DX6i vs. DX7 is different. - * - * DX7 etc. is: - * - * 0: Aileron - * 1: Flaps - * 2: Gear - * 3: Elevator - * 4: Aux2 - * 5: Throttle - * 6: Rudder - * - * DX6i is: - * - * 0: Aileron - * 1: Flaps - * 2: Elevator - * 3: Rudder - * 4: Throttle - * 5: Gear - * 6: - * - * We convert these to our standard Futaba-style assignment: - * - * 0: Throttle (Throttle) - * 1: Roll (Aileron) - * 2: Pitch (Elevator) - * 3: Yaw (Rudder) - * 4: Override (Flaps) - * 5: FUNC_0 (Gear) - * 6: FUNC_1 (Aux2) - */ - if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) { - system_state.rc_channel_data[0] = channels[5]; /* Throttle */ - system_state.rc_channel_data[1] = channels[0]; /* Roll */ - system_state.rc_channel_data[2] = channels[3]; /* Pitch */ - system_state.rc_channel_data[3] = channels[6]; /* Yaw */ - system_state.rc_channel_data[4] = channels[1]; /* Override */ - system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */ - system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */ - system_state.rc_channels = 7; - } else { - system_state.rc_channel_data[0] = channels[4]; /* Throttle */ - system_state.rc_channel_data[1] = channels[0]; /* Roll */ - system_state.rc_channel_data[2] = channels[2]; /* Pitch */ - system_state.rc_channel_data[3] = channels[3]; /* Yaw */ - system_state.rc_channel_data[4] = channels[1]; /* Override */ - system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */ - system_state.rc_channels = 6; - } - count = 0; - return true; -} - -static bool -mixer_get_sbus_input(void) -{ - /* XXX not implemented yet */ - return false; -} - static void mixer_get_rc_input(void) { - bool got_input = false; - - switch (system_state.serial_rx_mode) { - case RX_MODE_PPM_ONLY: - if (ppm_decoded_channels > 0) { - /* copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - got_input = true; - } - break; - - case RX_MODE_SPEKTRUM_6: - case RX_MODE_SPEKTRUM_7: - got_input = mixer_get_spektrum_input(); - break; - - case RX_MODE_FUTABA_SBUS: - got_input = mixer_get_sbus_input(); - break; - - default: - break; - } - if (got_input) { - mixer_input_drops = 0; + /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ + if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { + system_state.rc_channels = 0; system_state.fmu_report_due = true; - } else { - /* - * No data; count the 'frame drops' and once we hit the limit - * assume that we have lost input. - */ - if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) { - mixer_input_drops++; - - /* if we hit the limit, stop pretending we have input and let the FMU know */ - if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; - } - } + return; } + + /* otherwise, copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + system_state.fmu_report_due = true; } -- cgit v1.2.3