piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity
Files
324c8e9eeffaaa76ff9798271b5915969a156dbe
edgetx-gx12/radio/src/pulses/afhds3.cpp
T
Raphael CoefficandGitHub 324c8e9eef fix(PXX2): prevent legacy telemetry polling (S.PORT) if the module driver defines its own method (#2504) 
This is to prevent the S.PORT line to interfere with PXX2 telemetry reception, especially with R9M ACCESS.
2022-10-11 21:36:24 +10:00

965 lines
26 KiB
C++
Raw Blame History

/*
* 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.
*/
#include "afhds3.h"
#include "afhds3_transport.h"
#include "afhds3_config.h"
#include "pulses.h"
#include "../debug.h"
#include "../definitions.h"
#include "telemetry/telemetry.h"
#include "mixer_scheduler.h"
#include "hal/module_driver.h"
#if defined(INTMODULE_USART)
#include "intmodule_serial_driver.h"
#endif
#if defined(AFHDS3_EXT_UART)
#include "extmodule_serial_driver.h"
#else
#include "extmodule_driver.h"
#endif
#define FAILSAFE_HOLD 1
#define FAILSAFE_CUSTOM 2
#define FAILSAFE_KEEP_LAST 0x8000
//get channel value outside of afhds3 namespace
int32_t getChannelValue(uint8_t channel);
void processFlySkySensor(const uint8_t * packet, uint8_t type);
namespace afhds3
{
static uint8_t _phyMode_channels[] = {
18, // CLASSIC_FLCR1_18CH
10, // CLASSIC_FLCR6_10CH
18, // ROUTINE_FLCR1_18CH
8, // ROUTINE_FLCR6_8CH
12, // ROUTINE_LORA_12CH
};
// enum COMMAND_DIRECTION
// {
// RADIO_TO_MODULE = 0,
// MODULE_TO_RADIO = 1
// };
// enum DATA_TYPE
// {
// READY_DT, // 8 bytes 0x01 Not ready 0x02 Ready
// STATE_DT, // See MODULE_STATE
// MODE_DT,
// MOD_CONFIG_DT,
// CHANNELS_DT,
// TELEMETRY_DT,
// MODULE_POWER_DT,
// MODULE_VERSION_DT,
// EMPTY_DT,
// };
//enum used by command response -> translate to ModuleState
enum MODULE_READY_E
{
MODULE_STATUS_UNKNOWN = 0x00,
MODULE_STATUS_NOT_READY = 0x01,
MODULE_STATUS_READY = 0x02
};
enum ModuleState
{
STATE_NOT_READY = 0x00, //virtual
STATE_HW_ERROR = 0x01,
STATE_BINDING = 0x02,
STATE_SYNC_RUNNING = 0x03,
STATE_SYNC_DONE = 0x04,
STATE_STANDBY = 0x05,
STATE_UPDATING_WAIT = 0x06,
STATE_UPDATING_MOD = 0x07,
STATE_UPDATING_RX = 0x08,
STATE_UPDATING_RX_FAILED = 0x09,
STATE_RF_TESTING = 0x0a,
STATE_READY = 0x0b, //virtual
STATE_HW_TEST = 0xff,
};
// used for set command
enum MODULE_MODE_E {
STANDBY = 0x01,
BIND = 0x02, // after bind module will enter run mode
RUN = 0x03,
RX_UPDATE = 0x04, // after successful update module will enter standby mode,
// otherwise hw error will be raised
MODULE_MODE_UNKNOWN = 0xFF
};
enum CMD_RESULT
{
FAILURE = 0x01,
SUCCESS = 0x02,
};
enum CHANNELS_DATA_MODE
{
CHANNELS = 0x01,
FAIL_SAFE = 0x02,
};
PACK(struct ChannelsData {
uint8_t mode;
uint8_t channelsNumber;
int16_t data[AFHDS3_MAX_CHANNELS];
});
union ChannelsData_u
{
ChannelsData data;
uint8_t buffer[sizeof(ChannelsData)];
};
PACK(struct TelemetryData
{
uint8_t sensorType;
uint8_t length;
uint8_t type;
uint8_t semsorID;
uint8_t data[8];
});
enum MODULE_POWER_SOURCE
{
INTERNAL = 0x01,
EXTERNAL = 0x02,
};
PACK(struct ModuleVersion
{
uint32_t productNumber;
uint32_t hardwereVersion;
uint32_t bootloaderVersion;
uint32_t firmwareVersion;
uint32_t rfVersion;
});
PACK(struct CommandResult_s
{
uint16_t command;
uint8_t result;
uint8_t respLen;
});
union AfhdsFrameData
{
uint8_t value;
// Config_s Config;
ChannelsData Channels;
TelemetryData Telemetry;
ModuleVersion Version;
CommandResult_s CommandResult;
};
#define FRM302_STATUS 0x56
class ProtoState
{
public:
/**
* Initialize class for operation
* @param moduleIndex index of module one of INTERNAL_MODULE, EXTERNAL_MODULE
* @param resetFrameCount flag if current frame count should be reseted
*/
void init(uint8_t moduleIndex, void* buffer, const etx_serial_driver_t* drv);
void deinit() { trsp.deinit(); }
/**
* Fills DMA buffers with frame to be send depending on actual state
*/
void setupFrame();
/**
* Sends prepared buffers
*/
void sendFrame() { trsp.sendBuffer(); }
/**
* Fetch telemetry byte
*/
int getTelemetryByte(uint8_t* data) { return trsp.getTelemetryByte(data); }
/**
* Gets actual module status into provided buffer
* @param statusText target buffer for status
*/
void getStatusString(char * statusText) const;
/**
* Sends stop command to prevent any further module operations
*/
void stop();
Config_u* getConfig() { return &cfg; }
void applyConfigFromModel();
protected:
void resetConfig(uint8_t version);
private:
//friendship declaration - use for passing telemetry
friend void processTelemetryData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len);
void processTelemetryData(uint8_t data, uint8_t* buffer, uint8_t* len);
void parseData(uint8_t* rxBuffer, uint8_t rxBufferCount);
void setState(ModuleState state);
bool syncSettings();
void requestInfoAndRun(bool send = false);
uint8_t setFailSafe(int16_t* target);
inline int16_t convert(int channelValue);
void sendChannelsData();
void clearFrameData();
bool isConnected();
bool hasTelemetry();
Transport trsp;
/**
* Index of the module
*/
uint8_t module_index;
/**
* Reported state of the HF module
*/
ModuleState state;
bool modelIDSet;
uint8_t modelID;
/**
* Command count used for counting actual number of commands sent in run mode
*/
uint32_t cmdCount;
/**
* Command index of command to be send when cmdCount reached necessary value
*/
uint32_t cmdIndex;
/**
* Pointer to module config - it is making operations easier and faster
*/
ModuleData* moduleData;
/**
* Actual module configuration - must be requested from module
*/
Config_u cfg;
/**
* Actual module version - must be requested from module
*/
ModuleVersion version;
};
static const char* const moduleStateText[] =
{
"Not ready",
"HW Error",
"Binding",
"Disconnected",
"Connected",
"Standby",
"Waiting for update",
"Updating",
"Updating RX",
"Updating RX failed",
"Testing",
"Ready",
"HW test"
};
static const COMMAND periodicRequestCommands[] =
{
COMMAND::MODULE_STATE,
// COMMAND::MODULE_GET_CONFIG,
COMMAND::VIRTUAL_FAILSAFE
};
//Static collection of afhds3 object instances by module
static ProtoState protoState[NUM_MODULES];
void getStatusString(uint8_t module, char* buffer)
{
return protoState[module].getStatusString(buffer);
}
//friends function that can access telemetry parsing method
void processTelemetryData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len)
{
auto p_state = (ProtoState*)ctx;
p_state->processTelemetryData(data, buffer, len);
}
void ProtoState::getStatusString(char* buffer) const
{
strcpy(buffer, state <= ModuleState::STATE_READY ? moduleStateText[state]
: "Unknown");
}
void ProtoState::processTelemetryData(uint8_t byte, uint8_t* buffer, uint8_t* len)
{
uint8_t maxSize = TELEMETRY_RX_PACKET_SIZE;
if (!trsp.processTelemetryData(byte, buffer, *len, maxSize))
return;
parseData(buffer, *len);
*len = 0;
}
bool ProtoState::isConnected()
{
return this->state == ModuleState::STATE_SYNC_DONE;
}
bool ProtoState::hasTelemetry()
{
if (cfg.version == 0)
return cfg.v0.IsTwoWay;
else
return cfg.v1.IsTwoWay;
}
void ProtoState::setupFrame()
{
bool trsp_error = false;
if (trsp.handleRetransmissions(trsp_error)) return;
if (trsp_error) {
this->state = ModuleState::STATE_NOT_READY;
clearFrameData();
}
if (this->state == ModuleState::STATE_NOT_READY) {
TRACE("AFHDS3 [GET MODULE READY]");
trsp.sendFrame(COMMAND::MODULE_READY, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
// process backlog
if (trsp.processQueue()) return;
// config should be loaded already
if (syncSettings()) { return; }
::ModuleSettingsMode moduleMode = getModuleMode(module_index);
if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_BIND) {
if (state != STATE_BINDING) {
TRACE("AFHDS3 [BIND]");
applyConfigFromModel();
trsp.sendFrame(COMMAND::MODULE_SET_CONFIG,
FRAME_TYPE::REQUEST_SET_EXPECT_DATA, cfg.buffer,
cfg.version == 0 ? sizeof(cfg.v0) : sizeof(cfg.v1));
trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
(uint8_t)MODULE_MODE_E::BIND);
return;
}
}
else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_RANGECHECK) {
TRACE("AFHDS3 [RANGE CHECK] not supported");
}
else if (moduleMode == ::ModuleSettingsMode::MODULE_MODE_NORMAL) {
// if module is ready but not started
if (this->state == ModuleState::STATE_READY) {
trsp.sendFrame(MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
if (!modelIDSet) {
if (this->state != ModuleState::STATE_STANDBY) {
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
} else {
modelIDSet = true;
modelID = g_model.header.modelId[module_index];
trsp.sendFrame(COMMAND::MODEL_ID, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
&g_model.header.modelId[module_index], 1);
// always fetch config after setting model ID
trsp.enqueue(COMMAND::MODULE_GET_CONFIG, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
} else if (modelID != g_model.header.modelId[module_index]) {
modelIDSet = false;
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
}
if (this->state == ModuleState::STATE_STANDBY) {
cmdCount = 0;
requestInfoAndRun(true);
return;
}
// exit bind
if (this->state == STATE_BINDING) {
TRACE("AFHDS3 [EXIT BIND]");
auto mode = (uint8_t)MODULE_MODE_E::RUN;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
}
}
if (cmdCount++ >= 150) {
cmdCount = 0;
if (cmdIndex >= sizeof(periodicRequestCommands)) {
cmdIndex = 0;
}
COMMAND cmd = periodicRequestCommands[cmdIndex++];
if (cmd == COMMAND::VIRTUAL_FAILSAFE) {
if (isConnected() && !hasTelemetry()) {
TRACE("AFHDS ONE WAY FAILSAFE");
uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = {
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
setFailSafe((int16_t*)(&failSafe[1]));
trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA,
FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe,
AFHDS3_MAX_CHANNELS * 2 + 2);
return;
}
} else {
trsp.sendFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
}
if (isConnected()) {
sendChannelsData();
} else {
//default frame - request state
trsp.sendFrame(MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
}
}
void ProtoState::init(uint8_t moduleIndex, void* buffer, const etx_serial_driver_t* drv)
{
module_index = moduleIndex;
// TODO: move bit/byte transport to serial driver
if (drv) {
trsp.init(ByteTransport::Serial, buffer, drv);
} else {
trsp.init(ByteTransport::Pulses, buffer, nullptr);
}
//clear local vars because it is member of union
moduleData = &g_model.moduleData[module_index];
state = ModuleState::STATE_NOT_READY;
modelIDSet = false;
clearFrameData();
}
void ProtoState::clearFrameData()
{
TRACE("AFHDS3 clearFrameData");
trsp.clear();
cmdCount = 0;
cmdIndex = 0;
}
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 ProtoState::setState(ModuleState 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) {
trsp.clear();
}
}
void ProtoState::requestInfoAndRun(bool send)
{
// set model ID
// trsp.enqueue(COMMAND::MODEL_ID, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
// g_model.header.modelId[module_index]);
// RUN
trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
(uint8_t)MODULE_MODE_E::RUN);
if (send) { trsp.processQueue(); }
}
void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
{
AfhdsFrame* responseFrame = reinterpret_cast<AfhdsFrame*>(rxBuffer);
if (containsData((enum FRAME_TYPE) responseFrame->frameType)) {
switch (responseFrame->command) {
case COMMAND::MODULE_READY:
TRACE("AFHDS3 [MODULE_READY] %02X", responseFrame->value);
if (responseFrame->value == MODULE_STATUS_READY) {
setState(ModuleState::STATE_READY);
// requestInfoAndRun();
}
else {
setState(ModuleState::STATE_NOT_READY);
}
break;
case COMMAND::MODULE_GET_CONFIG: {
size_t len = min<size_t>(sizeof(cfg.buffer), rxBufferCount);
std::memcpy((void*) cfg.buffer, &responseFrame->value, len);
// 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));
TRACE("AFHDS3 [MODULE_VERSION] Product %d, HW %d, BOOT %d, FW %d",
version.productNumber, version.hardwereVersion,
version.bootloaderVersion, version.firmwareVersion);
break;
case COMMAND::MODULE_STATE:
TRACE("AFHDS3 [MODULE_STATE] %02X", responseFrame->value);
setState((ModuleState)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);
}
break;
case COMMAND::MODULE_SET_CONFIG:
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;
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;
}
}
}
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) {
TRACE("Command %02X NOT IMPLEMENTED!", responseFrame->command);
}
}
inline bool isSbus(uint8_t mode)
{
return (mode & 1);
}
inline bool isPWM(uint8_t mode)
{
return !(mode & 2);
}
bool ProtoState::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[] = {0x13, 0x20, 0x02, moduleData->afhds3.runPower, 0};
// TRACE("AFHDS3 SET TX POWER %d", moduleData->afhds3.runPower);
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
// sizeof(data));
// return true;
// }
// // 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");
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
// sizeof(data));
// return true;
// }
// PULSE_MODE modelPulseMode = isPWM(moduleData->afhds3.mode)
// ? 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[] = {0x16, 0x70, 0x01, (uint8_t)(modelPulseMode)};
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
// sizeof(data));
// return true;
// }
// SERIAL_MODE modelSerialMode = isSbus(moduleData->afhds3.mode)
// ? 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[] = {0x18, 0x70, 0x01, (uint8_t)(modelSerialMode)};
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
// sizeof(data));
// return true;
// }
// if (moduleData->afhds3.failsafeTimeout != cfg.config.failSafeTimout) {
// moduleData->afhds3.failsafeTimeout = cfg.config.failSafeTimout;
// uint8_t data[] = {0x12, 0x60, 0x02,
// (uint8_t)(moduleData->afhds3.failsafeTimeout & 0xFF),
// (uint8_t)(moduleData->afhds3.failsafeTimeout >> 8)};
// trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data,
// sizeof(data));
// TRACE("AFHDS3 FAILSAFE TMEOUT, %d", moduleData->afhds3.failsafeTimeout);
// return true;
// }
return false;
}
void ProtoState::sendChannelsData()
{
uint8_t channels_start = moduleData->channelsStart;
uint8_t channelsCount = 8 + moduleData->channelsCount;
uint8_t channels_last = channels_start + channelsCount;
int16_t buffer[AFHDS3_MAX_CHANNELS + 1] = {0};
uint8_t* header = (uint8_t*)buffer;
header[0] = CHANNELS_DATA_MODE::CHANNELS;
uint8_t channels = _phyMode_channels[cfg.v0.PhyMode];
header[1] = channels;
for (uint8_t channel = channels_start, index = 1; channel < channels_last;
channel++, index++) {
int16_t channelValue = convert(::getChannelValue(channel));
buffer[index] = channelValue;
}
trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP,
(uint8_t*)buffer, (channels + 1) * 2);
}
void ProtoState::stop()
{
TRACE("AFHDS3 STOP");
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
}
void ProtoState::resetConfig(uint8_t version)
{
memclear(&cfg, sizeof(cfg));
cfg.version = version;
if (cfg.version == 1) {
cfg.v1.SignalStrengthRCChannelNb = 0xFF;
cfg.v1.FailsafeTimeout = 500;
for (int i = 0; i < SES_NB_MAX_CHANNELS; i++)
cfg.v1.PWMFrequenciesV1.PWMFrequencies[i] = 50;
} else {
cfg.v0.SignalStrengthRCChannelNb = 0xFF;
cfg.v0.FailsafeTimeout = 500;
cfg.v0.PWMFrequency.Frequency = 50;
}
}
void ProtoState::applyConfigFromModel()
{
uint8_t version = 0;
if (moduleData->afhds3.phyMode >= ROUTINE_FLCR1_18CH) {
version = 1;
}
if (version != cfg.version) {
resetConfig(version);
}
if (cfg.version == 1) {
cfg.v1.EMIStandard = moduleData->afhds3.emi;
cfg.v1.IsTwoWay = moduleData->afhds3.telemetry;
cfg.v1.PhyMode = moduleData->afhds3.phyMode;
// Failsafe
setFailSafe(cfg.v1.FailSafe);
if (moduleData->failsafeMode != FAILSAFE_NOPULSES) {
cfg.v1.FailsafeOutputMode = true;
} else {
cfg.v1.FailsafeOutputMode = false;
}
} else {
cfg.v0.EMIStandard = moduleData->afhds3.emi;
cfg.v0.IsTwoWay = moduleData->afhds3.telemetry;
cfg.v0.PhyMode = moduleData->afhds3.phyMode;
// Failsafe
setFailSafe(cfg.v0.FailSafe);
if (moduleData->failsafeMode != FAILSAFE_NOPULSES) {
cfg.v0.FailsafeOutputMode = true;
} else {
cfg.v0.FailsafeOutputMode = false;
}
}
}
inline int16_t ProtoState::convert(int channelValue)
{
//pulseValue = limit<uint16_t>(0, 988 + ((channelValue + 1024) / 2), 0xfff);
//988 - 750 = 238
//238 * 20 = 4760
//2250 - 2012 = 238
//238 * 20 = 4760
// 988 ---- 2012
//-10240 ---- 10240
//-1024 ---- 1024
return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 10, AFHDS3_FAILSAFE_MAX);
}
uint8_t ProtoState::setFailSafe(int16_t* target)
{
int16_t pulseValue = 0;
uint8_t channels_start = moduleData->channelsStart;
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(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) (AFHDS3_MAX_CHANNELS);
}
Config_u* getConfig(uint8_t module)
{
auto p_state = &protoState[module];
return p_state->getConfig();
}
void applyModelConfig(uint8_t module)
{
auto p_state = &protoState[module];
p_state->applyConfigFromModel();
}
static void* initExternal(uint8_t module)
{
#if defined(AFHDS3_EXT_UART)
const etx_serial_driver_t* drv = &ExtmoduleSerialDriver;
uint16_t period = AFHDS3_UART_COMMAND_TIMEOUT * 1000 /* us */;
#else
const etx_serial_driver_t* drv = nullptr;
uint16_t period = AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT * 1000 /* us */;
telemetryPortInvertedInit(AFHDS3_SOFTSERIAL_BAUDRATE);
telemetryPortSetDirectionInput();
#endif
auto p_state = &protoState[module];
p_state->init(module, &extmodulePulsesData.afhds3, drv);
mixerSchedulerSetPeriod(module, period);
EXTERNAL_MODULE_ON();
return p_state;
}
static void deinitExternal(void* context)
{
EXTERNAL_MODULE_OFF();
mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0);
auto p_state = (ProtoState*)context;
p_state->deinit();
}
static void sendPulses(void* context)
{
auto p_state = (ProtoState*)context;
p_state->sendFrame();
}
static void setupPulses(void* context, int16_t* channels, uint8_t nChannels)
{
(void)channels;
(void)nChannels;
auto p_state = (ProtoState*)context;
p_state->setupFrame();
}
etx_module_driver_t externalDriver = {
.protocol = PROTOCOL_CHANNELS_AFHDS3,
.init = initExternal,
.deinit = deinitExternal,
.setupPulses = setupPulses,
.sendPulses = sendPulses,
.getByte = nullptr, // TODO
.processData = processTelemetryData,
};
#if defined(INTERNAL_MODULE_AFHDS3)
static void* initInternal(uint8_t module)
{
auto p_state = &protoState[module];
p_state->init(module, &intmodulePulsesData, &IntmoduleSerialDriver);
telemetryProtocol = PROTOCOL_TELEMETRY_AFHDS3;
mixerSchedulerSetPeriod(module, AFHDS3_UART_COMMAND_TIMEOUT * 1000 /* us */);
INTERNAL_MODULE_ON();
return p_state;
}
static void deinitInternal(void* context)
{
INTERNAL_MODULE_OFF();
telemetryProtocol = 0xFF;
mixerSchedulerSetPeriod(INTERNAL_MODULE, 0);
auto p_state = (ProtoState*)context;
p_state->deinit();
}
static int getByteInternal(void* context, uint8_t* data)
{
auto p_state = (ProtoState*)context;
return p_state->getTelemetryByte(data);
}
etx_module_driver_t internalDriver = {
.protocol = PROTOCOL_CHANNELS_AFHDS3,
.init = initInternal,
.deinit = deinitInternal,
.setupPulses = setupPulses,
.sendPulses = sendPulses,
.getByte = getByteInternal,
.processData = processTelemetryData,
};
#endif
}
Reference in New Issue View Git Blame Copy Permalink