AFHDS3: better layer separation (transport / protocol logic)

This commit is contained in:
Raphael Coeffic
2022-08-11 12:15:46 +02:00
committed by Raphael Coeffic
parent f75cb3f97d
commit bea7e4ec13
6 changed files with 655 additions and 440 deletions
+324 -2
View File
@@ -20,6 +20,8 @@
*/
#include "afhds3_transport.h"
#include "opentx_helpers.h"
#include "debug.h"
// timer is 2 MHz
#if defined(AFHDS3_SLOW)
@@ -30,9 +32,31 @@
#define BITLEN_AFHDS (17)
#endif
#define MAX_RETRIES_AFHDS3 5
namespace afhds3
{
enum AfhdsSpecialChars {
END = 0xC0, // Frame end
START = END,
ESC_END = 0xDC, // Escaped frame end - in case END occurs in fame then ESC
// ESC_END must be used
ESC = 0xDB, // Escaping character
ESC_ESC = 0xDD, // Escaping character in case ESC occurs in fame then ESC
// ESC_ESC must be used
};
enum DeviceAddress
{
TRANSMITTER = 0x01,
// MODULE = 0x03,
MODULE = 0x05,
};
//Address used in transmitted frames - it constrains of target address and source address
const uint8_t FrameAddress = DeviceAddress::TRANSMITTER | (DeviceAddress::MODULE << 4);
static void _serial_reset(void* buffer)
{
auto data = (SerialData*)buffer;
@@ -105,7 +129,7 @@ uint32_t _pulses_getSize(void* buffer)
return data->ptr - data->pulses;
}
void Transport::init(Type t)
void ByteTransport::init(Type t)
{
if (t == Serial) {
reset = _serial_reset;
@@ -123,6 +147,304 @@ void Transport::init(Type t)
flush = nullptr;
getSize = nullptr;
}
}
}
void FrameTransport::init(ByteTransport::Type t, void* buffer)
{
trsp.init(t);
trsp_buffer = buffer;
clear();
}
void FrameTransport::clear()
{
// reset send buffer
trsp.reset(trsp_buffer);
// reset parser
esc_state = 0;
}
void FrameTransport::sendByte(uint8_t b)
{
trsp.sendByte(trsp_buffer, b);
}
void FrameTransport::putBytes(uint8_t* data, int length)
{
for (int i = 0; i < length; i++) {
uint8_t byte = data[i];
crc += byte;
if (END == byte) {
sendByte(ESC);
sendByte(ESC_END);
}
else if (ESC == byte) {
sendByte(ESC);
sendByte(ESC_ESC);
}
else {
sendByte(byte);
}
}
}
void FrameTransport::putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data,
uint8_t dataLength, uint8_t frameIndex)
{
//header
trsp.reset(trsp_buffer);
crc = 0;
sendByte(START);
uint8_t buffer[] = {FrameAddress, frameIndex, frameType, command};
putBytes(buffer, 4);
//payload
if (dataLength > 0) {
putBytes(data, dataLength);
}
//footer
uint8_t crcValue = crc ^ 0xff;
putBytes(&crcValue, 1);
sendByte(END);
trsp.flush(trsp_buffer);
}
static bool _checkCRC(const uint8_t* data, uint8_t size)
{
uint8_t crc = 0;
//skip start byte
for (uint8_t i = 1; i < size; i++) {
crc += data[i];
}
return (crc ^ 0xff) == data[size];
}
bool FrameTransport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
uint8_t& rxBufferCount,
uint8_t maxSize)
{
if (rxBufferCount == 0 && byte != START) {
TRACE("AFHDS3 [SKIP] %02X", byte);
this->esc_state = 0;
return false;
}
if (byte == ESC) {
this->esc_state = rxBufferCount;
return false;
}
if (rxBufferCount > 1 && byte == END) {
rxBuffer[rxBufferCount++] = byte;
if (!_checkCRC(rxBuffer, rxBufferCount - 2)) {
TRACE("AFHDS3 [INVALID CRC]");
rxBufferCount = 0;
return false;
}
return true;
}
if (this->esc_state && byte == ESC_END) {
byte = END;
}
else if (esc_state && byte == ESC_ESC) {
byte = ESC;
}
//reset esc index
this->esc_state = 0;
if (rxBufferCount >= maxSize) {
TRACE("AFHDS3 [BUFFER OVERFLOW]");
rxBufferCount = 0;
}
rxBuffer[rxBufferCount++] = byte;
return false;
}
void CommandFifo::clearCommandFifo()
{
memclear(commandFifo, sizeof(commandFifo));
setIndex = getIndex = 0;
}
Frame* CommandFifo::getCommand()
{
if (isEmpty()) return nullptr;
return &commandFifo[getIndex];
}
void CommandFifo::enqueueACK(COMMAND command, uint8_t frameNumber)
{
uint32_t next = nextIndex(setIndex);
if (next != getIndex) {
commandFifo[setIndex].command = command;
commandFifo[setIndex].frameType = FRAME_TYPE::RESPONSE_ACK;
commandFifo[setIndex].payload = 0;
commandFifo[setIndex].payloadSize = 0;
commandFifo[setIndex].frameNumber = frameNumber;
commandFifo[setIndex].useFrameNumber = true;
setIndex = next;
}
}
void CommandFifo::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
uint8_t byteContent)
{
uint32_t next = nextIndex(setIndex);
if (next != getIndex) {
commandFifo[setIndex].command = command;
commandFifo[setIndex].frameType = frameType;
commandFifo[setIndex].payload = byteContent;
commandFifo[setIndex].payloadSize = useData ? 1 : 0;
commandFifo[setIndex].frameNumber = 0;
commandFifo[setIndex].useFrameNumber = false;
setIndex = next;
}
}
void Transport::init(ByteTransport::Type t, void* buffer)
{
trsp.init(t, buffer);
}
void Transport::clear()
{
// reset frame
trsp.clear();
// reset command layer
fifo.clearCommandFifo();
frameIndex = 1;
repeatCount = 0;
// reset internal state
operationState = State::UNKNOWN;
}
void Transport::sendFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data,
uint8_t dataLength)
{
operationState = State::SENDING_COMMAND;
repeatCount = 0;
trsp.putFrame(command, frameType, data, dataLength, frameIndex);
frameIndex++;
switch (frameType) {
case FRAME_TYPE::REQUEST_GET_DATA:
case FRAME_TYPE::REQUEST_SET_EXPECT_ACK:
case FRAME_TYPE::REQUEST_SET_EXPECT_DATA:
operationState = State::AWAITING_RESPONSE;
break;
default:
operationState = State::IDLE;
}
}
void Transport::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
uint8_t byteContent)
{
fifo.enqueue(command, frameType, useData, byteContent);
}
bool Transport::processQueue()
{
// check waiting commands
auto f = fifo.getCommand();
if (!f) return false;
trsp.putFrame(f->command, f->frameType, &f->payload, f->payloadSize,
f->useFrameNumber ? f->frameNumber : frameIndex);
TRACE(
"AFHDS3 [CMD QUEUE] cmd: 0x%02x frameType 0x%02x, useFrameNumber %d "
"frame Number %d size %d",
f->command, f->frameType, f->useFrameNumber, f->frameNumber,
f->payloadSize);
if (!f->useFrameNumber) frameIndex++;
fifo.skip();
return true;
}
bool Transport::handleRetransmissions(bool& error)
{
if (operationState == State::AWAITING_RESPONSE) {
if (repeatCount++ < MAX_RETRIES_AFHDS3) {
error = false;
return true; // re-send
}
TRACE("AFHDS3 [NO RESP]");
error = true;
return false;
}
if (operationState == State::UNKNOWN) {
error = true;
return false;
}
error = false;
repeatCount = 0;
return false;
}
bool Transport::handleReply(uint8_t* buffer, uint8_t len)
{
// TODO: check len...
AfhdsFrame* responseFrame = reinterpret_cast<AfhdsFrame*>(buffer);
if (responseFrame->frameType == FRAME_TYPE::REQUEST_SET_EXPECT_ACK) {
// check if such request is not queued
auto f = fifo.getCommand();
if (f && f->frameType == FRAME_TYPE::RESPONSE_ACK &&
f->frameNumber == responseFrame->frameNumber) {
// absorb retransmission
TRACE("ACK for frame %02X already queued", responseFrame->frameNumber);
return true;
}
TRACE("AFHDS3 [QUEUE ACK] cmd %02X type %02X number %02X",
responseFrame->command, responseFrame->frameType,
responseFrame->frameNumber);
fifo.enqueueACK((enum COMMAND)responseFrame->command,
responseFrame->frameNumber);
// TODO: send ACK immediately !!!
} else if (responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA ||
responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) {
if (operationState == State::AWAITING_RESPONSE) {
operationState = State::IDLE;
}
}
return false;
}
bool Transport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
uint8_t& rxBufferCount, uint8_t maxSize)
{
bool has_frame = trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize);
if (has_frame && handleReply(rxBuffer, rxBufferCount)) {
rxBufferCount = 0;
return false;
}
return has_frame;
}
};