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

Revert "Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs." 

This reverts commit ff3a014971.
This commit is contained in:
Julian Oes 2012-11-07 14:46:28 -08:00
parent 25ed791b70
commit 38b20f2a1a
8 changed files with 213 additions and 377 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
apps/drivers/px4io/px4io.cpp
View File

@ -743,15 +743,15 @@ px4io_main(int argc, char *argv[])
return ret;
}
if (!strcmp(argv[1], "rx_dsm_10bit")) {
if (!strcmp(argv[1], "rx_spektrum6")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_10BIT);
g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6);
}
if (!strcmp(argv[1], "rx_dsm_11bit")) {
if (!strcmp(argv[1], "rx_spektrum7")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_11BIT);
g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7);
}
if (!strcmp(argv[1], "rx_sbus")) {
if (g_dev == nullptr)
@ -763,5 +763,5 @@ px4io_main(int argc, char *argv[])
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_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'");
}

119
apps/px4io/comms.c
View File

@ -44,7 +44,6 @@
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <termios.h>
#include <string.h>
#include <poll.h>
@ -61,126 +60,26 @@
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
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);
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);
}
void
comms_check(void)
{
/*
* Check for serial data
*/
int ret = poll(pollfds, pollcount, 0);
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) {
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.
*/
static hrt_abstime last_report_time;
hrt_abstime now, delta;
@ -188,7 +87,7 @@ comms_check(void)
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 || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
last_report_time = now;
@ -202,6 +101,15 @@ comms_check(void)
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
/*
* Check for bytes and feed them to the RX engine.
* Limit the number of bytes we actually process on any one iteration.
*/
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]);
}
int frame_rx;
@ -219,7 +127,8 @@ comms_handle_config(const void *buffer, size_t length)
frame_rx++;
serial_rx_init(cfg->serial_rx_mode);
mixer_set_serial_mode(cfg->serial_rx_mode);
}
static void

184
apps/px4io/dsm.c
View File

@ -1,184 +0,0 @@
/****************************************************************************
*
* 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 dsm.c
*
* Serial protocol decoder for the Spektrum DSM* family of protocols.
*
* Decodes into the global PPM buffer and updates accordingly.
*/
#include <nuttx/config.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
#include "protocol.h"
#define DSM_FRAME_SIZE 16
#define DSM_FRAME_CHANNELS 7
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);
void
dsm_init(unsigned mode)
{
insync = false;
partial_frame_count = 0;
if (mode == RX_MODE_DSM_10BIT) {
channel_shift = 10;
} else {
channel_shift = 11;
}
last_frame_time = hrt_absolute_time();
}
void
dsm_input(int fd)
{
uint8_t buf[DSM_FRAME_SIZE];
ssize_t ret;
hrt_abstime now;
/*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 11ms; with 16 bytes at 115200bps
* frame transmission time is ~1.4ms.
*
* 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.
*/
now = hrt_absolute_time();
if ((now - last_frame_time) > 5000)
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);
/* if the read failed for any reason, just give up here */
if (ret < 1)
return;
/*
* Add bytes to the current frame
*/
memcpy(&frame[partial_frame_count], buf, ret);
partial_frame_count += ret;
/*
* If we don't have a full frame, return
*/
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();
partial_frame_count = 0;
}
static void
dsm_decode(void)
{
uint16_t data_mask = (1 << channel_shift) - 1;
/*
* 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.
*
* Each channel is a 16-bit unsigned value containing either a 10-
* or 11-bit channel value and a 4-bit channel number, shifted
* either 10 or 11 bits. The MSB may also be set to indicate the
* second frame in variants of the protocol where more than
* seven channels are being transmitted.
*/
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
/* ignore pad channels */
if (raw == 0xffff)
continue;
unsigned channel = (raw >> channel_shift) & 0xf;
/* ignore channels out of range */
if (channel >= PX4IO_INPUT_CHANNELS)
continue;
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;
}
ppm_last_valid_decode = hrt_absolute_time();
}

199
apps/px4io/mixer.c
View File

@ -32,9 +32,7 @@
****************************************************************************/
/**
* @file mixer.c
*
* Control channel input/output mixer and failsafe.
* @file Control channel input/output mixer and failsafe.
*/
#include <nuttx/config.h>
@ -45,6 +43,8 @@
#include <string.h>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
@ -55,12 +55,23 @@
#include "px4io.h"
/*
* Count of periodic calls in which we have no data.
*/
static unsigned mixer_input_drops;
#define MIXER_INPUT_DROP_LIMIT 10
/*
* Count of periodic calls in which we have no FMU input.
*/
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.
*/
@ -95,7 +106,8 @@ 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);
@ -103,6 +115,37 @@ mixer_init(void)
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)
@ -191,20 +234,148 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
}
}
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)
{
bool got_input = false;
/* 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;
return;
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;
}
/* 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;
if (got_input) {
mixer_input_drops = 0;
system_state.fmu_report_due = true;
} else {
/*
* No data; count the 'frame drops' and once we hit the limit
* assume that we have lost input.
*/
if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) {
mixer_input_drops++;
/* if we hit the limit, stop pretending we have input and let the FMU know */
if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) {
system_state.rc_channels = 0;
system_state.fmu_report_due = true;
}
}
}
}

9
apps/px4io/protocol.h
View File

@ -41,6 +41,11 @@
#pragma once
/*
* XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
* TREES ARE MERGED.
*/
#define PX4IO_OUTPUT_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
@ -64,8 +69,8 @@ struct px4io_config {
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_SPEKTRUM_6 1
#define RX_MODE_SPEKTRUM_7 2
#define RX_MODE_FUTABA_SBUS 3
};

2
apps/px4io/px4io.c
View File

@ -72,7 +72,7 @@ int user_start(int argc, char *argv[])
/* configure the high-resolution time/callout interface */
hrt_init();
/* init the FMU and receiver links */
/* init the FMU link */
comms_init();
/* configure the first 8 PWM outputs (i.e. all of them) */

9
apps/px4io/px4io.h
View File

@ -147,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.
@ -159,14 +160,6 @@ extern void safety_init(void);
extern void comms_init(void);
extern void comms_check(void);
/*
* Serial receiver decoders.
*/
extern void dsm_init(unsigned mode);
extern void dsm_input(int fd);
extern void sbus_init(unsigned mode);
extern void sbus_input(int fd);
/*
* Assertion codes
*/

58
apps/px4io/sbus.c
View File

@ -1,58 +0,0 @@
/****************************************************************************
*
* 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 sbus.c
*
* Serial protocol decoder for the Futaba S.bus protocol.
*/
#include <nuttx/config.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
#include "protocol.h"
void
sbus_init(unsigned mode)
{
}
void
sbus_input(int fd)
{
}