zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_RCProtocol: convert SUMD and ST24 to SoftSerial

master
Andrew Tridgell 6 years ago
parent
408db791e3
commit
b1e5e0aba9
4 changed files with 34 additions and 135 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. 75
      libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp
  2. 8
      libraries/AP_RCProtocol/AP_RCProtocol_ST24.h
  3. 75
      libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
  4. 11
      libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h

75
libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp
Unescape View File

@ -77,73 +77,14 @@ uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len) @@ -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) @@ -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
Unescape View File

@ -18,6 +18,7 @@ @@ -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: @@ -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: @@ -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};
};

75
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
Unescape View File

@ -84,72 +84,14 @@ uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value) @@ -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) @@ -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
Unescape View File

@ -18,6 +18,8 @@ @@ -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: @@ -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: @@ -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};
};

Write Preview
Loading…
Cancel
Save