diff options
Diffstat (limited to 'apps/px4io/mixer.c')
-rw-r--r-- | apps/px4io/mixer.c | 199 |
1 files changed, 177 insertions, 22 deletions
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index fab577383..471965fd7 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -40,10 +40,13 @@ #include <sys/types.h> #include <stdbool.h> - +#include <string.h> #include <assert.h> #include <errno.h> #include <fcntl.h> +#include <termios.h> +#include <unistd.h> +#include <fcntl.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> @@ -65,6 +68,11 @@ 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. */ static struct hrt_call mixer_input_call; @@ -98,12 +106,47 @@ 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); 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) { @@ -181,22 +224,145 @@ mixer_tick(void *arg) } static void -mixer_get_rc_input(void) +mixer_update(int mixer, uint16_t *inputs, int input_count) { + /* simple passthrough for now */ + if (mixer < input_count) { + mixers[mixer].current_value = inputs[mixer]; + } else { + mixers[mixer].current_value = 0; + } +} + +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]; + /* - * XXX currently only supporting PPM + * 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: * - * XXX check timestamp to ensure current + * 0: Aileron + * 1: Flaps + * 2: Elevator + * 3: Rudder + * 4: Throttle + * 5: Gear + * 6: <notused> + * + * 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 (ppm_decoded_channels > 0) { - mixer_input_drops = 0; - system_state.fmu_report_due = true; + 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; - /* 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]; + case RX_MODE_FUTABA_SBUS: + got_input = mixer_get_sbus_input(); + break; + default: + break; + } + + if (got_input) { + mixer_input_drops = 0; + system_state.fmu_report_due = true; } else { /* * No data; count the 'frame drops' and once we hit the limit @@ -213,14 +379,3 @@ mixer_get_rc_input(void) } } } - -static void -mixer_update(int mixer, uint16_t *inputs, int input_count) -{ - /* simple passthrough for now */ - if (mixer < input_count) { - mixers[mixer].current_value = inputs[mixer]; - } else { - mixers[mixer].current_value = 0; - } -} |