Merge with latest 2.3

This commit is contained in:
Bertrand Songis
2020-10-07 21:01:45 +02:00
376 changed files with 10566 additions and 8633 deletions
+526 -316
View File
@@ -18,85 +18,343 @@
* GNU General Public License for more details.
*/
#include "afhds3.h"
#include "../debug.h"
#include "../definitions.h"
#include <cstdio>
#define FAILSAFE_HOLD 1
#define FAILSAFE_CUSTOM 2
namespace afhds3 {
#define MAX_RETRIES_AFHDS3 5
extern void processFlySkySensor(const uint8_t * packet, uint8_t type);
uint8_t ModuleModeToState [] = {
ModuleState::STATE_READY, //not defined
ModuleState::STATE_STANDBY, //MODULE_MODE_E::STANDBY
ModuleState::STATE_BINDING, //MODULE_MODE_E::BIND
ModuleState::STATE_UPDATING_RX, //MODULE_MODE_E::RX_UPDATE
ModuleState::STATE_SYNC_DONE, //MODULE_MODE_E::RUN
extern void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us, bool inverted);
namespace afhds3
{
static const char* const moduleStateText[] =
{
"Not ready",
"HW Error",
"Binding",
"Connecting",
"Connected",
"Standby",
"Waiting for update",
"Updating",
"Updating RX",
"Updating RX failed",
"Testing",
"Ready",
"HW test"
};
static const char* const moduleStateText[] = {
"",
"HW Error",
"Binding",
"Connecting",
"Connected",
"Standby",
"Waiting for update",
"Updating",
"Updating RX",
"Updating RX failed",
"Testing"
"Not ready",
"Ready",
"HW test"
static const char* const powerSourceText[] =
{
"Unknown",
"Internal",
"External"
};
const char* afhds3::getState() {
if(data->state <= ModuleState::STATE_READY) return moduleStateText[data->state];
return "Unknown";
static const COMMAND periodicRequestCommands[] =
{
COMMAND::MODULE_STATE,
COMMAND::MODULE_POWER_STATUS,
COMMAND::MODULE_GET_CONFIG,
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
PulsesData* AFHDS3PulsesData[EXTERNAL_MODULE + 1] = { nullptr, nullptr };
//friends function that can access telemetry parsing method
void processTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize)
{
if (AFHDS3PulsesData[module]) {
AFHDS3PulsesData[module]->processTelemetryData(data, rxBuffer, rxBufferCount, maxSize);
}
}
void afhds3::putByte(uint8_t byte) {
this->putBytes(&byte, 1);
void CommandFifo::clearCommandFifo()
{
memclear(commandFifo, sizeof(commandFifo));
setIndex = getIndex = 0;
}
void afhds3::putBytes(uint8_t* data, int length) {
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 PulsesData::getStatusString(char * buffer) const
{
strcpy(buffer, this->state <= ModuleState::STATE_READY ? moduleStateText[this->state] : "Unknown");
}
void PulsesData::getPowerStatus(char* buffer) const
{
strcpy(buffer, this->powerSource <= MODULE_POWER_SOURCE::EXTERNAL ? powerSourceText[this->powerSource] : "Unknown");
}
void PulsesData::processTelemetryData(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize)
{
if (rxBufferCount == 0 && byte != AfhdsSpecialChars::START) {
TRACE("AFHDS3 [SKIP] %02X", byte);
this->esc_state = 0;
return;
}
if (byte == AfhdsSpecialChars::ESC) {
this->esc_state = rxBufferCount;
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 PulsesData::isConnectedUnicast()
{
return cfg.config.telemetry == TELEMETRY::TELEMETRY_ENABLED && this->state == ModuleState::STATE_SYNC_DONE;
}
bool PulsesData::isConnectedMulticast()
{
return cfg.config.telemetry == TELEMETRY::TELEMETRY_DISABLED && this->state == ModuleState::STATE_SYNC_RUNNING;
}
void PulsesData::setupFrame()
{
if (operationState == State::AWAITING_RESPONSE) {
if (repeatCount++ < MAX_RETRIES_AFHDS3) {
return; //re-send
}
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;
}
repeatCount = 0;
if (this->state == ModuleState::STATE_NOT_READY) {
TRACE("AFHDS3 [GET MODULE READY]");
putFrame(COMMAND::MODULE_READY, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
//check waiting commands
if (!isEmpty()) {
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: %d frameType %d, useFrameNumber %d frame Number %d size %d", f.command, f.frameType, f.useFrameNumber, f.frameNumber, f.payloadSize);
return;
}
//config should be loaded already
if (syncSettings()) {
return;
}
//if module is ready but not started
if (this->state == ModuleState::STATE_READY || this->state == ModuleState::STATE_STANDBY) {
cmdCount = 0;
repeatCount = 0;
requestInfoAndRun(true);
return;
}
::ModuleSettingsMode moduleMode = getModuleMode(module_index);
if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_BIND) {
if (state != STATE_BINDING) {
TRACE("AFHDS3 [BIND]");
setConfigFromModel();
putFrame(COMMAND::MODULE_SET_CONFIG, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer, sizeof(cfg.buffer));
requestedModuleMode = MODULE_MODE_E::BIND;
enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true, requestedModuleMode);
return;
}
}
else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_RANGECHECK) {
if (cfg.config.runPower != RUN_POWER::RUN_POWER_FIRST) {
TRACE("AFHDS3 [RANGE CHECK]");
cfg.config.runPower = RUN_POWER::RUN_POWER_FIRST;
uint8_t data[] = { 0x13, 0x20, 0x02, cfg.config.runPower, 0 };
TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return;
}
}
else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_NORMAL) { //exit bind
if (state == STATE_BINDING) {
TRACE("AFHDS3 [EXIT BIND]");
requestedModuleMode = MODULE_MODE_E::RUN;
putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &requestedModuleMode, 1);
return;
}
}
bool isConnected = isConnectedUnicast() || isConnectedMulticast();
if (cmdCount++ >= 150) {
cmdCount = 0;
if (cmdIndex >= sizeof(periodicRequestCommands)) {
cmdIndex = 0;
}
COMMAND cmd = periodicRequestCommands[cmdIndex++];
if (cmd == COMMAND::VIRTUAL_FAILSAFE) {
if (isConnected) {
if (isConnectedMulticast()) {
TRACE("AFHDS ONE WAY FAILSAFE");
uint16_t failSafe[MAX_CHANNELS + 1] = { ((MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0 };
setFailSafe((int16_t*) (&failSafe[1]));
putFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*) failSafe, MAX_CHANNELS * 2 + 2);
}
else {
TRACE("AFHDS TWO WAYS FAILSAFE");
uint8_t failSafe[3 + MAX_CHANNELS * 2] = { 0x11, 0x60, MAX_CHANNELS * 2 };
setFailSafe((int16_t*) (failSafe + 3));
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, failSafe, 3 + MAX_CHANNELS * 2);
}
}
else {
putFrame(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
}
}
else
{
putFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
}
}
else if (isConnected) {
sendChannelsData();
}
else {
//default frame - request state
putFrame(MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
}
}
void PulsesData::init(uint8_t moduleIndex, bool resetFrameCount)
{
module_index = moduleIndex;
AFHDS3PulsesData[module_index] = this;
//clear local vars because it is member of union
moduleData = &g_model.moduleData[module_index];
operationState = State::UNKNOWN;
state = ModuleState::STATE_NOT_READY;
clearFrameData();
}
void PulsesData::clearFrameData()
{
TRACE("AFHDS3 clearFrameData");
reset();
clearCommandFifo();
repeatCount = 0;
cmdCount = 0;
cmdIndex = 0;
this->frame_index = 1;
this->timeout = 0;
this->esc_state = 0;
}
void PulsesData::putBytes(uint8_t* data, int length)
{
for (int i = 0; i < length; i++) {
uint8_t byte = data[i];
this->data->crc += byte;
this->crc += byte;
if (END == byte) {
*this->data->ptr++ = ESC;
*this->data->ptr++ = ESC_END;
sendByte(ESC);
sendByte(ESC_END);
}
else if (ESC == byte) {
*this->data->ptr++ = ESC;
*this->data->ptr++ = ESC_ESC;
sendByte(ESC);
sendByte(ESC_ESC);
}
else {
*this->data->ptr++ = byte;
sendByte(byte);
}
}
}
void afhds3::putHeader(COMMAND command, FRAME_TYPE frame) {
void PulsesData::putFrame(COMMAND command, FRAME_TYPE frame, uint8_t* data, uint8_t dataLength, uint8_t* frameIndex)
{
//header
operationState = State::SENDING_COMMAND;
data->ptr = this->data->pulses;
data->crc = 0;
*data->ptr++ = END;
uint8_t buffer[] = { FrameAddress, data->frame_index, frame, command};
reset();
this->crc = 0;
sendByte(START);
if (frameIndex == nullptr) {
frameIndex = &this->frame_index;
}
uint8_t buffer[] = {FrameAddress, *frameIndex, frame, command};
putBytes(buffer, 4);
}
//payload
if (dataLength > 0) {
putBytes(data, dataLength);
}
//footer
uint8_t crcValue = this->crc ^ 0xff;
putBytes(&crcValue, 1);
sendByte(END);
*frameIndex = *frameIndex + 1;
void afhds3::putFooter() {
putByte(data->crc ^ 0xff);
*data->ptr++ = END;
data->frame_index++;
switch((FRAME_TYPE)data->pulses[3])
{
switch (frame) {
case FRAME_TYPE::REQUEST_GET_DATA:
case FRAME_TYPE::REQUEST_SET_EXPECT_ACK:
case FRAME_TYPE::REQUEST_SET_EXPECT_DATA:
@@ -105,25 +363,8 @@ void afhds3::putFooter() {
default:
operationState = State::IDLE;
}
}
void afhds3::putFrame(COMMAND command, FRAME_TYPE frame, uint8_t* data, uint8_t dataLength){
putHeader(command, frame);
if(dataLength > 0) putBytes(data, dataLength);
putFooter();
}
void afhds3::addToQueue(COMMAND command, FRAME_TYPE frameType, uint8_t* data, uint8_t dataLength) {
request* r = new request(command, frameType, data, dataLength);
commandQueue.push(r);
}
void afhds3::clearQueue() {
while(!commandQueue.empty()) {
request* r = commandQueue.front();
delete r;
commandQueue.pop();
}
flush();
}
bool checkCRC(const uint8_t* data, uint8_t size)
@@ -136,58 +377,73 @@ bool checkCRC(const uint8_t* data, uint8_t size)
return (crc ^ 0xff) == data[size];
}
bool containsData(enum FRAME_TYPE frameType) {
return frameType == FRAME_TYPE::RESPONSE_DATA || frameType == FRAME_TYPE::REQUEST_SET_EXPECT_DATA ||
frameType == FRAME_TYPE::REQUEST_SET_EXPECT_ACK || frameType == FRAME_TYPE::REQUEST_SET_EXPECT_DATA ||
frameType == FRAME_TYPE::REQUEST_SET_NO_RESP;
bool containsData(enum FRAME_TYPE frameType)
{
return (frameType == FRAME_TYPE::RESPONSE_DATA ||
frameType == FRAME_TYPE::REQUEST_SET_EXPECT_DATA ||
frameType == FRAME_TYPE::REQUEST_SET_EXPECT_ACK ||
frameType == FRAME_TYPE::REQUEST_SET_EXPECT_DATA ||
frameType == FRAME_TYPE::REQUEST_SET_NO_RESP);
}
void afhds3::setState(uint8_t state) {
if(state == data->state) return;
uint8_t oldState = data->state;
data->state = state;
if(oldState == ModuleState::STATE_BINDING) {
if(operationCallback != nullptr) {
operationCallback(false);
operationCallback = nullptr;
}
void PulsesData::setState(uint8_t state)
{
if (state == this->state) {
return;
}
uint8_t oldState = this->state;
this->state = state;
if (oldState == ModuleState::STATE_BINDING) {
setModuleMode(module_index, ::ModuleSettingsMode::MODULE_MODE_NORMAL);
}
if (state == ModuleState::STATE_NOT_READY) {
operationState = State::UNKNOWN;
}
}
void afhds3::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) {
void PulsesData::requestInfoAndRun(bool send)
{
if (!send) {
enqueue(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
}
enqueue(COMMAND::MODULE_POWER_STATUS, FRAME_TYPE::REQUEST_GET_DATA);
requestedModuleMode = MODULE_MODE_E::RUN;
enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true, (uint8_t) MODULE_MODE_E::RUN);
if (send) {
putFrame(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
}
}
if(!checkCRC(rxBuffer, rxBufferCount - 2)) {
void PulsesData::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
{
if (!checkCRC(rxBuffer, rxBufferCount - 2)) {
TRACE("AFHDS3 [INVALID CRC]");
return;
}
AfhdsFrame* responseFrame = reinterpret_cast<AfhdsFrame*>(rxBuffer);
AfhdsFrame* requestFrame = reinterpret_cast<AfhdsFrame*>(data->pulses);
uint8_t oldState = data->state;
if(containsData((enum FRAME_TYPE)responseFrame->frameType)) {
switch(responseFrame->command)
{
if (containsData((enum FRAME_TYPE) responseFrame->frameType)) {
switch (responseFrame->command) {
case COMMAND::MODULE_READY:
TRACE("AFHDS3 [MODULE_READY] %02X", responseFrame->value);
setState(responseFrame->value == MODULE_STATUS_READY ? ModuleState::STATE_READY : ModuleState::STATE_NOT_READY);
if(data->state == ModuleState::STATE_READY && oldState != data->state) {
addToQueue(COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA);
addToQueue(COMMAND::MODULE_POWER_STATUS, FRAME_TYPE::REQUEST_GET_DATA);
addToQueue(COMMAND::MODULE_GET_CONFIG, FRAME_TYPE::REQUEST_GET_DATA);
uint8_t mode = MODULE_MODE_E::RUN;
addToQueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
//addToQueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
if (responseFrame->value == MODULE_STATUS_READY) {
setState(ModuleState::STATE_READY);
requestInfoAndRun();
}
else {
setState(ModuleState::STATE_NOT_READY);
}
break;
case COMMAND::MODULE_GET_CONFIG:
std::memcpy((void*)cfg.buffer, &responseFrame->value, sizeof(cfg.buffer));
TRACE("AFHDS3 [MODULE_GET_CONFIG] telemetry %d", cfg.config.telemetry);
break;
std::memcpy((void*) cfg.buffer, &responseFrame->value, sizeof(cfg.buffer));
TRACE("AFHDS3 [MODULE_GET_CONFIG] bind power %d run power %d mode %d pwm/ppm %d ibus/sbus %d", cfg.config.bindPower, cfg.config.runPower, cfg.config.telemetry, cfg.config.pulseMode, cfg.config.serialMode);
break;
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", version.productNumber, version.hardwereVersion, version.bootloaderVersion, version.firmwareVersion);
break;
case COMMAND::MODULE_POWER_STATUS:
powerSource = (enum MODULE_POWER_SOURCE)responseFrame->value;
powerSource = (enum MODULE_POWER_SOURCE) responseFrame->value;
TRACE("AFHDS3 [MODULE_POWER_STATUS], %d", powerSource);
break;
case COMMAND::MODULE_STATE:
@@ -195,249 +451,225 @@ void afhds3::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) {
setState(responseFrame->value);
break;
case COMMAND::MODULE_MODE:
TRACE("AFHDS3 [MODULE_MODE] %02X", responseFrame->value);
if(responseFrame->value != CMD_RESULT::SUCCESS) {
setState(ModuleState::STATE_NOT_READY);
TRACE("AFHDS3 [MODULE_MODE] %02X", responseFrame->value);
if (responseFrame->value != CMD_RESULT::SUCCESS) {
setState(ModuleState::STATE_NOT_READY);
}
else {
if (requestedModuleMode == MODULE_MODE_E::RUN) {
enqueue(COMMAND::MODULE_GET_CONFIG, FRAME_TYPE::REQUEST_GET_DATA);
enqueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
}
requestedModuleMode = MODULE_MODE_UNKNOWN;
}
else addToQueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
break;
case COMMAND::MODULE_SET_CONFIG:
if(responseFrame->value != CMD_RESULT::SUCCESS) {
if (responseFrame->value != CMD_RESULT::SUCCESS) {
setState(ModuleState::STATE_NOT_READY);
}
TRACE("AFHDS3 [MODULE_SET_CONFIG], %02X", responseFrame->value);
break;
case COMMAND::TELEMETRY_DATA:
{
uint8_t* telemetry = &responseFrame->value;
uint8_t length = telemetry[1];
uint8_t id = telemetry[2];
if(telemetry[0] == 0x22) {
if(length == 5) {
if(id == 0xFA) telemetry[2] = 0xF8; //remap to afhds3 snr
processSensor(telemetry + 2, 0xAA);
if (telemetry[0] == 0x22) {
telemetry++;
while (telemetry < rxBuffer + rxBufferCount) {
uint8_t length = telemetry[0];
uint8_t id = telemetry[1];
if (id == 0xFE) {
id = 0xF7; //use new id because format is different
}
if (length == 0 || telemetry + length > rxBuffer + rxBufferCount) {
break;
}
if (length == 4) {
//one byte value fill missing byte
uint8_t data[] = { id, telemetry[2], telemetry[3], 0 };
::processFlySkySensor(data, 0xAA);
}
if (length == 5) {
if (id == 0xFA) {
telemetry[1] = 0xF8; //remap to afhds3 snr
}
::processFlySkySensor(telemetry + 1, 0xAA);
}
else if (length == 6 && id == FRM302_STATUS) {
//convert to ibus
uint16_t t = (uint16_t) (((int16_t) telemetry[3] * 10) + 400);
uint8_t dataTemp[] = { ++id, telemetry[2], (uint8_t) (t & 0xFF), (uint8_t) (t >> 8) };
::processFlySkySensor(dataTemp, 0xAA);
uint8_t dataVoltage[] = { ++id, telemetry[2], telemetry[4], telemetry[5] };
::processFlySkySensor(dataVoltage, 0xAA);
}
else if (length == 7) {
::processFlySkySensor(telemetry + 1, 0xAC);
}
telemetry += length;
}
else if(length == 6 && id == FRM302_STATUS) {
//convert to ibus
uint16_t t = (uint16_t)(((int16_t)telemetry[4] *10) + 400);
uint8_t dataTemp[] = { ++id, telemetry[3], (uint8_t)(t & 0xFF), (uint8_t)(t >> 8)};
processSensor(dataTemp, 0xAA);
uint8_t dataVoltage[] = { ++id, telemetry[3], telemetry[5], telemetry[6] };
processSensor(dataVoltage, 0xAA);
}
else if(length == 7) processSensor(telemetry + 2, 0xAC);
}
}
break;
case COMMAND::COMMAND_RESULT:
{
//AfhdsFrameData* respData = responseFrame->GetData();
//TRACE("COMMAND RESULT %02X result %d datalen %d", respData->CommandResult.command, respData->CommandResult.result, respData->CommandResult.respLen);
}
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);
}
else if (responseFrame->frameType == FRAME_TYPE::REQUEST_SET_EXPECT_ACK) {
addToQueue((enum COMMAND)responseFrame->command, FRAME_TYPE::RESPONSE_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);
}
else if(responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA || responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) {
if(operationState == State::AWAITING_RESPONSE /* && requestFrame->command == responseFrame->command*/) {
else if (responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA || responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) {
if (operationState == State::AWAITING_RESPONSE) {
operationState = State::IDLE;
}
}
}
void afhds3::onDataReceived(uint8_t byte, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize) {
if (rxBufferCount == 0 && byte != AfhdsSpecialChars::START) {
TRACE("AFHDS3 [SKIP] %02X", byte);
data->esc_state = 0;
return;
}
if (byte == AfhdsSpecialChars::ESC) {
data->esc_state = rxBufferCount;
return;
}
if (rxBufferCount > 1 && byte == AfhdsSpecialChars::END) {
rxBuffer[rxBufferCount++] = byte;
parseData(rxBuffer, rxBufferCount);
rxBufferCount = 0;
return;
}
if (data->esc_state && byte == AfhdsSpecialChars::ESC_END) byte = AfhdsSpecialChars::END;
else if (data->esc_state && byte == AfhdsSpecialChars::ESC_ESC) byte = AfhdsSpecialChars::ESC;
//reset esc index
data->esc_state = 0;
if (rxBufferCount >= maxSize) {
TRACE("AFHDS3 [BUFFER OVERFLOW]");
rxBufferCount = 0;
}
rxBuffer[rxBufferCount++] = byte;
}
void afhds3::trace(const char* message) {
char buffer[256];
char *pos = buffer;
for (int i = 0; i < data->ptr - data->pulses; i++) {
pos += std::snprintf(pos, buffer + sizeof(buffer) - pos, "%02X ", data->pulses[i]);
}
(*pos) = 0;
TRACE("%s size = %d data %s", message, data->ptr - data->pulses, buffer);
}
void afhds3::setupPulses() {
//TRACE("%d state %d repeatCount %d", (int)operationState, this->data->state, repeatCount);
if(operationState == State::AWAITING_RESPONSE) {
if(repeatCount++ < 5) return; //re-send
else reset(false);
}
data->ptr = data->pulses;
repeatCount = 0;
if (operationState == State::UNKNOWN || data->state == ModuleState::STATE_NOT_READY) {
putFrame(COMMAND::MODULE_READY, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
//check waiting commands
if(!commandQueue.empty()) {
request* r = commandQueue.front();
commandQueue.pop();
putFrame(r->command, r->frameType, r->payload, r->payloadSize);
trace("AFHDS3 [CMD QUEUE] data");
delete r;
return;
}
//config should be loaded already
if(syncSettings()) return;
switch(data->state) {
case ModuleState::STATE_READY:
reset();
break;
case ModuleState::STATE_HW_ERROR:
case ModuleState::STATE_BINDING:
case ModuleState::STATE_UPDATING_RX:
case ModuleState::STATE_UPDATING_WAIT:
case ModuleState::STATE_UPDATING_MOD:
case ModuleState::STATE_SYNC_RUNNING:
case ModuleState::STATE_HW_TEST:
if(idleCount++ % 100 == 0) putFrame(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
break;
case ModuleState::STATE_SYNC_DONE:
if(cfg.config.telemetry && idleCount++ % 800 == 0) {
uint8_t failSafe[3+MAX_CHANNELS*2] = {0x60, 0x12};
uint8_t channels = setFailSafe((int16_t*)(failSafe + 3));
failSafe[2] = channels *2;
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, failSafe, 3 + channels*2);
//trace("AFHDS3 [AFHDS3 SET FAILSAFE] data");
}
else sendChannelsData();
break;
}
inline bool isSbus(uint8_t mode)
{
return (mode & 1);
}
bool afhds3::syncSettings() {
if (moduleData->afhds3.runPower != cfg.config.runPower) {
inline bool isPWM(uint8_t mode)
{
return mode < 2;
}
RUN_POWER PulsesData::getMaxRunPower() const
{
if (powerSource == MODULE_POWER_SOURCE::EXTERNAL) {
return RUN_POWER::PLUS_33dBm;
}
return RUN_POWER::PLUS_27dbm;
}
RUN_POWER PulsesData::actualRunPower() const
{
uint8_t actualRfPower = cfg.config.runPower;
if (getMaxRunPower() < actualRfPower) {
actualRfPower = getMaxRunPower();
}
return (RUN_POWER) actualRfPower;
}
RUN_POWER PulsesData::getRunPower() const
{
RUN_POWER targetPower = (RUN_POWER) moduleData->afhds3.runPower;
if (getMaxRunPower() < targetPower) {
targetPower = getMaxRunPower();
}
return targetPower;
}
bool PulsesData::syncSettings()
{
RUN_POWER targetPower = getRunPower();
/*not sure if we need to prevent them in bind mode*/
if (getModuleMode(module_index) != ::ModuleSettingsMode::MODULE_MODE_BIND && targetPower != cfg.config.runPower) {
cfg.config.runPower = moduleData->afhds3.runPower;
uint8_t data[] = { 0x20, 0x13, 0x01, moduleData->afhds3.runPower };
uint8_t data[] = { 0x13, 0x20, 0x02, moduleData->afhds3.runPower, 0 };
TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
if(moduleData->afhds3.rxFreq != cfg.config.pwmFreq) {
cfg.config.pwmFreq = moduleData->afhds3.rxFreq;
uint8_t data[] = {0x70, 0x17, 0x02, (uint8_t)(moduleData->afhds3.rxFreq & 0xFF), (uint8_t)(moduleData->afhds3.rxFreq >> 8)};
//other settings only in 2 way mode (state must be synchronized)
if (this->state != ModuleState::STATE_SYNC_DONE) {
return false;
}
if (moduleData->afhds3.rxFreq() != cfg.config.pwmFreq) {
cfg.config.pwmFreq = moduleData->afhds3.rxFreq();
uint8_t data[] = { 0x17, 0x70, 0x02, (uint8_t) (moduleData->afhds3.rxFreq() & 0xFF), (uint8_t) (moduleData->afhds3.rxFreq() >> 8) };
TRACE("AFHDS3 SET RX FREQ");
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
PULSE_MODE modelPulseMode = moduleData->afhds3.isPWM() ? PULSE_MODE::PWM: PULSE_MODE::PPM_MODE;
if(modelPulseMode != cfg.config.pulseMode) {
PULSE_MODE modelPulseMode = isPWM(moduleData->subType) ? PULSE_MODE::PWM_MODE : PULSE_MODE::PPM_MODE;
if (modelPulseMode != cfg.config.pulseMode) {
cfg.config.pulseMode = modelPulseMode;
TRACE("AFHDS3 PWM/PPM %d", modelPulseMode);
uint8_t data[] = {0x70, 0x16, 0x01, (uint8_t)(modelPulseMode)};
uint8_t data[] = { 0x16, 0x70, 0x01, (uint8_t) (modelPulseMode) };
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
SERIAL_MODE modelSerialMode = moduleData->afhds3.isSbus() ? SERIAL_MODE::SBUS_MODE : SERIAL_MODE::IBUS;
if(modelSerialMode != cfg.config.serialMode) {
SERIAL_MODE modelSerialMode = isSbus(moduleData->subType) ? SERIAL_MODE::SBUS_MODE : SERIAL_MODE::IBUS;
if (modelSerialMode != cfg.config.serialMode) {
cfg.config.serialMode = modelSerialMode;
TRACE("AFHDS3 IBUS/SBUS %d", modelSerialMode);
uint8_t data[] = {0x70, 0x18, 0x01, (uint8_t)(modelSerialMode)};
uint8_t data[] = { 0x18, 0x70, 0x01, (uint8_t) (modelSerialMode) };
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
if(moduleData->afhds3.failsafeTimeout != cfg.config.failSafeTimout) {
if (moduleData->afhds3.failsafeTimeout != cfg.config.failSafeTimout) {
moduleData->afhds3.failsafeTimeout = cfg.config.failSafeTimout;
uint8_t data[] = { 0x60, 0x12, 0x02, (uint8_t)(moduleData->afhds3.failsafeTimeout & 0xFF), (uint8_t)(moduleData->afhds3.failsafeTimeout >> 8) };
uint8_t data[] = { 0x12, 0x60, 0x02, (uint8_t) (moduleData->afhds3.failsafeTimeout & 0xFF), (uint8_t) (moduleData->afhds3.failsafeTimeout >> 8) };
putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
TRACE("AFHDS3 TRACE FAILSAFE TMEOUT");
TRACE("AFHDS3 FAILSAFE TMEOUT, %d", moduleData->afhds3.failsafeTimeout);
return true;
}
return false;
}
void afhds3::sendChannelsData() {
void PulsesData::sendChannelsData()
{
uint8_t channels_start = moduleData->channelsStart;
uint8_t channelsCount = 8 + moduleData->channelsCount;
uint8_t channels_last = channels_start + channelsCount;
int16_t buffer[MAX_CHANNELS + 1] = { ((MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::CHANNELS), 0 };
uint8_t channels[2*((8 + moduleData->channelsCount) + 1)];
channels[0] = 0x01;
channels[1] = channelsCount;
for(uint8_t channel = channels_start; channel < channels_last; channel++) {
int16_t channelValue = convert(getChannelValue(channel));
*((int16_t*)(channels + (channel * 2) + 2)) = channelValue;
for (uint8_t channel = channels_start, index = 1; channel < channels_last; channel++, index++) {
int16_t channelValue = convert(::getChannelValue(channel));
buffer[index] = channelValue;
}
putFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP, channels, sizeof(channels));
putFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*) buffer, sizeof(buffer));
}
void afhds3::bind(bindCallback_t callback) {
operationCallback = callback;
TRACE("AFHDS3 [BIND]");
setModelData();
addToQueue(COMMAND::MODULE_SET_CONFIG, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer, sizeof(cfg.buffer));
uint8_t cmd = MODULE_MODE_E::BIND;
addToQueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &cmd, 1);
}
void afhds3::range(bindCallback_t callback) {
TRACE("AFHDS3 [RANGE CHECK] NOT IMPLEMENTED");
}
void afhds3::cancel() {
if(operationCallback!=nullptr) operationCallback(false);
reset(false);
}
void afhds3::stop() {
void PulsesData::stop()
{
TRACE("AFHDS3 STOP");
uint8_t cmd = MODULE_MODE_E::STANDBY;
putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &cmd, 1);
reset(true);
requestedModuleMode = MODULE_MODE_E::STANDBY;
putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &requestedModuleMode, 1);
}
void afhds3::setToDefault() {
moduleData->afhds3.bindPower = BIND_POWER::MIN_0dbm;
moduleData->afhds3.runPower = RUN_POWER::PLUS_15bBm;
moduleData->afhds3.emi = EMI_STANDARD::FCC;
moduleData->afhds3.telemetry = TELEMETRY::TELEMETRY_ENABLED;
moduleData->afhds3.rxFreq = 50;
moduleData->afhds3.failsafeTimeout = 1000;
moduleData->channelsCount = 14 - 8;
moduleData->failsafeMode = FAILSAFE_HOLD;
//" PWM+i"" PWM+s"" PPM+i"" PPM+s"
moduleData->afhds3.mode = 0;
for (uint8_t channel = 0; channel < MAX_OUTPUT_CHANNELS; channel++) {
failsafeChannels[channel] = 0;
}
void PulsesData::setConfigFromModel()
{
cfg.config.bindPower = moduleData->afhds3.bindPower;
cfg.config.runPower = getRunPower();
cfg.config.emiStandard = EMI_STANDARD::FCC;
cfg.config.telemetry = moduleData->afhds3.telemetry; //always use bidirectional mode
cfg.config.pwmFreq = moduleData->afhds3.rxFreq();
cfg.config.serialMode = isSbus(moduleData->subType) ? SERIAL_MODE::SBUS_MODE : SERIAL_MODE::IBUS;
cfg.config.pulseMode = isPWM(moduleData->subType) ? PULSE_MODE::PWM_MODE : PULSE_MODE::PPM_MODE;
//use max channels - because channel count can not be changed after bind
cfg.config.channelCount = MAX_CHANNELS;
cfg.config.failSafeTimout = moduleData->afhds3.failsafeTimeout;
setFailSafe(cfg.config.failSafeMode);
}
int16_t afhds3::convert(int channelValue) {
inline int16_t PulsesData::convert(int channelValue)
{
//pulseValue = limit<uint16_t>(0, 988 + ((channelValue + 1024) / 2), 0xfff);
//988 - 750 = 238
//238 * 20 = 4760
@@ -446,51 +678,29 @@ int16_t afhds3::convert(int channelValue) {
// 988 ---- 2012
//-10240 ---- 10240
//-1024 ---- 1024
return limit<int16_t>(FAILSAFE_MIN, channelValue*10, FAILSAFE_MAX);
return ::limit<int16_t>(FAILSAFE_MIN, channelValue * 10, FAILSAFE_MAX);
}
void afhds3::setModelData() {
cfg.config.bindPower = moduleData->afhds3.bindPower;
cfg.config.runPower = moduleData->afhds3.runPower;
cfg.config.emiStandard = moduleData->afhds3.emi;
cfg.config.telemetry = moduleData->afhds3.telemetry;
cfg.config.pwmFreq = moduleData->afhds3.rxFreq;
cfg.config.serialMode = moduleData->afhds3.isSbus() ? SERIAL_MODE::SBUS_MODE : SERIAL_MODE::IBUS;
cfg.config.pulseMode = moduleData->afhds3.isPWM() ? PULSE_MODE::PWM: PULSE_MODE::PPM_MODE;
cfg.config.channelCount = 8 + moduleData->channelsCount;
cfg.config.failSafeTimout = moduleData->afhds3.failsafeTimeout;
setFailSafe(cfg.config.failSafeMode);
}
uint8_t afhds3::setFailSafe(int16_t* target) {
uint8_t PulsesData::setFailSafe(int16_t* target)
{
int16_t pulseValue = 0;
uint8_t channels_start = moduleData->channelsStart;
uint8_t channels_last = channels_start + 8 + moduleData->channelsCount;;
uint8_t channels_last = channels_start + 8 + moduleData->channelsCount;
for (uint8_t channel = channels_start; channel < channels_last; channel++) {
if (moduleData->failsafeMode == FAILSAFE_CUSTOM) pulseValue = convert(failsafeChannels[channel]);
else if (moduleData->failsafeMode == FAILSAFE_HOLD) pulseValue = FAILSAFE_KEEP_LAST;
else pulseValue = convert(getChannelValue(channel));
target[channel-channels_start] = pulseValue;
}
return (uint8_t)(channels_last - channels_start);
}
void afhds3::onModelSwitch() {
//uint8_t cmd = MODULE_MODE_E::STANDBY;
//putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &cmd, 1);
//setModelData();
}
void afhds3::reset(bool resetFrameCount) {
TRACE("AFHDS3 RESET");
clearQueue();
repeatCount = 0;
idleCount = 0;
this->data->state = ModuleState::STATE_NOT_READY;
this->data->frame_index = 1;
this->data->timeout = 0;
this->data->esc_state = 0;
if (moduleData->failsafeMode == FAILSAFE_CUSTOM) {
pulseValue = convert(g_model.failsafeChannels[channel]);
}
else if (moduleData->failsafeMode == FAILSAFE_HOLD) {
pulseValue = FAILSAFE_KEEP_LAST;
}
else {
pulseValue = convert(::getChannelValue(channel));
}
target[channel - channels_start] = pulseValue;
}
//return max channels because channel count can not be change after bind
return (uint8_t) (MAX_CHANNELS);
}
}