fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 07:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch '#61-px4io-spektrum-decoder' into fixedwing_outdoor

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-01 10:51:25 +01:00
parent 00b79764d7 de88732e8e
commit 121a9fc490
19 changed files with 1085 additions and 515 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4io/px4io.cpp
+47 -69
View File
@@ -53,6 +53,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <arch/board/board.h>
@@ -88,15 +89,13 @@ 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;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
int _task; ///< worker task
volatile int _task; ///< worker task
volatile bool _task_should_exit;
volatile bool _connected; ///< true once we have received a valid frame
@@ -125,8 +124,6 @@ private:
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
*/
@@ -195,8 +192,7 @@ PX4IO::PX4IO() :
_primary_pwm_device(false),
_switch_armed(false),
_send_needed(false),
_config_needed(false),
_rx_mode(RX_MODE_PPM_ONLY)
_config_needed(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -206,33 +202,17 @@ PX4IO::PX4IO() :
PX4IO::~PX4IO()
{
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
/* tell the task we want it to go away */
_task_should_exit = true;
/* spin waiting for the thread to stop */
unsigned i = 10;
do {
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
/* spin waiting for the task to stop */
for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
/* give it another 100ms */
usleep(100000);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
g_dev = nullptr;
}
@@ -300,6 +280,16 @@ PX4IO::task_main()
goto out;
}
/* 115200bps, no parity, one stop bit */
{
struct termios t;
tcgetattr(_serial_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(_serial_fd, TCSANOW, &t);
}
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
if (_io_stream == nullptr) {
@@ -358,7 +348,6 @@ PX4IO::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
@@ -418,8 +407,16 @@ PX4IO::task_main()
}
out:
debug("exiting");
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_serial_fd);
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* tell the dtor that we are exiting */
_task = -1;
@@ -480,15 +477,17 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
}
_connected = true;
/* publish raw rc channel values from IO */
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
}
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
}
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
}
/* remember the latched arming switch state */
_switch_armed = rep->armed;
@@ -531,7 +530,6 @@ PX4IO::config_send()
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)
@@ -651,15 +649,6 @@ 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
@@ -761,25 +750,14 @@ px4io_main(int argc, char *argv[])
return ret;
}
if (!strcmp(argv[1], "rx_dsm_10bit")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_10BIT);
}
if (!strcmp(argv[1], "rx_dsm_11bit")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_11BIT);
}
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], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
!strcmp(argv[1], "rx_sbus") ||
!strcmp(argv[1], "rx_ppm"))
errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
if (!strcmp(argv[1], "test"))
test();
errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'");
errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
}
apps/drivers/px4io/uploader.cpp
+4 -2
View File
@@ -189,8 +189,10 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 10);
//log("discard 0x%02x", c);
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}
} while (ret == OK);
}
apps/px4io/comms.c
+52 -114
View File
@@ -32,10 +32,11 @@
****************************************************************************/
/**
* @file FMU communication for the PX4IO module.
* @file comms.c
*
* FMU communication for the PX4IO module.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
@@ -44,9 +45,9 @@
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <termios.h>
#include <string.h>
#include <poll.h>
#include <termios.h>
#include <nuttx/clock.h>
@@ -54,103 +55,59 @@
#include <systemlib/hx_stream.h>
#include <systemlib/perf_counter.h>
#define DEBUG
#include "px4io.h"
#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
int frame_rx;
int frame_bad;
static int fmu_fd;
static hx_stream_t stream;
static int rx_fd;
static struct px4io_report report;
static void comms_handle_frame(void *arg, const void *buffer, size_t length);
static struct pollfd pollfds[2];
static int pollcount;
void
static void
comms_init(void)
{
/* initialise the FMU interface */
fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK);
if (fmu_fd < 0)
lib_lowprintf("COMMS: fmu open failed %d\n", errno);
fmu_fd = open("/dev/ttyS1", O_RDWR);
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
pollfds[0].fd = fmu_fd;
pollfds[0].events = POLLIN;
pollcount = 1;
/* default state in the report to FMU */
report.i2f_magic = I2F_MAGIC;
}
static void
serial_rx_init(unsigned serial_mode)
{
if (serial_mode == system_state.serial_rx_mode)
return;
system_state.serial_rx_mode = serial_mode;
if (serial_mode == RX_MODE_PPM_ONLY) {
if (rx_fd != -1) {
pollcount = 1;
close(rx_fd);
rx_fd = -1;
}
return;
}
/* open the serial port used for DSM and S.bus communication */
rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
pollfds[1].fd = rx_fd;
pollfds[1].events = POLLIN;
pollcount = 2;
struct termios t;
tcgetattr(rx_fd, &t);
switch (serial_mode) {
case RX_MODE_DSM_10BIT:
case RX_MODE_DSM_11BIT:
/* 115200, no parity, one stop bit */
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
dsm_init(serial_mode);
break;
case RX_MODE_FUTABA_SBUS:
/* 100000, even parity, two stop bits */
cfsetspeed(&t, 100000);
t.c_cflag |= (CSTOPB | PARENB);
sbus_init(serial_mode);
break;
default:
break;
}
tcsetattr(rx_fd, TCSANOW, &t);
/* 115200bps, no parity, one stop bit */
tcgetattr(fmu_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(fmu_fd, TCSANOW, &t);
}
void
comms_check(void)
comms_main(void)
{
/*
* Check for serial data
*/
int ret = poll(pollfds, pollcount, 0);
comms_init();
struct pollfd fds;
fds.fd = fmu_fd;
fds.events = POLLIN;
debug("FMU: ready");
for (;;) {
/* wait for serial data, but no more than 100ms */
poll(&fds, 1, 100);
if (ret > 0) {
/*
* Pull bytes from FMU and feed them to the HX engine.
* Limit the number of bytes we actually process on any one iteration.
*/
if (pollfds[0].revents & POLLIN) {
if (fds.revents & POLLIN) {
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++)
@@ -158,55 +115,38 @@ comms_check(void)
}
/*
* Pull bytes from the serial RX port and feed them to the decoder
* if we care about serial RX data.
* Decide if it's time to send an update to the FMU.
*/
if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) {
switch (system_state.serial_rx_mode) {
case RX_MODE_DSM_10BIT:
case RX_MODE_DSM_11BIT:
dsm_input(rx_fd);
break;
static hrt_abstime last_report_time;
hrt_abstime now, delta;
case RX_MODE_FUTABA_SBUS:
sbus_input(rx_fd);
break;
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
default:
break;
system_state.fmu_report_due = false;
last_report_time = now;
/* populate the report */
for (int i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i];
if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) {
report.channel_count = system_state.rc_channels;
} else {
report.channel_count = 0;
}
report.armed = system_state.armed;
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
}
/*
* Decide if it's time to send an update to the FMU.
*/
static hrt_abstime last_report_time;
hrt_abstime now, delta;
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
last_report_time = now;
/* populate the report */
for (int i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i];
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
}
int frame_rx;
int frame_bad;
static void
comms_handle_config(const void *buffer, size_t length)
{
@@ -218,8 +158,6 @@ comms_handle_config(const void *buffer, size_t length)
}
frame_rx++;
serial_rx_init(cfg->serial_rx_mode);
}
static void
@@ -277,9 +215,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
comms_handle_config(buffer, length);
break;
default:
frame_bad++;
break;
}
}
frame_bad++;
}
apps/px4io/controls.c
+88
View File
@@ -0,0 +1,88 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file controls.c
*
* R/C inputs and servo outputs.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <termios.h>
#include <string.h>
#include <poll.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <systemlib/hx_stream.h>
#include <systemlib/perf_counter.h>
#define DEBUG
#include "px4io.h"
void
controls_main(void)
{
struct pollfd fds[2];
fds[0].fd = dsm_init("/dev/ttyS0");
fds[0].events = POLLIN;
fds[1].fd = sbus_init("/dev/ttyS2");
fds[1].events = POLLIN;
for (;;) {
/* run this loop at ~100Hz */
poll(fds, 2, 10);
if (fds[0].revents & POLLIN)
dsm_input();
if (fds[1].revents & POLLIN)
sbus_input();
/* XXX do ppm processing, bypass mode, etc. here */
/* do PWM output updates */
mixer_tick();
}
}
apps/px4io/dsm.c
+194 -37
View File
@@ -44,46 +44,69 @@
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#include "protocol.h"
#define DSM_FRAME_SIZE 16
#define DSM_FRAME_CHANNELS 7
static int dsm_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static uint8_t frame[DSM_FRAME_SIZE];
static unsigned partial_frame_count;
static bool insync;
static unsigned channel_shift;
static void dsm_decode(void);
unsigned dsm_frame_drops;
void
dsm_init(unsigned mode)
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
static void dsm_guess_format(bool reset);
static void dsm_decode(hrt_abstime now);
int
dsm_init(const char *device)
{
insync = false;
partial_frame_count = 0;
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY);
if (mode == RX_MODE_DSM_10BIT) {
channel_shift = 10;
if (dsm_fd >= 0) {
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(dsm_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(dsm_fd, TCSANOW, &t);
/* initialise the decoder */
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
/* reset the format detector */
dsm_guess_format(true);
debug("DSM: ready");
} else {
channel_shift = 11;
debug("DSM: open failed");
}
last_frame_time = hrt_absolute_time();
return dsm_fd;
}
void
dsm_input(int fd)
dsm_input(void)
{
uint8_t buf[DSM_FRAME_SIZE];
ssize_t ret;
hrt_abstime now;
@@ -97,25 +120,33 @@ dsm_input(int fd)
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_frame_time) > 5000)
partial_frame_count = 0;
if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) {
dsm_frame_drops++;
partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current frame.
*/
ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count);
ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
return;
last_rx_time = now;
/*
* Add bytes to the current frame
*/
memcpy(&frame[partial_frame_count], buf, ret);
partial_frame_count += ret;
/*
@@ -123,30 +154,137 @@ dsm_input(int fd)
*/
if (partial_frame_count < DSM_FRAME_SIZE)
return;
last_frame_time = now;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
dsm_decode();
dsm_decode(now);
partial_frame_count = 0;
}
static void
dsm_decode(void)
static bool
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{
uint16_t data_mask = (1 << channel_shift) - 1;
if (raw == 0xffff)
return false;
*channel = (raw >> shift) & 0xf;
uint16_t data_mask = (1 << shift) - 1;
*value = raw & data_mask;
//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
return true;
}
static void
dsm_guess_format(bool reset)
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
cs10 = 0;
cs11 = 0;
samples = 0;
channel_shift = 0;
return;
}
/* scan the channels in the current frame in both 10- and 11-bit mode */
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel);
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel);
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
}
/* wait until we have seen plenty of frames - 2 should normally be enough */
if (samples++ < 5)
return;
/*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
*
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
* stream, we may want to sniff for longer in some cases when we think we
* are talking to a DSM2 receiver in high-resolution mode (so that we can
* reject it, ideally).
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
0x3ff, /* 10 channels (DX10) */
0x3fff /* 18 channels (DX10) */
};
unsigned votes10 = 0;
unsigned votes11 = 0;
for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {
if (cs10 == masks[i])
votes10++;
if (cs11 == masks[i])
votes11++;
}
if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11;
debug("DSM: detected 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10;
debug("DSM: detected 10-bit format");
return;
}
/* call ourselves to reset our state ... we have to try again */
debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
dsm_guess_format(true);
}
static void
dsm_decode(hrt_abstime frame_time)
{
/*
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
dsm_guess_format(true);
last_frame_time = frame_time;
if (channel_shift == 0) {
dsm_guess_format(false);
system_state.dsm_input_ok = false;
return;
}
/*
* The encoding of the first byte is uncertain, so we're going
* to ignore it for now.
*
* The second byte may tell us about the protocol, but it's not
* actually very interesting since what we really want to know
* is how the channel data is formatted, and there doesn't seem
* to be a reliable way to determine this from the protocol ID
* alone.
* The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
*
* Each channel is a 16-bit unsigned value containing either a 10-
* or 11-bit channel value and a 4-bit channel number, shifted
@@ -155,30 +293,49 @@ dsm_decode(void)
* seven channels are being transmitted.
*/
const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS;
uint16_t dsm_channels[dsm_chancount];
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
/* ignore pad channels */
if (raw == 0xffff)
if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
continue;
unsigned channel = (raw >> channel_shift) & 0xf;
/* ignore channels out of range */
if (channel >= PX4IO_INPUT_CHANNELS)
continue;
/* update the decoded channel count */
if (channel > ppm_decoded_channels)
ppm_decoded_channels = channel;
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
unsigned data = raw & data_mask;
if (channel_shift == 11)
data /= 2;
ppm_buffer[channel] = 988 + data;
value /= 2;
/* stuff the decoded channel into the PPM input buffer */
dsm_channels[channel] = 988 + value;
}
/* DSM input is valid */
system_state.dsm_input_ok = true;
/* check if no S.BUS data is available */
if (!system_state.sbus_input_ok) {
for (unsigned i = 0; i < dsm_chancount; i++) {
system_state.rc_channel_data[i] = dsm_channels[i];
}
/* and note that we have received data from the R/C controller */
/* XXX failsafe will cause problems here - need a strategy for detecting it */
system_state.rc_channels_timestamp = frame_time;
system_state.rc_channels = dsm_chancount;
system_state.fmu_report_due = true;
}
ppm_last_valid_decode = hrt_absolute_time();
}
apps/px4io/mixer.c
+20 -36
View File
@@ -49,7 +49,6 @@
#include <fcntl.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <systemlib/ppm_decode.h>
@@ -60,17 +59,6 @@
*/
static unsigned fmu_input_drops;
#define FMU_INPUT_DROP_LIMIT 20
/*
* HRT periodic call used to check for control input data.
*/
static struct hrt_call mixer_input_call;
/*
* Mixer periodic tick.
*/
static void mixer_tick(void *arg);
/*
* Collect RC input data from the controller source(s).
*/
@@ -92,20 +80,8 @@ struct mixer {
/* XXX more config here */
} mixers[IO_SERVO_COUNT];
int
mixer_init(void)
{
/* look for control data at 50Hz */
hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
return 0;
}
static void
mixer_tick(void *arg)
void
mixer_tick(void)
{
uint16_t *control_values;
int control_count;
@@ -148,12 +124,11 @@ mixer_tick(void *arg)
/* we have no control input */
control_count = 0;
}
/*
* Tickle each mixer, if we have control data.
*/
if (control_count > 0) {
for (i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
for (i = 0; i < IO_SERVO_COUNT; i++) {
mixer_update(i, control_values, control_count);
/*
@@ -195,17 +170,26 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
static void
mixer_get_rc_input(void)
{
/* 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;
/* input was ok and timed out, mark as update */
if (system_state.ppm_input_ok) {
system_state.ppm_input_ok = false;
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;
/* mark PPM as valid */
system_state.ppm_input_ok = true;
/* check if no DSM and S.BUS data is available */
if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) {
/* 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;
}
}
apps/px4io/protocol.h
+1 -5
View File
@@ -62,11 +62,7 @@ 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_DSM_10BIT 1
#define RX_MODE_DSM_11BIT 2
#define RX_MODE_FUTABA_SBUS 3
/* XXX currently nothing here */
};
/* report from IO to FMU */
apps/px4io/px4io.c
+15 -76
View File
@@ -55,37 +55,17 @@
__EXPORT int user_start(int argc, char *argv[]);
struct sys_state_s system_state;
int gpio_fd;
static const char cursor[] = {'|', '/', '-', '\\'};
static struct hrt_call timer_tick_call;
volatile int timers[TIMER_NUM_TIMERS];
static void timer_tick(void *arg);
int user_start(int argc, char *argv[])
{
int cycle = 0;
bool heartbeat = false;
bool failsafe = false;
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* init the FMU and receiver links */
comms_init();
/* configure the first 8 PWM outputs (i.e. all of them) */
/* note, must do this after comms init to steal back PA0, which is CTS otherwise */
up_pwm_servo_init(0xff);
/* print some startup info */
lib_lowprintf("\nPX4IO: starting\n");
struct mallinfo minfo = mallinfo();
lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
/* start the software timer service */
hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL);
/* default all the LEDs to off while we start */
LED_AMBER(false);
@@ -95,64 +75,23 @@ int user_start(int argc, char *argv[])
/* turn on servo power */
POWER_SERVO(true);
/* start the mixer */
mixer_init();
/* start the safety switch handler */
safety_init();
/* set up some timers for the main loop */
timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */
timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/*
* Main loop servicing communication with FMU
*/
while(true) {
/* start the flight control signal handler */
task_create("FCon",
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)controls_main,
NULL);
/* check for communication from FMU, send updates */
comms_check();
/* blink the heartbeat LED */
if (timers[TIMER_BLINK_BLUE] == 0) {
timers[TIMER_BLINK_BLUE] = 250;
LED_BLUE(heartbeat = !heartbeat);
}
struct mallinfo minfo = mallinfo();
lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
if (timers[TIMER_BLINK_AMBER] == 0) {
timers[TIMER_BLINK_AMBER] = 125;
failsafe = !failsafe;
}
} else {
failsafe = false;
}
LED_AMBER(failsafe);
/* print some simple status */
if (timers[TIMER_STATUS_PRINT] == 0) {
timers[TIMER_STATUS_PRINT] = 1000;
lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r",
cursor[cycle++ & 3],
(system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"),
(system_state.armed ? "ARMED" : "SAFE"),
(system_state.rc_channels ? "RC OK" : "NO RC"),
(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
system_state.rc_channels,
frame_rx, frame_bad
);
}
}
/* Should never reach here */
return ERROR;
}
static void
timer_tick(void *arg)
{
for (unsigned i = 0; i < TIMER_NUM_TIMERS; i++)
if (timers[i] > 0)
timers[i]--;
}
/* we're done here, go run the communications loop */
comms_main();
}
apps/px4io/px4io.h
+17 -11
View File
@@ -56,10 +56,11 @@
* Debug logging
*/
#if 1
# define debug(fmt, ...) lib_lowprintf(fmt "\n", ##args)
#ifdef DEBUG
# include <debug.h>
# define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args)
#else
# define debug(fmt, ...) do {} while(0)
# define debug(fmt, args...) do {} while(0)
#endif
/*
@@ -71,11 +72,16 @@ struct sys_state_s
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
bool ppm_input_ok; /* valid PPM input data */
bool dsm_input_ok; /* valid Spektrum DSM data */
bool sbus_input_ok; /* valid Futaba S.Bus data */
/*
* Data from the remote control input(s)
*/
int rc_channels;
uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
uint64_t rc_channels_timestamp;
/*
* Control signals from FMU.
@@ -146,7 +152,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
/*
* Mixer
*/
extern int mixer_init(void);
extern void mixer_tick(void);
/*
* Safety switch/LED.
@@ -156,16 +162,16 @@ extern void safety_init(void);
/*
* FMU communications
*/
extern void comms_init(void);
extern void comms_check(void);
extern void comms_main(void) __attribute__((noreturn));
/*
* Serial receiver decoders.
* R/C receiver handling.
*/
extern void dsm_init(unsigned mode);
extern void dsm_input(int fd);
extern void sbus_init(unsigned mode);
extern void sbus_input(int fd);
extern void controls_main(void);
extern int dsm_init(const char *device);
extern void dsm_input(void);
extern int sbus_init(const char *device);
extern void sbus_input(void);
/*
* Assertion codes
apps/px4io/safety.c
+36
View File
@@ -51,6 +51,8 @@
#include "px4io.h"
static struct hrt_call arming_call;
static struct hrt_call heartbeat_call;
static struct hrt_call failsafe_call;
/*
* Count the number of times in a row that we see the arming button
@@ -63,13 +65,22 @@ static unsigned counter;
static bool safety_led_state;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
static void heartbeat_blink(void *arg);
static void failsafe_blink(void *arg);
void
safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
/* arrange for the heartbeat handler to be called at 4Hz */
hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
static void
@@ -117,3 +128,28 @@ safety_check_button(void *arg)
}
LED_SAFETY(safety_led_state);
}
static void
heartbeat_blink(void *arg)
{
static bool heartbeat = false;
/* XXX add flags here that need to be frobbed by various loops */
LED_BLUE(heartbeat = !heartbeat);
}
static void
failsafe_blink(void *arg)
{
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
failsafe = !failsafe;
} else {
failsafe = false;
}
LED_AMBER(failsafe);
}
apps/px4io/sbus.c
+186 -3
View File
@@ -41,18 +41,201 @@
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#include "protocol.h"
#include "debug.h"
void
sbus_init(unsigned mode)
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static uint8_t frame[SBUS_FRAME_SIZE];
static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static void sbus_decode(hrt_abstime frame_time);
int
sbus_init(const char *device)
{
if (sbus_fd < 0)
sbus_fd = open(device, O_RDONLY);
if (sbus_fd >= 0) {
struct termios t;
/* 100000bps, even parity, two stop bits */
tcgetattr(sbus_fd, &t);
cfsetspeed(&t, 100000);
t.c_cflag |= (CSTOPB | PARENB);
tcsetattr(sbus_fd, TCSANOW, &t);
/* initialise the decoder */
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
debug("Sbus: ready");
} else {
debug("Sbus: open failed");
}
return sbus_fd;
}
void
sbus_input(int fd)
sbus_input(void)
{
ssize_t ret;
hrt_abstime now;
/*
* The S.bus protocol doesn't provide reliable framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current frame.
*/
ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
return;
last_rx_time = now;
/*
* Add bytes to the current frame
*/
partial_frame_count += ret;
/*
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
return;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
sbus_decode(now);
partial_frame_count = 0;
}
/*
* S.bus decoder matrix.
*
* Each channel value can come from up to 3 input bytes. Each row in the
* matrix describes up to three bytes, and each entry gives:
*
* - byte offset in the data portion of the frame
* - right shift applied to the data byte
* - mask for the data byte
* - left shift applied to the result into the channel value
*/
struct sbus_bit_pick {
uint8_t byte;
uint8_t rshift;
uint8_t mask;
uint8_t lshift;
};
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} }
};
static void
sbus_decode(hrt_abstime frame_time)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
sbus_frame_drops++;
system_state.sbus_input_ok = false;
return;
}
/* if the failsafe bit is set, we consider that a loss of RX signal */
if (frame[23] & (1 << 4)) {
system_state.sbus_input_ok = false;
return;
}
unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
unsigned value = 0;
for (unsigned pick = 0; pick < 3; pick++) {
const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
if (decode->mask != 0) {
unsigned piece = frame[1 + decode->byte];
piece >>= decode->rshift;
piece &= decode->mask;
piece <<= decode->lshift;
value |= piece;
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998;
}
if (PX4IO_INPUT_CHANNELS >= 18) {
/* decode two switch channels */
chancount = 18;
}
system_state.rc_channels = chancount;
system_state.sbus_input_ok = true;
system_state.fmu_report_due = true;
/* and note that we have received data from the R/C controller */
system_state.rc_channels_timestamp = frame_time;
}