Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: convert SUMD and ST24 to SoftSerial

This commit is contained in:
Andrew Tridgell 2018-11-05 15:08:14 +11:00
parent 408db791e3
commit b1e5e0aba9
4 changed files with 34 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp
View File

@ -77,73 +77,14 @@ uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len)
void AP_RCProtocol_ST24::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;
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(b);
}
}
byte_ofs = st24_state.bit_ofs/10;
bit_ofs = st24_state.bit_ofs%10;
if (byte_ofs >= ST24_MAX_FRAMELEN) {
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
nbits = 10 - bit_ofs;
}
st24_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
st24_state.bit_ofs += nbits;
bit_ofs += nbits;
if (bits_s0 - nbits > 10) {
// we have a full frame
uint8_t byte;
uint8_t i;
for (i=0; i <= byte_ofs; i++) {
// get raw data
uint16_t v = st24_state.bytes[i];
// check start bit
if ((v & 1) != 0) {
break;
}
// check stop bits
if ((v & 0x200) != 0x200) {
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte, 115200);
}
memset(&st24_state, 0, sizeof(st24_state));
}
byte_ofs = st24_state.bit_ofs/10;
bit_ofs = st24_state.bit_ofs%10;
if (bits_s1+bit_ofs > 10) {
// invalid data
goto reset;
}
// pull in the low bits
st24_state.bit_ofs += bits_s1;
return;
reset:
memset(&st24_state, 0, sizeof(st24_state));
}
void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
void AP_RCProtocol_ST24::_process_byte(uint8_t byte)
{
if (baudrate != 115200) {
return;
}
switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED:
if (byte == ST24_STX1) {
@ -282,3 +223,11 @@ void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
break;
}
}
void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(byte);
}

8
libraries/AP_RCProtocol/AP_RCProtocol_ST24.h
View File

@ -18,6 +18,7 @@
#pragma once
#include "AP_RCProtocol.h"
#include "SoftSerial.h"
#define ST24_DATA_LEN_MAX 64
#define ST24_MAX_FRAMELEN 70
@ -41,6 +42,7 @@ public:
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
void _process_byte(uint8_t byte);
static uint8_t st24_crc8(uint8_t *ptr, uint8_t len);
enum ST24_PACKET_TYPE {
ST24_PACKET_TYPE_CHANNELDATA12 = 0,
@ -145,9 +147,5 @@ private:
ReceiverFcPacket _rxpacket;
struct {
uint16_t bytes[70];
uint16_t bit_ofs;
bool packet_parsed;
} st24_state;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
};

77
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
View File

@ -84,72 +84,14 @@ uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value)
void AP_RCProtocol_SUMD::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;
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(b);
}
}
byte_ofs = sumd_state.bit_ofs/10;
bit_ofs = sumd_state.bit_ofs%10;
if (byte_ofs >= SUMD_FRAME_MAXLEN) {
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
nbits = 10 - bit_ofs;
}
sumd_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
sumd_state.bit_ofs += nbits;
bit_ofs += nbits;
if (bits_s0 - nbits > 10) {
// we have a full frame
uint8_t byte;
uint8_t i;
for (i=0; i <= byte_ofs; i++) {
// get raw data
uint16_t v = sumd_state.bytes[i];
// check start bit
if ((v & 1) != 0) {
break;
}
// check stop bits
if ((v & 0x200) != 0x200) {
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte, 115200);
}
memset(&sumd_state, 0, sizeof(sumd_state));
}
byte_ofs = sumd_state.bit_ofs/10;
bit_ofs = sumd_state.bit_ofs%10;
if (bits_s1+bit_ofs > 10) {
// invalid data
goto reset;
}
// pull in the low bits
sumd_state.bit_ofs += bits_s1;
return;
reset:
memset(&sumd_state, 0, sizeof(sumd_state));
}
void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
void AP_RCProtocol_SUMD::_process_byte(uint8_t byte)
{
if (baudrate != 115200) {
return;
}
switch (_decode_state) {
case SUMD_DECODE_STATE_UNSYNCED:
#ifdef SUMD_DEBUG
@ -387,3 +329,12 @@ void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
break;
}
}
void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(byte);
}

11
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
View File

@ -18,6 +18,8 @@
#pragma once
#include "AP_RCProtocol.h"
#include "SoftSerial.h"
#define SUMD_MAX_CHANNELS 32
#define SUMD_FRAME_MAXLEN 40
class AP_RCProtocol_SUMD : public AP_RCProtocol_Backend {
@ -25,7 +27,9 @@ public:
AP_RCProtocol_SUMD(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
void _process_byte(uint8_t byte);
static uint16_t sumd_crc16(uint16_t crc, uint8_t value);
static uint8_t sumd_crc8(uint8_t crc, uint8_t value);
@ -61,9 +65,6 @@ private:
uint16_t _crc16 = 0x0000;
bool _sumd = true;
bool _crcOK = false;
struct {
uint16_t bytes[40];
uint16_t bit_ofs;
bool packet_parsed;
} sumd_state;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
};