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

Merged master into ekf2

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-24 09:53:54 +01:00
parent b958928095 45e4def3eb
commit d3149153f6
24 changed files with 1606 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rc/dsm.c
+251 -111
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@@ -40,7 +40,8 @@
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <board_config.h>
#include <px4_defines.h>
#include <fcntl.h>
#include <unistd.h>
@@ -49,27 +50,32 @@
#include "dsm.h"
#include <drivers/drv_hrt.h>
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#include <px4io.h>
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
#define dsm_udelay(arg) usleep(arg)
#else
#include <nuttx/arch.h>
#define dsm_udelay(arg) up_udelay(arg)
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#include <px4io.h>
#endif
//#define DSM_DEBUG
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
static enum DSM_DECODE_STATE {
DSM_DECODE_STATE_DESYNC = 0,
DSM_DECODE_STATE_SYNC
} dsm_decode_state = DSM_DECODE_STATE_DESYNC;
static int dsm_fd = -1; /**< File handle to the DSM UART */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received data */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last valid dsm frame */
static uint8_t dsm_frame[DSM_BUFFER_SIZE]; /**< DSM dsm frame receive buffer */
static uint8_t dsm_buf[DSM_FRAME_SIZE * 2];
static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
static unsigned dsm_channel_shift = 0; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops = 0; /**< Count of incomplete DSM frames */
static uint16_t dsm_chan_count = 0; /**< DSM channel count */
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values);
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values);
/**
* Attempt to decode a single channel raw channel datum
@@ -119,12 +125,12 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
static void
static bool
dsm_guess_format(bool reset)
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
static uint32_t cs10 = 0;
static uint32_t cs11 = 0;
static unsigned samples = 0;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
@@ -132,7 +138,7 @@ dsm_guess_format(bool reset)
cs11 = 0;
samples = 0;
dsm_channel_shift = 0;
return;
return false;
}
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
@@ -154,9 +160,16 @@ dsm_guess_format(bool reset)
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
}
samples++;
#ifdef DSM_DEBUG
printf("dsm guess format: samples: %d %s\n", samples,
(reset) ? "RESET" : "");
#endif
/* wait until we have seen plenty of frames - 5 should normally be enough */
if (samples++ < 5) {
return;
if (samples < 5) {
return false;
}
/*
@@ -195,40 +208,37 @@ dsm_guess_format(bool reset)
if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11;
//debug("DSM: 11-bit format");
return;
#ifdef DSM_DEBUG
printf("DSM: 11-bit format\n");
#endif
return true;
}
if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10;
//debug("DSM: 10-bit format");
return;
#ifdef DSM_DEBUG
printf("DSM: 10-bit format\n");
#endif
return true;
}
/* call ourselves to reset our state ... we have to try again */
//debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
#ifdef DSM_DEBUG
printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11);
#endif
dsm_guess_format(true);
return false;
}
/**
* Initialize the DSM receive functionality
*
* Open the UART for receiving DSM frames and configure it appropriately
*
* @param[in] device Device name of DSM UART
*/
int
dsm_init(const char *device)
dsm_config(int dsm_fd)
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#ifdef GPIO_SPEKTRUM_PWR_EN
// enable power on DSM connector
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
int ret = -1;
if (dsm_fd >= 0) {
@@ -247,17 +257,43 @@ dsm_init(const char *device)
/* reset the format detector */
dsm_guess_format(true);
//debug("DSM: ready");
} else {
//debug("DSM: open failed");
ret = 0;
}
return dsm_fd;
return ret;
}
/**
* Initialize the DSM receive functionality
*
* Open the UART for receiving DSM frames and configure it appropriately
*
* @param[in] device Device name of DSM UART
*/
int
dsm_init(const char *device)
{
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
dsm_channel_shift = 0;
dsm_frame_drops = 0;
dsm_chan_count = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
int ret = dsm_config(dsm_fd);
if (!ret) {
return dsm_fd;
} else {
return -1;
}
}
#ifdef GPIO_SPEKTRUM_PWR_EN
/**
* Handle DSM satellite receiver bind mode handler
*
@@ -267,64 +303,52 @@ dsm_init(const char *device)
void
dsm_bind(uint16_t cmd, int pulses)
{
#if !defined(GPIO_USART1_RX_SPEKTRUM)
#else
if (dsm_fd < 0) {
return;
}
switch (cmd) {
case dsm_bind_power_down:
case DSM_CMD_BIND_POWER_DOWN:
/*power down DSM satellite*/
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(0);
#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(0);
#endif
break;
case dsm_bind_power_up:
case DSM_CMD_BIND_POWER_UP:
/*power up DSM satellite*/
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(1);
#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(1);
#endif
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
case DSM_CMD_BIND_SET_RX_OUT:
/*Set UART RX pin to active output mode*/
stm32_configgpio(GPIO_USART1_RX_SPEKTRUM);
SPEKTRUM_RX_AS_GPIO();
break;
case dsm_bind_send_pulses:
case DSM_CMD_BIND_SEND_PULSES:
/*Pulse RX pin a number of times*/
for (int i = 0; i < pulses; i++) {
up_udelay(120);
stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, false);
up_udelay(120);
stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true);
dsm_udelay(120);
SPEKTRUM_RX_HIGH(false);
dsm_udelay(120);
SPEKTRUM_RX_HIGH(true);
}
break;
case dsm_bind_reinit_uart:
case DSM_CMD_BIND_REINIT_UART:
/*Restore USART RX pin to RS232 receive mode*/
stm32_configgpio(GPIO_USART1_RX);
SPEKTRUM_RX_AS_UART();
break;
}
#endif
}
#endif
/**
* Decode the entire dsm frame (all contained channels)
@@ -335,7 +359,7 @@ dsm_bind(uint16_t cmd, int pulses)
* @return true=DSM frame successfully decoded, false=no update
*/
bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values)
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values)
{
/*
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
@@ -350,13 +374,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
dsm_guess_format(true);
}
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
/* if we don't know the dsm_frame format, update the guessing state machine */
if (dsm_channel_shift == 0) {
dsm_guess_format(false);
return false;
if (!dsm_guess_format(false)) {
return false;
}
}
/*
@@ -380,6 +402,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
continue;
}
/* reset bit guessing state machine if the channel index is out of bounds */
if (channel > DSM_MAX_CHANNEL_COUNT) {
dsm_guess_format(true);
return false;
}
/* ignore channels out of range */
if (channel >= max_values) {
continue;
@@ -447,14 +475,32 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
*num_values = 12;
}
if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */
*num_values |= 0x8000;
}
/* Set the 11-bit data indicator */
*dsm_11_bit = (dsm_channel_shift == 11);
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
#ifdef DSM_DEBUG
printf("PARSED PACKET\n");
#endif
/* check all values */
for (unsigned i = 0; i < *num_values; i++) {
/* if the value is unrealistic, fail the parsing entirely */
if (values[i] < 600 || values[i] > 2400) {
#ifdef DSM_DEBUG
printf("DSM: VALUE RANGE FAIL\n");
#endif
dsm_chan_count = 0;
return false;
}
}
return true;
}
@@ -480,27 +526,35 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values)
dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes,
unsigned max_values)
{
ssize_t ret;
int ret = 1;
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 - dsm_last_rx_time) > 5000) {
if (dsm_partial_frame_count > 0) {
dsm_frame_drops++;
dsm_partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current dsm frame.
* a complete frame.
*/
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0]));
/* if the read failed for any reason, just give up here */
if (ret < 1) {
@@ -508,27 +562,113 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by
} else {
*n_bytes = ret;
*bytes = &dsm_frame[dsm_partial_frame_count];
*bytes = &dsm_buf[0];
}
/*
* Try to decode something with what we got
*/
return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values);
}
bool
dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
{
/* this is set by the decoding state machine and will default to false
* once everything that was decodable has been decoded.
*/
bool decode_ret = false;
/* keep decoding until we have consumed the buffer */
for (unsigned d = 0; d < len; d++) {
/* overflow check */
if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) {
dsm_partial_frame_count = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
#ifdef DSM_DEBUG
printf("DSM: RESET (BUF LIM)\n");
#endif
}
if (dsm_partial_frame_count == DSM_FRAME_SIZE) {
dsm_partial_frame_count = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
#ifdef DSM_DEBUG
printf("DSM: RESET (PACKET LIM)\n");
#endif
}
#ifdef DSM_DEBUG
#if 1
printf("dsm state: %s%s, count: %d, val: %02x\n",
(dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
(dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
dsm_partial_frame_count,
(unsigned)frame[d]);
#endif
#endif
switch (dsm_decode_state) {
case DSM_DECODE_STATE_DESYNC:
/* we are de-synced and only interested in the frame marker */
if ((now - dsm_last_rx_time) > 5000) {
dsm_decode_state = DSM_DECODE_STATE_SYNC;
dsm_partial_frame_count = 0;
dsm_chan_count = 0;
dsm_frame[dsm_partial_frame_count++] = frame[d];
}
break;
case DSM_DECODE_STATE_SYNC: {
dsm_frame[dsm_partial_frame_count++] = frame[d];
/* decode whatever we got and expect */
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
break;
}
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels);
/* we consumed the partial frame, reset */
dsm_partial_frame_count = 0;
/* if decoding failed, set proto to desync */
if (decode_ret == false) {
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
dsm_frame_drops++;
}
}
break;
default:
#ifdef DSM_DEBUG
printf("UNKNOWN PROTO STATE");
#endif
decode_ret = false;
}
}
if (frame_drops) {
*frame_drops = dsm_frame_drops;
}
if (decode_ret) {
*num_values = dsm_chan_count;
}
dsm_last_rx_time = now;
/*
* Add bytes to the current dsm frame
*/
dsm_partial_frame_count += ret;
/*
* If we don't have a full dsm frame, return
*/
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a dsm frame. Go ahead and
* decode it.
*/
dsm_partial_frame_count = 0;
return dsm_decode(now, values, num_values, max_values);
/* return false as default */
return decode_ret;
}
src/lib/rc/dsm.h
+25 -1
View File
@@ -42,11 +42,35 @@
#pragma once
#include <stdint.h>
#include <px4_config.h>
#include <board_config.h>
#include <px4_defines.h>
__BEGIN_DECLS
#define DSM_FRAME_SIZE 16 /**< DSM frame size in bytes */
#define DSM_FRAME_CHANNELS 7 /**< Max supported DSM channels per frame */
#define DSM_MAX_CHANNEL_COUNT 18 /**< Max channel count of any DSM RC */
#define DSM_BUFFER_SIZE (DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2)
__EXPORT int dsm_init(const char *device);
__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values);
__EXPORT int dsm_config(int dsm_fd);
__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes,
uint8_t **bytes, unsigned max_values);
__EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels);
#ifdef GPIO_SPEKTRUM_PWR_EN
__EXPORT void dsm_bind(uint16_t cmd, int pulses);
#endif
enum DSM_CMD { /* DSM bind states */
DSM_CMD_BIND_POWER_DOWN = 0,
DSM_CMD_BIND_POWER_UP,
DSM_CMD_BIND_SET_RX_OUT,
DSM_CMD_BIND_SEND_PULSES,
DSM_CMD_BIND_REINIT_UART
};
__END_DECLS
src/lib/rc/sbus.c
+24 -3
View File
@@ -44,6 +44,10 @@
#include <termios.h>
#include <string.h>
#ifdef TIOCSSINGLEWIRE
#include <sys/ioctl.h>
#endif
#include "sbus.h"
#include <drivers/drv_hrt.h>
@@ -122,6 +126,21 @@ sbus_init(const char *device, bool singlewire)
{
int sbus_fd = open(device, O_RDWR | O_NONBLOCK);
int ret = sbus_config(sbus_fd, singlewire);
if (!ret) {
return sbus_fd;
} else {
return -1;
}
}
int
sbus_config(int sbus_fd, bool singlewire)
{
int ret = -1;
if (sbus_fd >= 0) {
struct termios t;
@@ -133,8 +152,8 @@ sbus_init(const char *device, bool singlewire)
if (singlewire) {
/* only defined in configs capable of IOCTL */
#ifdef SBUS_SERIAL_PORT
//ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#ifdef TIOCSSINGLEWIRE
ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif
}
@@ -143,9 +162,11 @@ sbus_init(const char *device, bool singlewire)
last_rx_time = hrt_absolute_time();
last_frame_time = last_rx_time;
sbus_frame_drops = 0;
ret = 0;
}
return sbus_fd;
return ret;
}
void
src/lib/rc/sbus.h
+1
View File
@@ -68,6 +68,7 @@ __EXPORT int sbus_init(const char *device, bool singlewire);
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
__EXPORT int sbus_config(int sbus_fd, bool singlewire);
__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
bool *sbus_frame_drop,
uint16_t max_channels);