mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Add prototypical support for Spektrum satellite remotes to PX4IO.
This commit is contained in:
px4dev
parent
87fd9fcc06
commit
39659e57f8
@ -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
|
||||
@ -145,6 +150,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'");
|
||||
}
|
||||
|
||||
@ -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++;
|
||||
}
|
||||
|
||||
|
||||
@ -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>
|
||||
@ -64,6 +67,11 @@ static unsigned mixer_input_drops;
|
||||
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.
|
||||
*/
|
||||
@ -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)
|
||||
{
|
||||
@ -180,23 +223,146 @@ mixer_tick(void *arg)
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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: <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 (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)
|
||||
{
|
||||
/*
|
||||
* XXX currently only supporting PPM
|
||||
*
|
||||
* XXX check timestamp to ensure current
|
||||
*/
|
||||
if (ppm_decoded_channels > 0) {
|
||||
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;
|
||||
system_state.fmu_report_due = true;
|
||||
|
||||
/* 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];
|
||||
|
||||
} 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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -155,6 +155,8 @@ CONFIG_STM32_ADC3=n
|
||||
# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
|
||||
# CONFIG_USARTn_2STOP - Two stop bits
|
||||
#
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
|
||||
CONFIG_USART1_SERIAL_CONSOLE=y
|
||||
CONFIG_USART2_SERIAL_CONSOLE=n
|
||||
CONFIG_USART3_SERIAL_CONSOLE=n
|
||||
@ -163,11 +165,11 @@ CONFIG_USART1_TXBUFSIZE=32
|
||||
CONFIG_USART2_TXBUFSIZE=32
|
||||
CONFIG_USART3_TXBUFSIZE=32
|
||||
|
||||
CONFIG_USART1_RXBUFSIZE=32
|
||||
CONFIG_USART1_RXBUFSIZE=64
|
||||
CONFIG_USART2_RXBUFSIZE=128
|
||||
CONFIG_USART3_RXBUFSIZE=32
|
||||
|
||||
CONFIG_USART1_BAUD=57600
|
||||
CONFIG_USART1_BAUD=115200
|
||||
CONFIG_USART2_BAUD=115200
|
||||
CONFIG_USART3_BAUD=115200
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user