zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_RCProtocol: use SoftSerial for DSM

mission-4.1.18
Andrew Tridgell 6 years ago
parent
b93dee7fe6
commit
3bd27ba094
2 changed files with 30 additions and 92 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 108
      libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
  2. 14
      libraries/AP_RCProtocol/AP_RCProtocol_DSM.h

108
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
Unescape View File

@ -34,79 +34,10 @@ extern const AP_HAL::HAL& hal; @@ -34,79 +34,10 @@ extern const AP_HAL::HAL& hal;
void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
// convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps
uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000;
uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000;
uint8_t bit_ofs, byte_ofs;
uint16_t nbits;
// maintain time using pulse widths
clock_us += (width_s0 + width_s1);
if (bits_s0 == 0 || bits_s1 == 0) {
// invalid data
goto reset;
uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(ss.get_byte_timestamp_us(), b);
}
byte_ofs = dsm_state.bit_ofs/10;
bit_ofs = dsm_state.bit_ofs%10;
if (byte_ofs > 15) {
// invalid data
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
nbits = 10 - bit_ofs;
}
dsm_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
dsm_state.bit_ofs += nbits;
bit_ofs += nbits;
if (bits_s0 - nbits > 10) {
if (dsm_state.bit_ofs == 16*10) {
// we have a full frame
uint8_t bytes[16];
uint8_t i;
for (i=0; i<16; i++) {
// get raw data
uint16_t v = dsm_state.bytes[i];
// check start bit
if ((v & 1) != 0) {
goto reset;
}
// check stop bits
if ((v & 0x200) != 0x200) {
goto reset;
}
bytes[i] = ((v>>1) & 0xFF);
}
uint16_t values[8];
uint16_t num_values=0;
if (dsm_decode(clock_us, bytes, values, &num_values, 8) &&
num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, false);
}
}
memset(&dsm_state, 0, sizeof(dsm_state));
}
byte_ofs = dsm_state.bit_ofs/10;
bit_ofs = dsm_state.bit_ofs%10;
if (bits_s1+bit_ofs > 10) {
// invalid data
goto reset;
}
// pull in the low bits
dsm_state.bit_ofs += bits_s1;
return;
reset:
memset(&dsm_state, 0, sizeof(dsm_state));
}
/**
@ -158,10 +89,6 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne @@ -158,10 +89,6 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne
*/
void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16])
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
cs10 = 0;
@ -206,7 +133,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] @@ -206,7 +133,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static uint32_t masks[] = {
static const uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
@ -253,16 +180,16 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] @@ -253,16 +180,16 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values)
{
#if 0
#if 1
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
#endif
/*
* If we have lost signal for at least a second, reset the
* If we have lost signal for at least 200ms, reset the
* format guessing heuristic.
*/
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) {
if (((frame_time - dsm_last_frame_time) > 200000U) && (dsm_channel_shift != 0)) {
dsm_guess_format(true, dsm_frame);
}
@ -445,22 +372,18 @@ void AP_RCProtocol_DSM::update(void) @@ -445,22 +372,18 @@ void AP_RCProtocol_DSM::update(void)
}
// support byte input
void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, uint8_t b)
{
if (baudrate != 115200) {
return;
}
uint32_t now = AP_HAL::millis();
if (now - byte_input.last_byte_ms > 3 ||
if (timestamp_us - byte_input.last_byte_us > 3000U ||
byte_input.ofs == sizeof(byte_input.buf)) {
byte_input.ofs = 0;
}
byte_input.last_byte_ms = now;
byte_input.last_byte_us = timestamp_us;
byte_input.buf[byte_input.ofs++] = b;
if (byte_input.ofs == 16) {
uint16_t values[8];
uint16_t num_values=0;
if (dsm_decode(AP_HAL::micros(), byte_input.buf, values, &num_values, 8) &&
if (dsm_decode(timestamp_us, byte_input.buf, values, &num_values, 8) &&
num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, false);
}
@ -468,3 +391,12 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) @@ -468,3 +391,12 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
byte_input.ofs = 0;
}
}
// support byte input
void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(AP_HAL::micros(), b);
}

14
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
Unescape View File

@ -18,6 +18,7 @@ @@ -18,6 +18,7 @@
#pragma once
#include "AP_RCProtocol.h"
#include "SoftSerial.h"
class AP_RCProtocol_DSM : public AP_RCProtocol_Backend {
public:
@ -28,6 +29,7 @@ public: @@ -28,6 +29,7 @@ public:
void update(void) override;
private:
void _process_byte(uint32_t timestamp_us, uint8_t byte);
void dsm_decode();
bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]);
@ -42,6 +44,11 @@ private: @@ -42,6 +44,11 @@ private:
uint16_t bit_ofs;
} dsm_state;
// format guessing state
uint32_t cs10;
uint32_t cs11;
unsigned samples;
// bind state machine
enum {
BIND_STATE_NONE,
@ -52,12 +59,11 @@ private: @@ -52,12 +59,11 @@ private:
} bind_state;
uint32_t bind_last_ms;
// sum of clock pulses
uint32_t clock_us;
struct {
uint8_t buf[16];
uint8_t ofs;
uint32_t last_byte_ms;
uint32_t last_byte_us;
} byte_input;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
};

Write Preview
Loading…
Cancel
Save