diff options
author | px4dev <px4@purgatory.org> | 2012-11-05 00:55:22 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-11-05 00:55:45 -0800 |
commit | 39659e57f817c4f49be595d6cecf05e67e2d89db (patch) | |
tree | e7e4e1e06c3e075a33f5788ed38d257cfde1bd45 /apps | |
parent | 87fd9fcc067d8115624170ee0f934c1f75e19633 (diff) | |
download | px4-firmware-39659e57f817c4f49be595d6cecf05e67e2d89db.tar.gz px4-firmware-39659e57f817c4f49be595d6cecf05e67e2d89db.tar.bz2 px4-firmware-39659e57f817c4f49be595d6cecf05e67e2d89db.zip |
Add prototypical support for Spektrum satellite remotes to PX4IO.
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 68 | ||||
-rw-r--r-- | apps/px4io/comms.c | 51 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 199 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 12 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 14 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 7 |
6 files changed, 310 insertions, 41 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 5050c6ce7..9e2025231 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -87,6 +87,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + void set_rx_mode(unsigned mode); + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -114,6 +116,9 @@ private: // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO + bool _config_needed; ///< if true, we need to set a config update to IO + + uint8_t _rx_mode; ///< the current RX mode on IO /** * Trampoline to the worker task @@ -146,6 +151,11 @@ private: void io_send(); /** + * Send a config packet to PX4IO + */ + void config_send(); + + /** * Mixer control callback; invoked to fetch a control from a specific * group/index during mixing. */ @@ -176,7 +186,9 @@ PX4IO::PX4IO() : _mixers(nullptr), _primary_pwm_device(false), _switch_armed(false), - _send_needed(false) + _send_needed(false), + _config_needed(false), + _rx_mode(RX_MODE_PPM_ONLY) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -373,6 +385,12 @@ PX4IO::task_main() _send_needed = false; io_send(); } + + /* send a config packet to IO if required */ + if (_config_needed) { + _config_needed = false; + config_send(); + } } out: @@ -473,6 +491,20 @@ PX4IO::io_send() debug("send error %d", ret); } +void +PX4IO::config_send() +{ + px4io_config cfg; + int ret; + + cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + cfg.serial_rx_mode = _rx_mode; + + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); + if (ret) + debug("config error %d", ret); +} + int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { @@ -586,6 +618,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } +void +PX4IO::set_rx_mode(unsigned mode) +{ + if (mode != _rx_mode) { + _rx_mode = mode; + _config_needed = true; + } +} + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace @@ -642,9 +683,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "test")) - test(); - /* note, stop not currently implemented */ if (!strcmp(argv[1], "update")) { @@ -690,5 +728,25 @@ px4io_main(int argc, char *argv[]) return ret; } - errx(1, "need a verb, only support 'start' and 'update'"); + if (!strcmp(argv[1], "rx_spektrum6")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6); + } + if (!strcmp(argv[1], "rx_spektrum7")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7); + } + if (!strcmp(argv[1], "rx_sbus")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + } + + if (!strcmp(argv[1], "test")) + test(); + + + errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'"); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 8fdf7d1c8..1edff25b1 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -116,18 +116,32 @@ int frame_rx; int frame_bad; static void -comms_handle_frame(void *arg, const void *buffer, size_t length) +comms_handle_config(const void *buffer, size_t length) { - struct px4io_command *cmd = (struct px4io_command *)buffer; + const struct px4io_config *cfg = (struct px4io_config *)buffer; - /* make sure it's what we are expecting */ - if ((length != sizeof(struct px4io_command)) || - (cmd->f2i_magic != F2I_MAGIC)) { - frame_bad++; + if (length != sizeof(*cfg)) { + frame_bad++; return; } + frame_rx++; + mixer_set_serial_mode(cfg->serial_rx_mode); + +} + +static void +comms_handle_command(const void *buffer, size_t length) +{ + const struct px4io_command *cmd = (struct px4io_command *)buffer; + + if (length != sizeof(*cmd)) { + frame_bad++; + return; + } + + frame_rx++; irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -146,6 +160,29 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) system_state.relays[i] = cmd->relay_state[i]; -out: irqrestore(flags); } + + +static void +comms_handle_frame(void *arg, const void *buffer, size_t length) +{ + const uint16_t *type = (const uint16_t *)buffer; + + + /* make sure it's what we are expecting */ + if (length > 2) { + switch (*type) { + case F2I_MAGIC: + comms_handle_command(buffer, length); + break; + case F2I_CONFIG_MAGIC: + comms_handle_config(buffer, length); + break; + default: + break; + } + } + frame_bad++; +} + 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; - } -} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c4a63d886..7467f1adc 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -62,6 +62,18 @@ struct px4io_command { bool arm_ok; }; +/* config message from FMU to IO */ +struct px4io_config { + uint16_t f2i_config_magic; +#define F2I_CONFIG_MAGIC 0x6366 + + uint8_t serial_rx_mode; +#define RX_MODE_PPM_ONLY 0 +#define RX_MODE_SPEKTRUM_6 1 +#define RX_MODE_SPEKTRUM_7 2 +#define RX_MODE_FUTABA_SBUS 3 +}; + /* report from IO to FMU */ struct px4io_report { uint16_t i2f_magic; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index dc8e227fc..7039e5d58 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -102,7 +102,7 @@ int user_start(int argc, char *argv[]) safety_init(); /* set up some timers for the main loop */ - timers[TIMER_BLINK_AMBER] = 250; /* heartbeat blink @ 2Hz */ + timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */ timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */ /* @@ -114,21 +114,21 @@ int user_start(int argc, char *argv[]) comms_check(); /* blink the heartbeat LED */ - if (timers[TIMER_BLINK_AMBER] == 0) { - timers[TIMER_BLINK_AMBER] = 250; - LED_AMBER(heartbeat = !heartbeat); + if (timers[TIMER_BLINK_BLUE] == 0) { + timers[TIMER_BLINK_BLUE] = 250; + LED_BLUE(heartbeat = !heartbeat); } /* blink the failsafe LED if we don't have FMU input */ if (!system_state.mixer_use_fmu) { - if (timers[TIMER_BLINK_BLUE] == 0) { - timers[TIMER_BLINK_BLUE] = 125; + if (timers[TIMER_BLINK_AMBER] == 0) { + timers[TIMER_BLINK_AMBER] = 125; failsafe = !failsafe; } } else { failsafe = false; } - LED_BLUE(failsafe); + LED_AMBER(failsafe); /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 096b109d6..274b27ff3 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -101,6 +101,12 @@ struct sys_state_s * If true, new control data from the FMU has been received. */ bool fmu_data_received; + + /* + * Current serial interface mode, per the serial_rx_mode parameter + * in the config packet. + */ + uint8_t serial_rx_mode; }; extern struct sys_state_s system_state; @@ -141,6 +147,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern int mixer_init(void); +extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. |