dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Makefile
View File

@ -115,9 +115,20 @@ endif
upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
#
# JTAG firmware uploading with OpenOCD
#
ifeq ($(JTAGCONFIG),)
JTAGCONFIG=interface/olimex-jtag-tiny.cfg
endif
upload-jtag-px4fmu:
@echo Attempting to flash PX4FMU board via JTAG
@openocd -f interface/olimex-jtag-tiny.cfg -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
@openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
upload-jtag-px4io: all
@echo Attempting to flash PX4IO board via JTAG
@openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown
#
# Hacks and fixups

98
apps/drivers/px4io/px4io.cpp
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;
/* 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) {
/* spin waiting for the task to stop */
for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
/* give it another 100ms */
usleep(100000);
}
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
break;
}
} while (_task != -1);
}
/* 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);
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,7 +477,8 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
}
_connected = true;
/* publish raw rc channel values from IO */
/* 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++)
@ -489,6 +487,7 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
}
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'");
}

4
apps/drivers/px4io/uploader.cpp
View File

@ -189,8 +189,10 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 10);
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}
} while (ret == OK);
}

128
apps/px4io/comms.c
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,130 +55,65 @@
#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 */
/* 115200bps, no parity, one stop bit */
tcgetattr(fmu_fd, &t);
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);
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++)
hx_stream_rx(stream, buf[i]);
}
/*
* Pull bytes from the serial RX port and feed them to the decoder
* if we care about serial RX data.
*/
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;
case RX_MODE_FUTABA_SBUS:
sbus_input(rx_fd);
break;
default:
break;
}
}
}
/*
* Decide if it's time to send an update to the FMU.
*/
@ -196,16 +132,20 @@ comms_check(void)
/* 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));
}
}
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++;
}

88
apps/px4io/controls.c Normal file
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();
}
}

225
apps/px4io/dsm.c
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;
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY);
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();
if (mode == RX_MODE_DSM_10BIT) {
channel_shift = 10;
/* 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)
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;
/*
* The encoding of the first byte is uncertain, so we're going
* to ignore it for now.
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
*
* 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.
* 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 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();
}

44
apps/px4io/mixer.c
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;
/* 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;
}
/* 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;
}
}

6
apps/px4io/protocol.h
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 */

89
apps/px4io/px4io.c
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();
}

28
apps/px4io/px4io.h
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

36
apps/px4io/safety.c
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);
}

189
apps/px4io/sbus.c
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;
}

184
nuttx/arch/arm/src/stm32/stm32_serial.c
View File

@ -185,11 +185,15 @@ struct up_dev_s
uint8_t parity; /* 0=none, 1=odd, 2=even */
uint8_t bits; /* Number of bits (7 or 8) */
bool stopbits2; /* True: Configure with 2 stop bits instead of 1 */
bool iflow; /* input flow control (RTS) enabled */
bool oflow; /* output flow control (CTS) enabled */
uint32_t baud; /* Configured baud */
#else
const uint8_t parity; /* 0=none, 1=odd, 2=even */
const uint8_t bits; /* Number of bits (7 or 8) */
const bool stopbits2; /* True: Configure with 2 stop bits instead of 1 */
const bool iflow; /* input flow control (RTS) enabled */
const bool oflow; /* output flow control (CTS) enabled */
const uint32_t baud; /* Configured baud */
#endif
@ -221,7 +225,7 @@ struct up_dev_s
* Private Function Prototypes
****************************************************************************/
static void up_setspeed(struct uart_dev_s *dev);
static void up_set_format(struct uart_dev_s *dev);
static int up_setup(struct uart_dev_s *dev);
static void up_shutdown(struct uart_dev_s *dev);
static int up_attach(struct uart_dev_s *dev);
@ -393,6 +397,8 @@ static struct up_dev_s g_usart1priv =
.parity = CONFIG_USART1_PARITY,
.bits = CONFIG_USART1_BITS,
.stopbits2 = CONFIG_USART1_2STOP,
.iflow = false,
.oflow = false,
.baud = CONFIG_USART1_BAUD,
.apbclock = STM32_PCLK2_FREQUENCY,
.usartbase = STM32_USART1_BASE,
@ -444,6 +450,8 @@ static struct up_dev_s g_usart2priv =
.parity = CONFIG_USART2_PARITY,
.bits = CONFIG_USART2_BITS,
.stopbits2 = CONFIG_USART2_2STOP,
.iflow = false,
.oflow = false,
.baud = CONFIG_USART2_BAUD,
.apbclock = STM32_PCLK1_FREQUENCY,
.usartbase = STM32_USART2_BASE,
@ -495,6 +503,8 @@ static struct up_dev_s g_usart3priv =
.parity = CONFIG_USART3_PARITY,
.bits = CONFIG_USART3_BITS,
.stopbits2 = CONFIG_USART3_2STOP,
.iflow = false,
.oflow = false,
.baud = CONFIG_USART3_BAUD,
.apbclock = STM32_PCLK1_FREQUENCY,
.usartbase = STM32_USART3_BASE,
@ -546,17 +556,15 @@ static struct up_dev_s g_uart4priv =
.parity = CONFIG_UART4_PARITY,
.bits = CONFIG_UART4_BITS,
.stopbits2 = CONFIG_UART4_2STOP,
.iflow = false,
.oflow = false,
.baud = CONFIG_UART4_BAUD,
.apbclock = STM32_PCLK1_FREQUENCY,
.usartbase = STM32_UART4_BASE,
.tx_gpio = GPIO_UART4_TX,
.rx_gpio = GPIO_UART4_RX,
#ifdef GPIO_UART4_CTS
.cts_gpio = GPIO_UART4_CTS,
#endif
#ifdef GPIO_UART4_RTS
.rts_gpio = GPIO_UART4_RTS,
#endif
.cts_gpio = 0, /* flow control not supported on this port */
.rts_gpio = 0, /* flow control not supported on this port */
#ifdef CONFIG_UART4_RXDMA
.rxdma_channel = DMAMAP_UART4_RX,
.rxfifo = g_uart4rxfifo,
@ -597,17 +605,15 @@ static struct up_dev_s g_uart5priv =
.parity = CONFIG_UART5_PARITY,
.bits = CONFIG_UART5_BITS,
.stopbits2 = CONFIG_UART5_2STOP,
.iflow = false,
.oflow = false,
.baud = CONFIG_UART5_BAUD,
.apbclock = STM32_PCLK1_FREQUENCY,
.usartbase = STM32_UART5_BASE,
.tx_gpio = GPIO_UART5_TX,
.rx_gpio = GPIO_UART5_RX,
#ifdef GPIO_UART5_CTS
.cts_gpio = GPIO_UART5_CTS,
#endif
#ifdef GPIO_UART5_RTS
.rts_gpio = GPIO_UART5_RTS,
#endif
.cts_gpio = 0, /* flow control not supported on this port */
.rts_gpio = 0, /* flow control not supported on this port */
#ifdef CONFIG_UART5_RXDMA
.rxdma_channel = DMAMAP_UART5_RX,
.rxfifo = g_uart5rxfifo,
@ -648,6 +654,8 @@ static struct up_dev_s g_usart6priv =
.parity = CONFIG_USART6_PARITY,
.bits = CONFIG_USART6_BITS,
.stopbits2 = CONFIG_USART6_2STOP,
.iflow = false,
.oflow = false,
.baud = CONFIG_USART6_BAUD,
.apbclock = STM32_PCLK2_FREQUENCY,
.usartbase = STM32_USART6_BASE,
@ -812,21 +820,22 @@ static int up_dma_nextrx(struct up_dev_s *priv)
#endif
/****************************************************************************
* Name: up_setspeed
* Name: up_set_format
*
* Description:
* Set the serial line speed.
* Set the serial line format and speed.
*
****************************************************************************/
#ifndef CONFIG_SUPPRESS_UART_CONFIG
static void up_setspeed(struct uart_dev_s *dev)
static void up_set_format(struct uart_dev_s *dev)
{
struct up_dev_s *priv = (struct up_dev_s*)dev->priv;
uint32_t usartdiv32;
uint32_t mantissa;
uint32_t fraction;
uint32_t brr;
uint32_t regval;
/* Configure the USART Baud Rate. The baud rate for the receiver and
* transmitter (Rx and Tx) are both set to the same value as programmed
@ -856,8 +865,52 @@ static void up_setspeed(struct uart_dev_s *dev)
fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
brr |= fraction << USART_BRR_FRAC_SHIFT;
up_serialout(priv, STM32_USART_BRR_OFFSET, brr);
/* Configure parity mode */
regval = up_serialin(priv, STM32_USART_CR1_OFFSET);
regval &= ~(USART_CR1_PCE|USART_CR1_PS);
if (priv->parity == 1) /* Odd parity */
{
regval |= (USART_CR1_PCE|USART_CR1_PS);
}
#endif
else if (priv->parity == 2) /* Even parity */
{
regval |= USART_CR1_PCE;
}
up_serialout(priv, STM32_USART_CR1_OFFSET, regval);
/* Configure STOP bits */
regval = up_serialin(priv, STM32_USART_CR2_OFFSET);
regval &= ~(USART_CR2_STOP_MASK);
if (priv->stopbits2)
{
regval |= USART_CR2_STOP2;
}
up_serialout(priv, STM32_USART_CR2_OFFSET, regval);
/* Configure hardware flow control */
regval = up_serialin(priv, STM32_USART_CR3_OFFSET);
regval &= ~(USART_CR3_CTSE|USART_CR3_RTSE);
if (priv->iflow && (priv->rts_gpio != 0))
{
regval |= USART_CR3_RTSE;
}
if (priv->oflow && (priv->cts_gpio != 0))
{
regval |= USART_CR3_CTSE;
}
up_serialout(priv, STM32_USART_CR3_OFFSET, regval);
}
#endif /* CONFIG_SUPPRESS_UART_CONFIG */
/****************************************************************************
* Name: up_setup
@ -894,43 +947,28 @@ static int up_setup(struct uart_dev_s *dev)
}
/* Configure CR2 */
/* Clear STOP, CLKEN, CPOL, CPHA, LBCL, and interrupt enable bits */
/* Clear CLKEN, CPOL, CPHA, LBCL, and interrupt enable bits */
regval = up_serialin(priv, STM32_USART_CR2_OFFSET);
regval &= ~(USART_CR2_STOP_MASK|USART_CR2_CLKEN|USART_CR2_CPOL|
regval &= ~(USART_CR2_CLKEN|USART_CR2_CPOL|
USART_CR2_CPHA|USART_CR2_LBCL|USART_CR2_LBDIE);
/* Configure STOP bits */
if (priv->stopbits2)
{
regval |= USART_CR2_STOP2;
}
up_serialout(priv, STM32_USART_CR2_OFFSET, regval);
/* Configure CR1 */
/* Clear M, PCE, PS, TE, REm and all interrupt enable bits */
/* Clear M, TE, REm and all interrupt enable bits */
regval = up_serialin(priv, STM32_USART_CR1_OFFSET);
regval &= ~(USART_CR1_M|USART_CR1_PCE|USART_CR1_PS|USART_CR1_TE|
regval &= ~(USART_CR1_M|USART_CR1_TE|
USART_CR1_RE|USART_CR1_ALLINTS);
/* Configure word length and parity mode */
/* Configure word length */
if (priv->bits == 9) /* Default: 1 start, 8 data, n stop */
{
regval |= USART_CR1_M; /* 1 start, 9 data, n stop */
}
if (priv->parity == 1) /* Odd parity */
{
regval |= (USART_CR1_PCE|USART_CR1_PS);
}
else if (priv->parity == 2) /* Even parity */
{
regval |= USART_CR1_PCE;
}
up_serialout(priv, STM32_USART_CR1_OFFSET, regval);
/* Configure CR3 */
@ -943,16 +981,17 @@ static int up_setup(struct uart_dev_s *dev)
up_serialout(priv, STM32_USART_CR3_OFFSET, regval);
/* Configure the USART Baud Rate. */
/* Configure the USART line format and speed. */
up_setspeed(dev);
up_set_format(dev);
/* Enable Rx, Tx, and the USART */
regval = up_serialin(priv, STM32_USART_CR1_OFFSET);
regval |= (USART_CR1_UE|USART_CR1_TE|USART_CR1_RE);
up_serialout(priv, STM32_USART_CR1_OFFSET, regval);
#endif
#endif /* CONFIG_SUPPRESS_UART_CONFIG */
/* Set up the cached interrupt enables value */
@ -1279,12 +1318,21 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg)
break;
}
/* TODO: Other termios fields are not yet returned.
* Note that only cfsetospeed is not necessary because we have
* knowledge that only one speed is supported.
cfsetispeed(termiosp, priv->baud);
/* Note that since we only support 8/9 bit modes and
* there is no way to report 9-bit mode, we always claim 8.
*/
cfsetispeed(termiosp, priv->baud);
termiosp->c_cflag =
((priv->parity != 0) ? PARENB : 0) |
((priv->parity == 1) ? PARODD : 0) |
((priv->stopbits2) ? CSTOPB : 0) |
((priv->oflow) ? CCTS_OFLOW : 0) |
((priv->iflow) ? CRTS_IFLOW : 0) |
CS8;
/* TODO: CCTS_IFLOW, CCTS_OFLOW */
}
break;
@ -1298,16 +1346,48 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg)
break;
}
/* TODO: Handle other termios settings.
* Note that only cfgetispeed is used besued we have knowledge
/* Perform some sanity checks before accepting any changes */
if (((termiosp->c_cflag & CSIZE) != CS8) ||
((termiosp->c_cflag & CCTS_OFLOW) && (priv->cts_gpio == 0)) ||
((termiosp->c_cflag & CRTS_IFLOW) && (priv->rts_gpio == 0)))
{
ret = -EINVAL;
break;
}
if (termiosp->c_cflag & PARENB)
{
priv->parity = (termiosp->c_cflag & PARODD) ? 1 : 2;
}
else
{
priv->parity = 0;
}
priv->stopbits2 = (termiosp->c_cflag & CSTOPB) != 0;
priv->oflow = (termiosp->c_cflag & CCTS_OFLOW) != 0;
priv->iflow = (termiosp->c_cflag & CRTS_IFLOW) != 0;
/* Note that since there is no way to request 9-bit mode
* and no way to support 5/6/7-bit modes, we ignore them
* all here.
*/
/* Note that only cfgetispeed is used because we have knowledge
* that only one speed is supported.
*/
priv->baud = cfgetispeed(termiosp);
up_setspeed(dev);
/* effect the changes immediately - note that we do not implement
* TCSADRAIN / TCSAFLUSH
*/
up_set_format(dev);
}
break;
#endif
#endif /* CONFIG_SERIAL_TERMIOS */
#ifdef CONFIG_USART_BREAKS
case TIOCSBRK: /* BSD compatibility: Turn break on, unconditionally */
@ -1945,10 +2025,10 @@ void stm32_serial_dma_poll(void)
int up_putc(int ch)
{
#if CONSOLE_UART > 0
// struct up_dev_s *priv = uart_devs[CONSOLE_UART - 1];
// uint16_t ie;
struct up_dev_s *priv = uart_devs[CONSOLE_UART - 1];
uint16_t ie;
// up_disableusartint(priv, &ie);
up_disableusartint(priv, &ie);
/* Check for LF */
@ -1960,7 +2040,7 @@ int up_putc(int ch)
}
up_lowputc(ch);
// up_restoreusartint(priv, ie);
up_restoreusartint(priv, ie);
#endif
return ch;
}

12
nuttx/configs/px4io/include/board.h
View File

@ -96,6 +96,18 @@
#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY)
/*
* Some of the USART pins are not available; override the GPIO
* definitions with an invalid pin configuration.
*/
#define GPIO_USART2_CTS 0xffffffff
#define GPIO_USART2_RTS 0xffffffff
#define GPIO_USART2_CK 0xffffffff
#define GPIO_USART3_TX 0xffffffff
#define GPIO_USART3_CK 0xffffffff
#define GPIO_USART3_CTS 0xffffffff
#define GPIO_USART3_RTS 0xffffffff
/*
* High-resolution timer
*/

22
nuttx/configs/px4io/io/defconfig
View File

@ -124,7 +124,7 @@ CONFIG_STM32_TIM7=n
CONFIG_STM32_WWDG=n
CONFIG_STM32_SPI2=n
CONFIG_STM32_USART2=y
CONFIG_STM32_USART3=n
CONFIG_STM32_USART3=y
CONFIG_STM32_I2C1=y
CONFIG_STM32_I2C2=n
CONFIG_STM32_BKP=n
@ -161,13 +161,13 @@ CONFIG_USART1_SERIAL_CONSOLE=y
CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_USART1_TXBUFSIZE=32
CONFIG_USART2_TXBUFSIZE=32
CONFIG_USART3_TXBUFSIZE=32
CONFIG_USART1_TXBUFSIZE=64
CONFIG_USART2_TXBUFSIZE=64
CONFIG_USART3_TXBUFSIZE=64
CONFIG_USART1_RXBUFSIZE=64
CONFIG_USART2_RXBUFSIZE=128
CONFIG_USART3_RXBUFSIZE=32
CONFIG_USART2_RXBUFSIZE=64
CONFIG_USART3_RXBUFSIZE=64
CONFIG_USART1_BAUD=115200
CONFIG_USART2_BAUD=115200
@ -341,26 +341,28 @@ CONFIG_DEBUG_CAN=n
CONFIG_DEBUG_I2C=n
CONFIG_DEBUG_INPUT=n
CONFIG_MSEC_PER_TICK=1
CONFIG_HAVE_CXX=n
CONFIG_HAVE_CXXINITIALIZE=n
CONFIG_MM_REGIONS=1
CONFIG_MM_SMALL=y
CONFIG_ARCH_LOWPUTC=y
CONFIG_RR_INTERVAL=200
CONFIG_RR_INTERVAL=0
CONFIG_SCHED_INSTRUMENTATION=n
CONFIG_TASK_NAME_SIZE=0
CONFIG_TASK_NAME_SIZE=8
CONFIG_START_YEAR=1970
CONFIG_START_MONTH=1
CONFIG_START_DAY=1
CONFIG_GREGORIAN_TIME=n
CONFIG_JULIAN_TIME=n
# this eats ~1KiB of RAM ... work out why
CONFIG_DEV_CONSOLE=y
CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=n
CONFIG_SEM_PREALLOCHOLDERS=0
CONFIG_SEM_NNESTPRIO=0
CONFIG_FDCLONE_DISABLE=n
CONFIG_FDCLONE_DISABLE=y
CONFIG_FDCLONE_STDIO=y
CONFIG_SDCLONE_DISABLE=y
CONFIG_SCHED_WORKQUEUE=n
@ -469,7 +471,7 @@ CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=8
CONFIG_NFILE_STREAMS=0
CONFIG_NAME_MAX=32
CONFIG_STDIO_BUFFER_SIZE=64
CONFIG_STDIO_BUFFER_SIZE=32
CONFIG_STDIO_LINEBUFFER=n
CONFIG_NUNGET_CHARS=2
CONFIG_PREALLOC_MQ_MSGS=4

209
nuttx/drivers/serial/serial.c
View File

@ -252,7 +252,7 @@ static inline ssize_t uart_irqwrite(FAR uart_dev_t *dev, FAR const char *buffer,
{
int ch = *buffer++;
/* If this is the console, then we should replace LF with CR-LF */
/* assume that this is console text output and always do \n -> \r\n conversion */
if (ch == '\n')
{
@ -277,6 +277,7 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t
FAR uart_dev_t *dev = inode->i_private;
ssize_t nread = buflen;
int ret;
char ch;
/* We may receive console writes through this path from interrupt handlers and
* from debug output in the IDLE task! In these cases, we will need to do things
@ -308,8 +309,7 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t
if (ret < 0)
{
/* A signal received while waiting for access to the xmit.head will
* abort the transfer. After the transfer has started, we are committed
* and signals will be ignored.
* abort the transfer.
*/
return ret;
@ -323,24 +323,74 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t
uart_disabletxint(dev);
for (; buflen; buflen--)
{
int ch = *buffer++;
ch = *buffer++;
/* If the ONLCR flag is set, we should translate \n to \r\n */
/* Do output post-processing */
ret = OK;
if ((ch == '\n') && (dev->termios_s.c_oflag & ONLCR))
#ifdef CONFIG_SERIAL_TERMIOS
if (dev->tc_oflag & OPOST)
{
/* Mapping CR to NL? */
if ((ch == '\r') && (dev->tc_oflag & OCRNL))
{
ch = '\n';
}
/* Are we interested in newline processing? */
if ((ch == '\n') && (dev->tc_oflag & (ONLCR | ONLRET)))
{
ret = uart_putxmitchar(dev, '\r');
if (ret != OK)
{
break;
}
}
/* Specifically not handled:
*
* OXTABS - primarily a full-screen terminal optimisation
* ONOEOT - Unix interoperability hack
* OLCUC - Not specified by Posix
* ONOCR - low-speed interactive optimisation
*/
}
#else /* !CONFIG_SERIAL_TERMIOS */
/* If this is the console, convert \n -> \r\n */
if (dev->isconsole && ch == '\n')
{
ret = uart_putxmitchar(dev, '\r');
}
#endif
/* Put the character into the transmit buffer */
if (ret == OK)
{
ret = uart_putxmitchar(dev, ch);
if (ret != OK)
{
break;
}
/* Were we awakened by a signal? That should be the only condition that
}
if (dev->xmit.head != dev->xmit.tail)
{
uart_enabletxint(dev);
}
uart_givesem(&dev->xmit.sem);
/* Were we interrupted by a signal? That should be the only condition that
* uart_putxmitchar() should return an error.
*/
@ -367,17 +417,8 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t
nread = -EINTR;
}
break;
}
}
if (dev->xmit.head != dev->xmit.tail)
{
uart_enabletxint(dev);
}
uart_givesem(&dev->xmit.sem);
return nread;
}
@ -393,6 +434,7 @@ static ssize_t uart_read(FAR struct file *filep, FAR char *buffer, size_t buflen
ssize_t recvd = 0;
int16_t tail;
int ret;
char ch;
/* Only one user can access dev->recv.tail at a time */
@ -430,8 +472,7 @@ static ssize_t uart_read(FAR struct file *filep, FAR char *buffer, size_t buflen
{
/* Take the next character from the tail of the buffer */
*buffer++ = dev->recv.buffer[tail];
recvd++;
ch = dev->recv.buffer[tail];
/* Increment the tail index. Most operations are done using the
* local variable 'tail' so that the final dev->recv.tail update
@ -444,6 +485,49 @@ static ssize_t uart_read(FAR struct file *filep, FAR char *buffer, size_t buflen
}
dev->recv.tail = tail;
#ifdef CONFIG_SERIAL_TERMIOS
/* Do input processing if any is enabled */
if (dev->tc_iflag & (INLCR | IGNCR | ICRNL))
{
/* \n -> \r or \r -> \n translation? */
if ((ch == '\n') && (dev->tc_iflag & INLCR))
{
ch = '\r';
}
else if ((ch == '\r') && (dev->tc_iflag & ICRNL))
{
ch = '\n';
}
/* discarding \r ? */
if ((ch == '\r') & (dev->tc_iflag & IGNCR))
{
continue;
}
}
/* Specifically not handled:
*
* All of the local modes; echo, line editing, etc.
* Anything to do with break or parity errors.
* ISTRIP - we should be 8-bit clean.
* IUCLC - Not Posix
* IXON/OXOFF - no xon/xoff flow control.
*/
#endif
/* store the received character */
*buffer++ = ch;
recvd++;
}
#ifdef CONFIG_DEV_SERIAL_FULLBLOCKS
@ -573,10 +657,38 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
/* Handle TTY-level IOCTLs here */
/* Let low-level driver handle the call first */
int ret = dev->ops->ioctl(filep, cmd, arg);
/* Append any higher level TTY flags */
if (ret == OK)
{
switch (cmd)
{
case FIONREAD:
{
int count;
irqstate_t state = irqsave();
/* determine the number of bytes available in the buffer */
if (dev->recv.tail <= dev->recv.head)
{
count = dev->recv.head - dev->recv.tail;
}
else
{
count = dev->recv.size - (dev->recv.tail - dev->recv.head);
}
irqrestore(state);
*(int *)arg = count;
}
#ifdef CONFIG_SERIAL_TERMIOS
case TCGETS:
{
struct termios *termiosp = (struct termios*)arg;
@ -587,11 +699,13 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
break;
}
/* Fetch the out flags */
termiosp->c_oflag = dev->termios_s.c_oflag;
/* Fetch the in flags */
termiosp->c_iflag = dev->termios_s.c_iflag;
/* and update with flags from this layer */
termiosp->c_iflag = dev->tc_iflag;
termiosp->c_oflag = dev->tc_oflag;
termiosp->c_lflag = dev->tc_lflag;
}
break;
case TCSETS:
@ -604,12 +718,16 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
break;
}
/* Set the out flags */
dev->termios_s.c_oflag = termiosp->c_oflag;
/* Set the in flags */
dev->termios_s.c_iflag = termiosp->c_iflag;
/* update the flags we keep at this layer */
dev->tc_iflag = termiosp->c_iflag;
dev->tc_oflag = termiosp->c_oflag;
dev->tc_lflag = termiosp->c_lflag;
}
break;
#endif
}
}
return ret;
}
@ -900,6 +1018,25 @@ static int uart_open(FAR struct file *filep)
dev->recv.head = 0;
dev->recv.tail = 0;
/* initialise termios state */
#ifdef CONFIG_SERIAL_TERMIOS
dev->tc_iflag = 0;
if (dev->isconsole == true)
{
/* enable \n -> \r\n translation for the console */
dev->tc_oflag = OPOST | ONLCR;
}
else
{
dev->tc_oflag = 0;
}
#endif
/* Enable the RX interrupt */
uart_enablerxint(dev);
@ -937,14 +1074,22 @@ int uart_register(FAR const char *path, FAR uart_dev_t *dev)
#ifndef CONFIG_DISABLE_POLL
sem_init(&dev->pollsem, 0, 1);
#endif
/* Setup termios flags */
memset(&dev->termios_s, 0, sizeof(dev->termios_s));
#ifdef CONFIG_SERIAL_TERMIOS
if (dev->isconsole == true)
{
/* Device is console, set up termios flags */
dev->termios_s.c_oflag |= ONLCR;
/* enable \n -> \r\n translation for the console as early as possible */
dev->tc_oflag = OPOST | ONLCR;
dev->tc_iflag = 0;
}
#endif
dbg("Registering %s\n", path);
return register_driver(path, &g_serialops, 0666, dev);
}

4
nuttx/include/nuttx/fs/ioctl.h
View File

@ -106,6 +106,10 @@
* OUT: None
*/
#define FIONREAD _FIOC(0x0004) /* IN: Location to return value (int *)
* OUT: Bytes readable from this fd
*/
/* NuttX file system ioctl definitions **************************************/
#define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE)

9
nuttx/include/nuttx/serial/serial.h
View File

@ -46,7 +46,9 @@
#include <stdint.h>
#include <stdbool.h>
#include <semaphore.h>
#ifdef CONFIG_SERIAL_TERMIOS
# include <termios.h>
#endif
#include <nuttx/fs/fs.h>
/************************************************************************************
@ -216,7 +218,12 @@ struct uart_dev_s
#endif
/* Terminal control flags */
struct termios termios_s;
#ifdef CONFIG_SERIAL_TERMIOS
tcflag_t tc_iflag; /* Input modes */
tcflag_t tc_oflag; /* Output modes */
tcflag_t tc_lflag; /* Local modes */
#endif
};
typedef struct uart_dev_s uart_dev_t;

10
nuttx/include/termios.h
View File

@ -58,7 +58,7 @@
#define INLCR (1 << 5) /* Bit 5: Map NL to CR on input */
#define INPCK (1 << 6) /* Bit 6: Enable input parity check */
#define ISTRIP (1 << 7) /* Bit 7: Strip character */
#define IUCLC (1 << 8) /* Bit 8: Map upper-case to lower-case on input (LEGACY) */
/* Bit 8: unused */
#define IXANY (1 << 9) /* Bit 9: Enable any character to restart output */
#define IXOFF (1 << 10) /* Bit 10: Enable start/stop input control */
#define IXON (1 << 11) /* Bit 11: Enable start/stop output control */
@ -67,7 +67,7 @@
/* Terminal output modes (c_oflag in the termios structure) */
#define OPOST (1 << 0) /* Bit 0: Post-process output */
#define OLCUC (1 << 1) /* Bit 1: Map lower-case to upper-case on output (LEGACY) */
/* Bit 1: unused */
#define ONLCR (1 << 2) /* Bit 2: Map NL to CR-NL on output */
#define OCRNL (1 << 3) /* Bit 3: Map CR to NL on output */
#define ONOCR (1 << 4) /* Bit 4: No CR output at column 0 */
@ -109,6 +109,9 @@
#define PARODD (1 << 5) /* Bit 5: Odd parity, else even */
#define HUPCL (1 << 6) /* Bit 6: Hang up on last close */
#define CLOCAL (1 << 7) /* Bit 7: Ignore modem status lines */
#define CCTS_OFLOW (1 << 8) /* Bit 8: CTS flow control of output */
#define CRTSCTS CCTS_OFLOW
#define CRTS_IFLOW (1 << 9) /* Bit 9: RTS flow control of input */
/* Local Modes (c_lflag in the termios structure) */
@ -121,7 +124,6 @@
#define ISIG (1 << 6) /* Bit 6: Enable signals */
#define NOFLSH (1 << 7) /* Bit 7: Disable flush after interrupt or quit */
#define TOSTOP (1 << 8) /* Bit 8: Send SIGTTOU for background output */
#define XCASE (1 << 9) /* Bit 9: Canonical upper/lower presentation (LEGACY) */
/* The following are subscript names for the termios c_cc array */
@ -230,7 +232,7 @@ struct termios
* cf[set|get][o|i]speed() POSIX interfaces.
*/
const speed_t c_speed; /* Input/output speed (non-POSIX)*/
speed_t c_speed; /* Input/output speed (non-POSIX)*/
};
/****************************************************************************