AFHDS3: better layer separation (transport / protocol logic)
This commit is contained in:
committed by
Raphael Coeffic
parent
f75cb3f97d
commit
bea7e4ec13
+76
-383
@@ -20,11 +20,13 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "afhds3.h"
|
#include "afhds3.h"
|
||||||
|
#include "afhds3_transport.h"
|
||||||
|
#include "pulses.h"
|
||||||
|
|
||||||
#include "../debug.h"
|
#include "../debug.h"
|
||||||
#include "../definitions.h"
|
#include "../definitions.h"
|
||||||
#include <cstdio>
|
// #include <cstdio>
|
||||||
|
|
||||||
#include "pulses.h"
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
#include "mixer_scheduler.h"
|
#include "mixer_scheduler.h"
|
||||||
#include "hal/module_driver.h"
|
#include "hal/module_driver.h"
|
||||||
@@ -44,8 +46,6 @@
|
|||||||
|
|
||||||
#define FAILSAFE_KEEP_LAST 0x8000
|
#define FAILSAFE_KEEP_LAST 0x8000
|
||||||
|
|
||||||
#define MAX_RETRIES_AFHDS3 5
|
|
||||||
|
|
||||||
//get channel value outside of afhds3 namespace
|
//get channel value outside of afhds3 namespace
|
||||||
int32_t getChannelValue(uint8_t channel);
|
int32_t getChannelValue(uint8_t channel);
|
||||||
|
|
||||||
@@ -62,52 +62,6 @@ static uint8_t _phyMode_channels[] = {
|
|||||||
12, // ROUTINE_LORA_12CH
|
12, // ROUTINE_LORA_12CH
|
||||||
};
|
};
|
||||||
|
|
||||||
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,
|
|
||||||
};
|
|
||||||
|
|
||||||
enum FRAME_TYPE: uint8_t
|
|
||||||
{
|
|
||||||
REQUEST_GET_DATA = 0x01, //Get data response: ACK + DATA
|
|
||||||
REQUEST_SET_EXPECT_DATA = 0x02, //Set data response: ACK + DATA
|
|
||||||
REQUEST_SET_EXPECT_ACK = 0x03, //Set data response: ACK
|
|
||||||
REQUEST_SET_NO_RESP = 0x05, //Set data response: none
|
|
||||||
RESPONSE_DATA = 0x10, //Response ACK + DATA
|
|
||||||
RESPONSE_ACK = 0x20, //Response ACK
|
|
||||||
NOT_USED = 0xff
|
|
||||||
};
|
|
||||||
|
|
||||||
enum COMMAND: uint8_t
|
|
||||||
{
|
|
||||||
MODULE_READY = 0x01,
|
|
||||||
MODULE_STATE = 0x02,
|
|
||||||
MODULE_MODE = 0x03,
|
|
||||||
MODULE_SET_CONFIG = 0x04,
|
|
||||||
MODULE_GET_CONFIG = 0x06,
|
|
||||||
CHANNELS_FAILSAFE_DATA = 0x07,
|
|
||||||
TELEMETRY_DATA = 0x09,
|
|
||||||
SEND_COMMAND = 0x0C,
|
|
||||||
COMMAND_RESULT = 0x0D,
|
|
||||||
// MODULE_POWER_STATUS = 0x0F,
|
|
||||||
// MODULE_VERSION = 0x1F,
|
|
||||||
MODULE_VERSION = 0x20,
|
|
||||||
VIRTUAL_FAILSAFE = 0x99, // virtual command used to trigger failsafe
|
|
||||||
UNDEFINED = 0xFF
|
|
||||||
};
|
|
||||||
|
|
||||||
enum COMMAND_DIRECTION
|
enum COMMAND_DIRECTION
|
||||||
{
|
{
|
||||||
RADIO_TO_MODULE = 0,
|
RADIO_TO_MODULE = 0,
|
||||||
@@ -356,82 +310,9 @@ union AfhdsFrameData
|
|||||||
CommandResult_s CommandResult;
|
CommandResult_s CommandResult;
|
||||||
};
|
};
|
||||||
|
|
||||||
PACK(struct AfhdsFrame
|
|
||||||
{
|
|
||||||
uint8_t startByte;
|
|
||||||
uint8_t address;
|
|
||||||
uint8_t frameNumber;
|
|
||||||
uint8_t frameType;
|
|
||||||
uint8_t command;
|
|
||||||
uint8_t value;
|
|
||||||
|
|
||||||
AfhdsFrameData * GetData()
|
|
||||||
{
|
|
||||||
return reinterpret_cast<AfhdsFrameData*>(&value);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
#define FRM302_STATUS 0x56
|
#define FRM302_STATUS 0x56
|
||||||
|
|
||||||
enum State
|
class ProtoState
|
||||||
{
|
|
||||||
UNKNOWN = 0,
|
|
||||||
SENDING_COMMAND,
|
|
||||||
AWAITING_RESPONSE,
|
|
||||||
IDLE
|
|
||||||
};
|
|
||||||
|
|
||||||
// one byte frames for request queue
|
|
||||||
struct Frame
|
|
||||||
{
|
|
||||||
enum COMMAND command;
|
|
||||||
enum FRAME_TYPE frameType;
|
|
||||||
uint8_t payload;
|
|
||||||
uint8_t frameNumber;
|
|
||||||
bool useFrameNumber;
|
|
||||||
uint8_t payloadSize;
|
|
||||||
};
|
|
||||||
|
|
||||||
// simple fifo implementation because Pulses is used as member of union and can not be non trivial type
|
|
||||||
struct CommandFifo
|
|
||||||
{
|
|
||||||
Frame commandFifo[8];
|
|
||||||
volatile uint32_t setIndex;
|
|
||||||
volatile uint32_t getIndex;
|
|
||||||
|
|
||||||
void clearCommandFifo();
|
|
||||||
|
|
||||||
inline uint32_t nextIndex(uint32_t idx) const
|
|
||||||
{
|
|
||||||
return (idx + 1) & (sizeof(commandFifo) / sizeof(commandFifo[0]) - 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline uint32_t prevIndex(uint32_t idx) const
|
|
||||||
{
|
|
||||||
if (idx == 0)
|
|
||||||
{
|
|
||||||
return (sizeof(commandFifo) / sizeof(commandFifo[0]) - 1);
|
|
||||||
}
|
|
||||||
return (idx - 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline bool isEmpty() const
|
|
||||||
{
|
|
||||||
return (getIndex == setIndex);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline void skip()
|
|
||||||
{
|
|
||||||
getIndex = nextIndex(getIndex);
|
|
||||||
}
|
|
||||||
|
|
||||||
void enqueueACK(COMMAND command, uint8_t frameNumber);
|
|
||||||
|
|
||||||
void enqueue(COMMAND command, FRAME_TYPE frameType, bool useData = false, uint8_t byteContent = 0);
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
class ProtoState: public CommandFifo
|
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
@@ -474,15 +355,9 @@ class ProtoState: public CommandFifo
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
inline void sendByte(uint8_t b);
|
|
||||||
|
|
||||||
inline void putBytes(uint8_t* data, int length);
|
|
||||||
|
|
||||||
inline void putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data = nullptr, uint8_t dataLength = 0, uint8_t* frame_index = nullptr);
|
|
||||||
|
|
||||||
void parseData(uint8_t* rxBuffer, uint8_t rxBufferCount);
|
void parseData(uint8_t* rxBuffer, uint8_t rxBufferCount);
|
||||||
|
|
||||||
void setState(uint8_t state);
|
void setState(ModuleState state);
|
||||||
|
|
||||||
bool syncSettings();
|
bool syncSettings();
|
||||||
|
|
||||||
@@ -513,47 +388,43 @@ class ProtoState: public CommandFifo
|
|||||||
bool isConnectedMulticast();
|
bool isConnectedMulticast();
|
||||||
|
|
||||||
Transport trsp;
|
Transport trsp;
|
||||||
void* trsp_buffer;
|
|
||||||
uint8_t frame_index;
|
|
||||||
uint8_t crc;
|
|
||||||
uint8_t state;
|
|
||||||
uint8_t timeout;
|
|
||||||
uint8_t esc_state;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Index of the module
|
* Index of the module
|
||||||
*/
|
*/
|
||||||
uint8_t module_index;
|
uint8_t module_index;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Reported state of the HF module
|
||||||
|
*/
|
||||||
|
ModuleState state;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Target mode to be set to the module one of MODULE_MODE_E
|
* Target mode to be set to the module one of MODULE_MODE_E
|
||||||
*/
|
*/
|
||||||
uint8_t requestedModuleMode;
|
uint8_t requestedModuleMode;
|
||||||
/**
|
|
||||||
* Internal operation state one of UNKNOWN, SENDING_COMMAND, AWAITING_RESPONSE, IDLE
|
|
||||||
* Used to avoid sending commands when not allowed to
|
|
||||||
*/
|
|
||||||
State operationState;
|
|
||||||
/**
|
|
||||||
* Actual repeat count for requested command/operation - incremented by every attempt sending anything
|
|
||||||
*/
|
|
||||||
uint16_t repeatCount;
|
|
||||||
/**
|
/**
|
||||||
* Command count used for counting actual number of commands sent in run mode
|
* Command count used for counting actual number of commands sent in run mode
|
||||||
*/
|
*/
|
||||||
uint32_t cmdCount;
|
uint32_t cmdCount;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Command index of command to be send when cmdCount reached necessary value
|
* Command index of command to be send when cmdCount reached necessary value
|
||||||
*/
|
*/
|
||||||
uint32_t cmdIndex;
|
uint32_t cmdIndex;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Actual power source of the module - should be requested time to time
|
* Actual power source of the module - should be requested time to time
|
||||||
* Currently requested once
|
* Currently requested once
|
||||||
*/
|
*/
|
||||||
enum MODULE_POWER_SOURCE powerSource;
|
enum MODULE_POWER_SOURCE powerSource;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Pointer to module config - it is making operations easier and faster
|
* Pointer to module config - it is making operations easier and faster
|
||||||
*/
|
*/
|
||||||
ModuleData* moduleData;
|
ModuleData* moduleData;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Actual module configuration - must be requested from module
|
* Actual module configuration - must be requested from module
|
||||||
*/
|
*/
|
||||||
@@ -597,9 +468,6 @@ static const COMMAND periodicRequestCommands[] =
|
|||||||
COMMAND::VIRTUAL_FAILSAFE
|
COMMAND::VIRTUAL_FAILSAFE
|
||||||
};
|
};
|
||||||
|
|
||||||
//Address used in transmitted frames - it constrains of target address and source address
|
|
||||||
const uint8_t FrameAddress = DeviceAddress::TRANSMITTER | (DeviceAddress::MODULE << 4);
|
|
||||||
|
|
||||||
//Static collection of afhds3 object instances by module
|
//Static collection of afhds3 object instances by module
|
||||||
static ProtoState protoState[NUM_MODULES];
|
static ProtoState protoState[NUM_MODULES];
|
||||||
|
|
||||||
@@ -630,40 +498,6 @@ void processTelemetryData(uint8_t data, uint8_t module)
|
|||||||
maxSize);
|
maxSize);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CommandFifo::clearCommandFifo()
|
|
||||||
{
|
|
||||||
memclear(commandFifo, sizeof(commandFifo));
|
|
||||||
setIndex = getIndex = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
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 ProtoState::getStatusString(char* buffer) const
|
void ProtoState::getStatusString(char* buffer) const
|
||||||
{
|
{
|
||||||
strcpy(buffer, state <= ModuleState::STATE_READY ? moduleStateText[state]
|
strcpy(buffer, state <= ModuleState::STATE_READY ? moduleStateText[state]
|
||||||
@@ -677,38 +511,11 @@ void ProtoState::getPowerStatus(char* buffer) const
|
|||||||
|
|
||||||
void ProtoState::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize)
|
void ProtoState::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize)
|
||||||
{
|
{
|
||||||
if (rxBufferCount == 0 && byte != AfhdsSpecialChars::START) {
|
if (!trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize))
|
||||||
TRACE("AFHDS3 [SKIP] %02X", byte);
|
|
||||||
this->esc_state = 0;
|
|
||||||
return;
|
return;
|
||||||
}
|
|
||||||
|
|
||||||
if (byte == AfhdsSpecialChars::ESC) {
|
parseData(rxBuffer, rxBufferCount);
|
||||||
this->esc_state = rxBufferCount;
|
rxBufferCount = 0;
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (rxBufferCount > 1 && byte == AfhdsSpecialChars::END) {
|
|
||||||
rxBuffer[rxBufferCount++] = byte;
|
|
||||||
parseData(rxBuffer, rxBufferCount);
|
|
||||||
rxBufferCount = 0;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (this->esc_state && byte == AfhdsSpecialChars::ESC_END) {
|
|
||||||
byte = AfhdsSpecialChars::END;
|
|
||||||
}
|
|
||||||
else if (esc_state && byte == AfhdsSpecialChars::ESC_ESC) {
|
|
||||||
byte = AfhdsSpecialChars::ESC;
|
|
||||||
}
|
|
||||||
//reset esc index
|
|
||||||
this->esc_state = 0;
|
|
||||||
|
|
||||||
if (rxBufferCount >= maxSize) {
|
|
||||||
TRACE("AFHDS3 [BUFFER OVERFLOW]");
|
|
||||||
rxBufferCount = 0;
|
|
||||||
}
|
|
||||||
rxBuffer[rxBufferCount++] = byte;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ProtoState::isConnectedUnicast()
|
bool ProtoState::isConnectedUnicast()
|
||||||
@@ -726,40 +533,22 @@ bool ProtoState::isConnectedMulticast()
|
|||||||
|
|
||||||
void ProtoState::setupFrame()
|
void ProtoState::setupFrame()
|
||||||
{
|
{
|
||||||
if (operationState == State::AWAITING_RESPONSE) {
|
bool trsp_error = false;
|
||||||
if (repeatCount++ < MAX_RETRIES_AFHDS3) {
|
if (trsp.handleRetransmissions(trsp_error)) return;
|
||||||
return; //re-send
|
|
||||||
}
|
if (trsp_error) {
|
||||||
else
|
|
||||||
{
|
|
||||||
TRACE("AFHDS3 [NO RESP] module state %d", this->state);
|
|
||||||
clearFrameData();
|
|
||||||
this->state = ModuleState::STATE_NOT_READY;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (operationState == State::UNKNOWN) {
|
|
||||||
this->state = ModuleState::STATE_NOT_READY;
|
this->state = ModuleState::STATE_NOT_READY;
|
||||||
|
clearFrameData();
|
||||||
}
|
}
|
||||||
repeatCount = 0;
|
|
||||||
|
|
||||||
if (this->state == ModuleState::STATE_NOT_READY) {
|
if (this->state == ModuleState::STATE_NOT_READY) {
|
||||||
TRACE("AFHDS3 [GET MODULE READY]");
|
TRACE("AFHDS3 [GET MODULE READY]");
|
||||||
putFrame(COMMAND::MODULE_READY, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.sendFrame(COMMAND::MODULE_READY, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check waiting commands
|
// process backlog
|
||||||
if (!isEmpty()) {
|
if (trsp.processQueue()) return;
|
||||||
Frame f = commandFifo[getIndex];
|
|
||||||
putFrame(f.command, f.frameType, &f.payload, f.payloadSize,
|
|
||||||
f.useFrameNumber ? &f.frameNumber : &frame_index);
|
|
||||||
getIndex = nextIndex(getIndex);
|
|
||||||
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);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// config should be loaded already
|
// config should be loaded already
|
||||||
if (syncSettings()) { return; }
|
if (syncSettings()) { return; }
|
||||||
@@ -771,7 +560,7 @@ void ProtoState::setupFrame()
|
|||||||
case STATE_BINDING:
|
case STATE_BINDING:
|
||||||
// TODO: poll status? handle timeout?
|
// TODO: poll status? handle timeout?
|
||||||
// requestedModuleMode = MODULE_MODE_E::BIND;
|
// requestedModuleMode = MODULE_MODE_E::BIND;
|
||||||
// putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
// trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
||||||
// &requestedModuleMode, 1);
|
// &requestedModuleMode, 1);
|
||||||
// return;
|
// return;
|
||||||
break;
|
break;
|
||||||
@@ -781,13 +570,13 @@ void ProtoState::setupFrame()
|
|||||||
TRACE("AFHDS3 [BIND]");
|
TRACE("AFHDS3 [BIND]");
|
||||||
setConfigFromModel();
|
setConfigFromModel();
|
||||||
|
|
||||||
putFrame(COMMAND::MODULE_SET_CONFIG,
|
trsp.sendFrame(COMMAND::MODULE_SET_CONFIG,
|
||||||
FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer,
|
FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer,
|
||||||
cfg.version == 0 ? sizeof(cfg.v0) : sizeof(cfg.v1));
|
cfg.version == 0 ? sizeof(cfg.v0) : sizeof(cfg.v1));
|
||||||
|
|
||||||
requestedModuleMode = MODULE_MODE_E::BIND;
|
requestedModuleMode = MODULE_MODE_E::BIND;
|
||||||
enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
|
trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
|
||||||
requestedModuleMode);
|
requestedModuleMode);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
// default:
|
// default:
|
||||||
@@ -802,17 +591,18 @@ void ProtoState::setupFrame()
|
|||||||
// cfg.config.runPower = RUN_POWER::RUN_POWER_FIRST;
|
// cfg.config.runPower = RUN_POWER::RUN_POWER_FIRST;
|
||||||
// uint8_t data[] = { 0x13, 0x20, 0x02, cfg.config.runPower, 0 };
|
// uint8_t data[] = { 0x13, 0x20, 0x02, cfg.config.runPower, 0 };
|
||||||
// TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
|
// TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
|
||||||
// putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
||||||
// data, sizeof(data));
|
// data, sizeof(data));
|
||||||
// return;
|
// return;
|
||||||
// }
|
// }
|
||||||
// }
|
// }
|
||||||
else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_NORMAL) {
|
else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_NORMAL) {
|
||||||
|
|
||||||
// if module is ready but not started
|
// if module is ready but not started
|
||||||
if (this->state == ModuleState::STATE_READY ||
|
if (this->state == ModuleState::STATE_READY ||
|
||||||
this->state == ModuleState::STATE_STANDBY) {
|
this->state == ModuleState::STATE_STANDBY) {
|
||||||
|
|
||||||
cmdCount = 0;
|
cmdCount = 0;
|
||||||
repeatCount = 0;
|
|
||||||
requestInfoAndRun(true);
|
requestInfoAndRun(true);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@@ -821,7 +611,7 @@ void ProtoState::setupFrame()
|
|||||||
if (state == STATE_BINDING) {
|
if (state == STATE_BINDING) {
|
||||||
TRACE("AFHDS3 [EXIT BIND]");
|
TRACE("AFHDS3 [EXIT BIND]");
|
||||||
requestedModuleMode = MODULE_MODE_E::RUN;
|
requestedModuleMode = MODULE_MODE_E::RUN;
|
||||||
putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
||||||
&requestedModuleMode, 1);
|
&requestedModuleMode, 1);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@@ -843,7 +633,7 @@ void ProtoState::setupFrame()
|
|||||||
uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = {
|
uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = {
|
||||||
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
|
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
|
||||||
setFailSafe((int16_t*)(&failSafe[1]));
|
setFailSafe((int16_t*)(&failSafe[1]));
|
||||||
putFrame(COMMAND::CHANNELS_FAILSAFE_DATA,
|
trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA,
|
||||||
FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe,
|
FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe,
|
||||||
AFHDS3_MAX_CHANNELS * 2 + 2);
|
AFHDS3_MAX_CHANNELS * 2 + 2);
|
||||||
} else {
|
} else {
|
||||||
@@ -851,31 +641,31 @@ void ProtoState::setupFrame()
|
|||||||
uint8_t failSafe[3 + AFHDS3_MAX_CHANNELS * 2] = {
|
uint8_t failSafe[3 + AFHDS3_MAX_CHANNELS * 2] = {
|
||||||
0x11, 0x60, AFHDS3_MAX_CHANNELS * 2};
|
0x11, 0x60, AFHDS3_MAX_CHANNELS * 2};
|
||||||
setFailSafe((int16_t*)(failSafe + 3));
|
setFailSafe((int16_t*)(failSafe + 3));
|
||||||
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
||||||
failSafe, 3 + AFHDS3_MAX_CHANNELS * 2);
|
failSafe, 3 + AFHDS3_MAX_CHANNELS * 2);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
putFrame(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.sendFrame(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
putFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.sendFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
}
|
}
|
||||||
} else if (isConnected) {
|
} else if (isConnected) {
|
||||||
sendChannelsData();
|
sendChannelsData();
|
||||||
} else {
|
} else {
|
||||||
//default frame - request state
|
//default frame - request state
|
||||||
putFrame(MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.sendFrame(MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t* ProtoState::getBuffer()
|
uint8_t* ProtoState::getBuffer()
|
||||||
{
|
{
|
||||||
return (uint8_t*)trsp_buffer;
|
return trsp.getFrameBuffer();
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t ProtoState::getBufferSize()
|
uint32_t ProtoState::getBufferSize()
|
||||||
{
|
{
|
||||||
return trsp.getSize(trsp_buffer);
|
return trsp.getFrameSize();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ProtoState::init(uint8_t moduleIndex, bool resetFrameCount)
|
void ProtoState::init(uint8_t moduleIndex, bool resetFrameCount)
|
||||||
@@ -883,22 +673,19 @@ void ProtoState::init(uint8_t moduleIndex, bool resetFrameCount)
|
|||||||
module_index = moduleIndex;
|
module_index = moduleIndex;
|
||||||
#if defined(INTERNAL_MODULE_AFHDS3)
|
#if defined(INTERNAL_MODULE_AFHDS3)
|
||||||
if (moduleIndex == INTERNAL_MODULE) {
|
if (moduleIndex == INTERNAL_MODULE) {
|
||||||
trsp_buffer = &intmodulePulsesData.afhds3;
|
trsp.init(ByteTransport::Serial, &intmodulePulsesData.afhds3);
|
||||||
trsp.init(Transport::Serial);
|
|
||||||
} else
|
} else
|
||||||
#endif
|
#endif
|
||||||
{
|
{
|
||||||
trsp_buffer = &extmodulePulsesData.afhds3;
|
|
||||||
#if defined(AFHDS3_EXT_UART)
|
#if defined(AFHDS3_EXT_UART)
|
||||||
trsp.init(Transport::Serial);
|
trsp.init(ByteTransport::Serial, &extmodulePulsesData.afhds3);
|
||||||
#else
|
#else
|
||||||
trsp.init(Transport::Pulses);
|
trsp.init(ByteTransport::Pulses, &extmodulePulsesData.afhds3);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
//clear local vars because it is member of union
|
//clear local vars because it is member of union
|
||||||
moduleData = &g_model.moduleData[module_index];
|
moduleData = &g_model.moduleData[module_index];
|
||||||
operationState = State::UNKNOWN;
|
|
||||||
state = ModuleState::STATE_NOT_READY;
|
state = ModuleState::STATE_NOT_READY;
|
||||||
clearFrameData();
|
clearFrameData();
|
||||||
}
|
}
|
||||||
@@ -906,84 +693,10 @@ void ProtoState::init(uint8_t moduleIndex, bool resetFrameCount)
|
|||||||
void ProtoState::clearFrameData()
|
void ProtoState::clearFrameData()
|
||||||
{
|
{
|
||||||
TRACE("AFHDS3 clearFrameData");
|
TRACE("AFHDS3 clearFrameData");
|
||||||
trsp.reset(trsp_buffer);
|
trsp.clear();
|
||||||
clearCommandFifo();
|
|
||||||
repeatCount = 0;
|
|
||||||
cmdCount = 0;
|
cmdCount = 0;
|
||||||
cmdIndex = 0;
|
cmdIndex = 0;
|
||||||
frame_index = 1;
|
|
||||||
timeout = 0;
|
|
||||||
esc_state = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ProtoState::sendByte(uint8_t b)
|
|
||||||
{
|
|
||||||
trsp.sendByte(trsp_buffer, b);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ProtoState::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 ProtoState::putFrame(COMMAND command, FRAME_TYPE frame, uint8_t* data, uint8_t dataLength, uint8_t* frameIndex)
|
|
||||||
{
|
|
||||||
//header
|
|
||||||
operationState = State::SENDING_COMMAND;
|
|
||||||
trsp.reset(trsp_buffer);
|
|
||||||
crc = 0;
|
|
||||||
sendByte(START);
|
|
||||||
if (frameIndex == nullptr) {
|
|
||||||
frameIndex = &frame_index;
|
|
||||||
}
|
|
||||||
uint8_t buffer[] = {FrameAddress, *frameIndex, frame, command};
|
|
||||||
putBytes(buffer, 4);
|
|
||||||
|
|
||||||
//payload
|
|
||||||
if (dataLength > 0) {
|
|
||||||
putBytes(data, dataLength);
|
|
||||||
}
|
|
||||||
//footer
|
|
||||||
uint8_t crcValue = crc ^ 0xff;
|
|
||||||
putBytes(&crcValue, 1);
|
|
||||||
sendByte(END);
|
|
||||||
*frameIndex = *frameIndex + 1;
|
|
||||||
|
|
||||||
switch (frame) {
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
trsp.flush(trsp_buffer);
|
|
||||||
}
|
|
||||||
|
|
||||||
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 containsData(enum FRAME_TYPE frameType)
|
bool containsData(enum FRAME_TYPE frameType)
|
||||||
@@ -995,7 +708,7 @@ bool containsData(enum FRAME_TYPE frameType)
|
|||||||
frameType == FRAME_TYPE::REQUEST_SET_NO_RESP);
|
frameType == FRAME_TYPE::REQUEST_SET_NO_RESP);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ProtoState::setState(uint8_t state)
|
void ProtoState::setState(ModuleState state)
|
||||||
{
|
{
|
||||||
if (state == this->state) {
|
if (state == this->state) {
|
||||||
return;
|
return;
|
||||||
@@ -1006,33 +719,29 @@ void ProtoState::setState(uint8_t state)
|
|||||||
setModuleMode(module_index, ::ModuleSettingsMode::MODULE_MODE_NORMAL);
|
setModuleMode(module_index, ::ModuleSettingsMode::MODULE_MODE_NORMAL);
|
||||||
}
|
}
|
||||||
if (state == ModuleState::STATE_NOT_READY) {
|
if (state == ModuleState::STATE_NOT_READY) {
|
||||||
operationState = State::UNKNOWN;
|
trsp.clear();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ProtoState::requestInfoAndRun(bool send)
|
void ProtoState::requestInfoAndRun(bool send)
|
||||||
{
|
{
|
||||||
if (!send) {
|
if (!send) {
|
||||||
enqueue(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.enqueue(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
}
|
}
|
||||||
|
|
||||||
// enqueue(COMMAND::MODULE_POWER_STATUS, FRAME_TYPE::REQUEST_GET_DATA);
|
// trsp.enqueue(COMMAND::MODULE_POWER_STATUS, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
|
|
||||||
requestedModuleMode = MODULE_MODE_E::RUN;
|
requestedModuleMode = MODULE_MODE_E::RUN;
|
||||||
enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
|
trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
|
||||||
(uint8_t)requestedModuleMode);
|
(uint8_t)requestedModuleMode);
|
||||||
|
|
||||||
if (send) {
|
if (send) {
|
||||||
putFrame(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.sendFrame(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
|
void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
|
||||||
{
|
{
|
||||||
if (!checkCRC(rxBuffer, rxBufferCount - 2)) {
|
|
||||||
TRACE("AFHDS3 [INVALID CRC]");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
AfhdsFrame* responseFrame = reinterpret_cast<AfhdsFrame*>(rxBuffer);
|
AfhdsFrame* responseFrame = reinterpret_cast<AfhdsFrame*>(rxBuffer);
|
||||||
if (containsData((enum FRAME_TYPE) responseFrame->frameType)) {
|
if (containsData((enum FRAME_TYPE) responseFrame->frameType)) {
|
||||||
switch (responseFrame->command) {
|
switch (responseFrame->command) {
|
||||||
@@ -1046,14 +755,15 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
|
|||||||
setState(ModuleState::STATE_NOT_READY);
|
setState(ModuleState::STATE_NOT_READY);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case COMMAND::MODULE_GET_CONFIG:
|
case COMMAND::MODULE_GET_CONFIG: {
|
||||||
std::memcpy((void*) cfg.buffer, &responseFrame->value, sizeof(cfg.buffer));
|
size_t len = min<size_t>(sizeof(cfg.buffer), rxBufferCount);
|
||||||
|
std::memcpy((void*) cfg.buffer, &responseFrame->value, len);
|
||||||
// TRACE(
|
// TRACE(
|
||||||
// "AFHDS3 [MODULE_GET_CONFIG] bind power %d run power %d mode %d "
|
// "AFHDS3 [MODULE_GET_CONFIG] bind power %d run power %d mode %d "
|
||||||
// "pwm/ppm %d ibus/sbus %d",
|
// "pwm/ppm %d ibus/sbus %d",
|
||||||
// cfg.config.bindPower, cfg.config.runPower, cfg.config.telemetry,
|
// cfg.config.bindPower, cfg.config.runPower, cfg.config.telemetry,
|
||||||
// cfg.config.pulseMode, cfg.config.serialMode);
|
// cfg.config.pulseMode, cfg.config.serialMode);
|
||||||
break;
|
} break;
|
||||||
case COMMAND::MODULE_VERSION:
|
case COMMAND::MODULE_VERSION:
|
||||||
std::memcpy((void*) &version, &responseFrame->value, sizeof(version));
|
std::memcpy((void*) &version, &responseFrame->value, sizeof(version));
|
||||||
TRACE("AFHDS3 [MODULE_VERSION] Product %d, HW %d, BOOT %d, FW %d",
|
TRACE("AFHDS3 [MODULE_VERSION] Product %d, HW %d, BOOT %d, FW %d",
|
||||||
@@ -1066,7 +776,7 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
|
|||||||
// break;
|
// break;
|
||||||
case COMMAND::MODULE_STATE:
|
case COMMAND::MODULE_STATE:
|
||||||
TRACE("AFHDS3 [MODULE_STATE] %02X", responseFrame->value);
|
TRACE("AFHDS3 [MODULE_STATE] %02X", responseFrame->value);
|
||||||
setState(responseFrame->value);
|
setState((ModuleState)responseFrame->value);
|
||||||
break;
|
break;
|
||||||
case COMMAND::MODULE_MODE:
|
case COMMAND::MODULE_MODE:
|
||||||
TRACE("AFHDS3 [MODULE_MODE] %02X", responseFrame->value);
|
TRACE("AFHDS3 [MODULE_MODE] %02X", responseFrame->value);
|
||||||
@@ -1075,8 +785,8 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
|
|||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
if (requestedModuleMode == MODULE_MODE_E::RUN) {
|
if (requestedModuleMode == MODULE_MODE_E::RUN) {
|
||||||
enqueue(COMMAND::MODULE_GET_CONFIG, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.enqueue(COMMAND::MODULE_GET_CONFIG, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
enqueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
|
trsp.enqueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
|
||||||
}
|
}
|
||||||
requestedModuleMode = MODULE_MODE_UNKNOWN;
|
requestedModuleMode = MODULE_MODE_UNKNOWN;
|
||||||
}
|
}
|
||||||
@@ -1129,36 +839,19 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case COMMAND::COMMAND_RESULT:
|
case COMMAND::COMMAND_RESULT: {
|
||||||
{
|
// AfhdsFrameData* respData = responseFrame->GetData();
|
||||||
//AfhdsFrameData* respData = responseFrame->GetData();
|
// TRACE("COMMAND RESULT %02X result %d datalen %d",
|
||||||
//TRACE("COMMAND RESULT %02X result %d datalen %d", respData->CommandResult.command, respData->CommandResult.result, respData->CommandResult.respLen);
|
// respData->CommandResult.command, respData->CommandResult.result,
|
||||||
}
|
// respData->CommandResult.respLen);
|
||||||
break;
|
} break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (responseFrame->frameType == FRAME_TYPE::REQUEST_GET_DATA || responseFrame->frameType == FRAME_TYPE::REQUEST_SET_EXPECT_DATA) {
|
if (responseFrame->frameType == FRAME_TYPE::REQUEST_GET_DATA ||
|
||||||
|
responseFrame->frameType == FRAME_TYPE::REQUEST_SET_EXPECT_DATA) {
|
||||||
TRACE("Command %02X NOT IMPLEMENTED!", responseFrame->command);
|
TRACE("Command %02X NOT IMPLEMENTED!", responseFrame->command);
|
||||||
}
|
}
|
||||||
else if (responseFrame->frameType == FRAME_TYPE::REQUEST_SET_EXPECT_ACK) {
|
|
||||||
//check if such request is not queued
|
|
||||||
if (!isEmpty()) {
|
|
||||||
Frame f = commandFifo[getIndex];
|
|
||||||
if (f.frameType == FRAME_TYPE::RESPONSE_ACK && f.frameNumber == responseFrame->frameNumber) {
|
|
||||||
TRACE("ACK for frame %02X already queued", responseFrame->frameNumber);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
TRACE("AFHDS3 [QUEUE ACK] cmd %02X type %02X number %02X", responseFrame->command, responseFrame->frameType, responseFrame->frameNumber);
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool isSbus(uint8_t mode)
|
inline bool isSbus(uint8_t mode)
|
||||||
@@ -1210,7 +903,7 @@ bool ProtoState::syncSettings()
|
|||||||
// cfg.config.runPower = moduleData->afhds3.runPower;
|
// cfg.config.runPower = moduleData->afhds3.runPower;
|
||||||
// uint8_t data[] = {0x13, 0x20, 0x02, moduleData->afhds3.runPower, 0};
|
// uint8_t data[] = {0x13, 0x20, 0x02, moduleData->afhds3.runPower, 0};
|
||||||
// TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
|
// TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
|
||||||
// putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
||||||
// sizeof(data));
|
// sizeof(data));
|
||||||
// return true;
|
// return true;
|
||||||
// }
|
// }
|
||||||
@@ -1226,7 +919,7 @@ bool ProtoState::syncSettings()
|
|||||||
// (uint8_t)(moduleData->afhds3.rxFreq() & 0xFF),
|
// (uint8_t)(moduleData->afhds3.rxFreq() & 0xFF),
|
||||||
// (uint8_t)(moduleData->afhds3.rxFreq() >> 8)};
|
// (uint8_t)(moduleData->afhds3.rxFreq() >> 8)};
|
||||||
// TRACE("AFHDS3 SET RX FREQ");
|
// TRACE("AFHDS3 SET RX FREQ");
|
||||||
// putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
||||||
// sizeof(data));
|
// sizeof(data));
|
||||||
// return true;
|
// return true;
|
||||||
// }
|
// }
|
||||||
@@ -1238,7 +931,7 @@ bool ProtoState::syncSettings()
|
|||||||
// cfg.config.pulseMode = modelPulseMode;
|
// cfg.config.pulseMode = modelPulseMode;
|
||||||
// TRACE("AFHDS3 PWM/PPM %d", modelPulseMode);
|
// TRACE("AFHDS3 PWM/PPM %d", modelPulseMode);
|
||||||
// uint8_t data[] = {0x16, 0x70, 0x01, (uint8_t)(modelPulseMode)};
|
// uint8_t data[] = {0x16, 0x70, 0x01, (uint8_t)(modelPulseMode)};
|
||||||
// putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
||||||
// sizeof(data));
|
// sizeof(data));
|
||||||
// return true;
|
// return true;
|
||||||
// }
|
// }
|
||||||
@@ -1250,7 +943,7 @@ bool ProtoState::syncSettings()
|
|||||||
// cfg.config.serialMode = modelSerialMode;
|
// cfg.config.serialMode = modelSerialMode;
|
||||||
// TRACE("AFHDS3 IBUS/SBUS %d", modelSerialMode);
|
// TRACE("AFHDS3 IBUS/SBUS %d", modelSerialMode);
|
||||||
// uint8_t data[] = {0x18, 0x70, 0x01, (uint8_t)(modelSerialMode)};
|
// uint8_t data[] = {0x18, 0x70, 0x01, (uint8_t)(modelSerialMode)};
|
||||||
// putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
||||||
// sizeof(data));
|
// sizeof(data));
|
||||||
// return true;
|
// return true;
|
||||||
// }
|
// }
|
||||||
@@ -1260,7 +953,7 @@ bool ProtoState::syncSettings()
|
|||||||
// uint8_t data[] = {0x12, 0x60, 0x02,
|
// uint8_t data[] = {0x12, 0x60, 0x02,
|
||||||
// (uint8_t)(moduleData->afhds3.failsafeTimeout & 0xFF),
|
// (uint8_t)(moduleData->afhds3.failsafeTimeout & 0xFF),
|
||||||
// (uint8_t)(moduleData->afhds3.failsafeTimeout >> 8)};
|
// (uint8_t)(moduleData->afhds3.failsafeTimeout >> 8)};
|
||||||
// putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
|
||||||
// sizeof(data));
|
// sizeof(data));
|
||||||
// TRACE("AFHDS3 FAILSAFE TMEOUT, %d", moduleData->afhds3.failsafeTimeout);
|
// TRACE("AFHDS3 FAILSAFE TMEOUT, %d", moduleData->afhds3.failsafeTimeout);
|
||||||
// return true;
|
// return true;
|
||||||
@@ -1289,7 +982,7 @@ void ProtoState::sendChannelsData()
|
|||||||
buffer[index] = channelValue;
|
buffer[index] = channelValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
putFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP,
|
trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP,
|
||||||
(uint8_t*)buffer, (channels + 1) * 2);
|
(uint8_t*)buffer, (channels + 1) * 2);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1297,7 +990,7 @@ void ProtoState::stop()
|
|||||||
{
|
{
|
||||||
TRACE("AFHDS3 STOP");
|
TRACE("AFHDS3 STOP");
|
||||||
requestedModuleMode = MODULE_MODE_E::STANDBY;
|
requestedModuleMode = MODULE_MODE_E::STANDBY;
|
||||||
putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
|
||||||
&requestedModuleMode, 1);
|
&requestedModuleMode, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -32,7 +32,7 @@
|
|||||||
#include "fifo.h"
|
#include "fifo.h"
|
||||||
|
|
||||||
#include "hal/module_driver.h"
|
#include "hal/module_driver.h"
|
||||||
#include "afhds3_transport.h"
|
#include "afhds3_module.h"
|
||||||
|
|
||||||
#define AFHDS3_MIN_FREQ 50
|
#define AFHDS3_MIN_FREQ 50
|
||||||
#define AFHDS3_MAX_FREQ 400
|
#define AFHDS3_MAX_FREQ 400
|
||||||
|
|||||||
@@ -0,0 +1,73 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) EdgeTX
|
||||||
|
*
|
||||||
|
* Based on code named
|
||||||
|
* opentx - https://github.com/opentx/opentx
|
||||||
|
* th9x - http://code.google.com/p/th9x
|
||||||
|
* er9x - http://code.google.com/p/er9x
|
||||||
|
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||||
|
*
|
||||||
|
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License version 2 as
|
||||||
|
* published by the Free Software Foundation.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "hal.h"
|
||||||
|
|
||||||
|
#define AFHDS_MAX_PULSES 128
|
||||||
|
|
||||||
|
// max number of transitions measured so far 290 + 10%
|
||||||
|
// TODO: define as a function of MAX_PULSES
|
||||||
|
#define AFHDS_MAX_PULSES_TRANSITIONS 320
|
||||||
|
|
||||||
|
#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO)
|
||||||
|
#define AFHDS3_EXT_UART
|
||||||
|
#else
|
||||||
|
#define AFHDS3_EXT_SOFTSERIAL
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define AFHDS3_UART_BAUDRATE 1500000
|
||||||
|
#define AFHDS3_UART_COMMAND_TIMEOUT 5
|
||||||
|
|
||||||
|
#if defined(AFHDS3_SLOW)
|
||||||
|
#define AFHDS3_SOFTSERIAL_BAUDRATE 57600
|
||||||
|
#define AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT 20
|
||||||
|
#else
|
||||||
|
#define AFHDS3_SOFTSERIAL_BAUDRATE 115200
|
||||||
|
#define AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT 15
|
||||||
|
#endif
|
||||||
|
|
||||||
|
namespace afhds3
|
||||||
|
{
|
||||||
|
|
||||||
|
struct SerialData {
|
||||||
|
uint8_t pulses[AFHDS_MAX_PULSES];
|
||||||
|
uint8_t* ptr;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct PulsesData {
|
||||||
|
uint16_t pulses[AFHDS_MAX_PULSES_TRANSITIONS];
|
||||||
|
uint16_t* ptr;
|
||||||
|
};
|
||||||
|
|
||||||
|
#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO)
|
||||||
|
typedef SerialData ExtmoduleData;
|
||||||
|
#else
|
||||||
|
typedef PulsesData ExtmoduleData;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(INTERNAL_MODULE_AFHDS3)
|
||||||
|
typedef SerialData IntmoduleData;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
};
|
||||||
@@ -20,6 +20,8 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "afhds3_transport.h"
|
#include "afhds3_transport.h"
|
||||||
|
#include "opentx_helpers.h"
|
||||||
|
#include "debug.h"
|
||||||
|
|
||||||
// timer is 2 MHz
|
// timer is 2 MHz
|
||||||
#if defined(AFHDS3_SLOW)
|
#if defined(AFHDS3_SLOW)
|
||||||
@@ -30,9 +32,31 @@
|
|||||||
#define BITLEN_AFHDS (17)
|
#define BITLEN_AFHDS (17)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define MAX_RETRIES_AFHDS3 5
|
||||||
|
|
||||||
namespace afhds3
|
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)
|
static void _serial_reset(void* buffer)
|
||||||
{
|
{
|
||||||
auto data = (SerialData*)buffer;
|
auto data = (SerialData*)buffer;
|
||||||
@@ -105,7 +129,7 @@ uint32_t _pulses_getSize(void* buffer)
|
|||||||
return data->ptr - data->pulses;
|
return data->ptr - data->pulses;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Transport::init(Type t)
|
void ByteTransport::init(Type t)
|
||||||
{
|
{
|
||||||
if (t == Serial) {
|
if (t == Serial) {
|
||||||
reset = _serial_reset;
|
reset = _serial_reset;
|
||||||
@@ -123,6 +147,304 @@ void Transport::init(Type t)
|
|||||||
flush = nullptr;
|
flush = nullptr;
|
||||||
getSize = 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;
|
||||||
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -21,68 +21,195 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <stdint.h>
|
#include "afhds3_module.h"
|
||||||
#include "hal.h"
|
#include "definitions.h"
|
||||||
|
|
||||||
#define AFHDS_MAX_PULSES 128
|
|
||||||
|
|
||||||
// max number of transitions measured so far 290 + 10%
|
|
||||||
// TODO: define as a function of MAX_PULSES
|
|
||||||
#define AFHDS_MAX_PULSES_TRANSITIONS 320
|
|
||||||
|
|
||||||
#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO)
|
|
||||||
#define AFHDS3_EXT_UART
|
|
||||||
#else
|
|
||||||
#define AFHDS3_EXT_SOFTSERIAL
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define AFHDS3_UART_BAUDRATE 1500000
|
|
||||||
#define AFHDS3_UART_COMMAND_TIMEOUT 5
|
|
||||||
|
|
||||||
#if defined(AFHDS3_SLOW)
|
|
||||||
#define AFHDS3_SOFTSERIAL_BAUDRATE 57600
|
|
||||||
#define AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT 20
|
|
||||||
#else
|
|
||||||
#define AFHDS3_SOFTSERIAL_BAUDRATE 115200
|
|
||||||
#define AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT 15
|
|
||||||
#endif
|
|
||||||
|
|
||||||
namespace afhds3
|
namespace afhds3
|
||||||
{
|
{
|
||||||
|
|
||||||
struct SerialData {
|
|
||||||
uint8_t pulses[AFHDS_MAX_PULSES];
|
|
||||||
uint8_t* ptr;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct PulsesData {
|
|
||||||
uint16_t pulses[AFHDS_MAX_PULSES_TRANSITIONS];
|
|
||||||
uint16_t* ptr;
|
|
||||||
};
|
|
||||||
|
|
||||||
#if defined(EXTMODULE_USART) && defined(EXTMODULE_TX_INVERT_GPIO)
|
|
||||||
typedef SerialData ExtmoduleData;
|
|
||||||
#else
|
|
||||||
typedef PulsesData ExtmoduleData;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(INTERNAL_MODULE_AFHDS3)
|
|
||||||
typedef SerialData IntmoduleData;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
struct Transport {
|
|
||||||
|
|
||||||
enum Type {
|
|
||||||
Serial,
|
|
||||||
Pulses
|
|
||||||
};
|
|
||||||
|
|
||||||
|
struct ByteTransport {
|
||||||
|
enum Type { Serial, Pulses };
|
||||||
|
|
||||||
void (*reset)(void* buffer);
|
void (*reset)(void* buffer);
|
||||||
void (*sendByte)(void* buffer, uint8_t b);
|
void (*sendByte)(void* buffer, uint8_t b);
|
||||||
void (*flush)(void* buffer);
|
void (*flush)(void* buffer);
|
||||||
uint32_t (*getSize)(void* buffer);
|
uint32_t (*getSize)(void* buffer);
|
||||||
|
|
||||||
void init(Type t);
|
void init(Type t);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum FRAME_TYPE: uint8_t
|
||||||
|
{
|
||||||
|
REQUEST_GET_DATA = 0x01, //Get data response: ACK + DATA
|
||||||
|
REQUEST_SET_EXPECT_DATA = 0x02, //Set data response: ACK + DATA
|
||||||
|
REQUEST_SET_EXPECT_ACK = 0x03, //Set data response: ACK
|
||||||
|
REQUEST_SET_NO_RESP = 0x05, //Set data response: none
|
||||||
|
RESPONSE_DATA = 0x10, //Response ACK + DATA
|
||||||
|
RESPONSE_ACK = 0x20, //Response ACK
|
||||||
|
NOT_USED = 0xff
|
||||||
|
};
|
||||||
|
|
||||||
|
enum COMMAND: uint8_t
|
||||||
|
{
|
||||||
|
MODULE_READY = 0x01,
|
||||||
|
MODULE_STATE = 0x02,
|
||||||
|
MODULE_MODE = 0x03,
|
||||||
|
MODULE_SET_CONFIG = 0x04,
|
||||||
|
MODULE_GET_CONFIG = 0x06,
|
||||||
|
CHANNELS_FAILSAFE_DATA = 0x07,
|
||||||
|
TELEMETRY_DATA = 0x09,
|
||||||
|
SEND_COMMAND = 0x0C,
|
||||||
|
COMMAND_RESULT = 0x0D,
|
||||||
|
// MODULE_POWER_STATUS = 0x0F,
|
||||||
|
// MODULE_VERSION = 0x1F,
|
||||||
|
MODULE_VERSION = 0x20,
|
||||||
|
VIRTUAL_FAILSAFE = 0x99, // virtual command used to trigger failsafe
|
||||||
|
UNDEFINED = 0xFF
|
||||||
|
};
|
||||||
|
|
||||||
|
// one byte frames for request queue
|
||||||
|
struct Frame
|
||||||
|
{
|
||||||
|
enum COMMAND command;
|
||||||
|
enum FRAME_TYPE frameType;
|
||||||
|
uint8_t payload;
|
||||||
|
uint8_t frameNumber;
|
||||||
|
bool useFrameNumber;
|
||||||
|
uint8_t payloadSize;
|
||||||
|
};
|
||||||
|
|
||||||
|
union AfhdsFrameData;
|
||||||
|
|
||||||
|
PACK(struct AfhdsFrame {
|
||||||
|
uint8_t startByte;
|
||||||
|
uint8_t address;
|
||||||
|
uint8_t frameNumber;
|
||||||
|
uint8_t frameType;
|
||||||
|
uint8_t command;
|
||||||
|
uint8_t value;
|
||||||
|
|
||||||
|
AfhdsFrameData* GetData()
|
||||||
|
{
|
||||||
|
return reinterpret_cast<AfhdsFrameData*>(&value);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
// simple fifo implementation because Pulses is used as member of union and can
|
||||||
|
// not be non trivial type
|
||||||
|
struct CommandFifo {
|
||||||
|
Frame commandFifo[8];
|
||||||
|
volatile uint32_t setIndex;
|
||||||
|
volatile uint32_t getIndex;
|
||||||
|
|
||||||
|
void clearCommandFifo();
|
||||||
|
Frame* getCommand();
|
||||||
|
|
||||||
|
inline uint32_t nextIndex(uint32_t idx) const
|
||||||
|
{
|
||||||
|
return (idx + 1) & (sizeof(commandFifo) / sizeof(commandFifo[0]) - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint32_t prevIndex(uint32_t idx) const
|
||||||
|
{
|
||||||
|
if (idx == 0) {
|
||||||
|
return (sizeof(commandFifo) / sizeof(commandFifo[0]) - 1);
|
||||||
|
}
|
||||||
|
return (idx - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool isEmpty() const { return (getIndex == setIndex); }
|
||||||
|
|
||||||
|
inline void skip() { getIndex = nextIndex(getIndex); }
|
||||||
|
|
||||||
|
void enqueueACK(COMMAND command, uint8_t frameNumber);
|
||||||
|
|
||||||
|
void enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
|
||||||
|
uint8_t byteContent);
|
||||||
|
};
|
||||||
|
|
||||||
|
struct FrameTransport
|
||||||
|
{
|
||||||
|
ByteTransport trsp;
|
||||||
|
void* trsp_buffer;
|
||||||
|
|
||||||
|
uint8_t crc;
|
||||||
|
// uint8_t timeout;
|
||||||
|
uint8_t esc_state;
|
||||||
|
|
||||||
|
void init(ByteTransport::Type t, void* buffer);
|
||||||
|
void clear();
|
||||||
|
|
||||||
|
void sendByte(uint8_t b);
|
||||||
|
void putBytes(uint8_t* data, int length);
|
||||||
|
|
||||||
|
void putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data,
|
||||||
|
uint8_t dataLength, uint8_t frameIndex);
|
||||||
|
|
||||||
|
uint32_t getFrameSize() { return trsp.getSize(trsp_buffer); }
|
||||||
|
|
||||||
|
bool processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
|
||||||
|
uint8_t& rxBufferCount, uint8_t maxSize);
|
||||||
|
};
|
||||||
|
|
||||||
|
class Transport
|
||||||
|
{
|
||||||
|
enum State {
|
||||||
|
UNKNOWN = 0,
|
||||||
|
SENDING_COMMAND,
|
||||||
|
AWAITING_RESPONSE,
|
||||||
|
IDLE
|
||||||
|
};
|
||||||
|
|
||||||
|
FrameTransport trsp;
|
||||||
|
CommandFifo fifo;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Internal operation state one of UNKNOWN, SENDING_COMMAND, AWAITING_RESPONSE, IDLE
|
||||||
|
* Used to avoid sending commands when not allowed to
|
||||||
|
*/
|
||||||
|
State operationState;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Current frame index
|
||||||
|
*/
|
||||||
|
uint8_t frameIndex;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Actual repeat count for requested command/operation - incremented by every
|
||||||
|
* attempt sending anything
|
||||||
|
*/
|
||||||
|
uint16_t repeatCount;
|
||||||
|
|
||||||
|
bool handleReply(uint8_t* buffer, uint8_t len);
|
||||||
|
|
||||||
|
public:
|
||||||
|
void init(ByteTransport::Type t, void* buffer);
|
||||||
|
|
||||||
|
void clear();
|
||||||
|
|
||||||
|
void sendFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data = nullptr,
|
||||||
|
uint8_t dataLength = 0);
|
||||||
|
|
||||||
|
void enqueue(COMMAND command, FRAME_TYPE frameType, bool useData = false,
|
||||||
|
uint8_t byteContent = 0);
|
||||||
|
|
||||||
|
uint8_t* getFrameBuffer() { return (uint8_t*)trsp.trsp_buffer; }
|
||||||
|
uint32_t getFrameSize() { return trsp.getFrameSize(); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Process retransmissions
|
||||||
|
* error: transport is in error state
|
||||||
|
* @return true if something was just sent
|
||||||
|
*/
|
||||||
|
bool handleRetransmissions(bool& error);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Process queue backlog
|
||||||
|
* @return true if something was just sent
|
||||||
|
*/
|
||||||
|
bool processQueue();
|
||||||
|
|
||||||
|
bool processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
|
||||||
|
uint8_t& rxBufferCount, uint8_t maxSize);
|
||||||
|
};
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -29,7 +29,7 @@
|
|||||||
#include "pxx2.h"
|
#include "pxx2.h"
|
||||||
#include "multi.h"
|
#include "multi.h"
|
||||||
#include "afhds2.h"
|
#include "afhds2.h"
|
||||||
#include "afhds3_transport.h"
|
#include "afhds3_module.h"
|
||||||
#include "modules_helpers.h"
|
#include "modules_helpers.h"
|
||||||
#include "ff.h"
|
#include "ff.h"
|
||||||
#include "hal/module_driver.h"
|
#include "hal/module_driver.h"
|
||||||
|
|||||||
Reference in New Issue
Block a user