NV14 implementation partially merged with actual Flysky version.

This commit is contained in:
Jakub
2020-01-05 15:48:13 +01:00
parent b12e55b2b7
commit fef09a5f74
55 changed files with 2945 additions and 1613 deletions
+31
View File
@@ -0,0 +1,31 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* 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 "afhds2.h"
afhds2::afhds2() {
// TODO Auto-generated constructor stub
}
afhds2::~afhds2() {
// TODO Auto-generated destructor stub
}
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* 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.
*/
#ifndef PULSES_AFHDS2_H_
#define PULSES_AFHDS2_H_
#include <functional>
#include <map>
#include <list>
struct FlySkySerialPulsesData {
uint8_t pulses[64];
uint8_t * ptr;
uint8_t frame_index;
uint8_t crc;
uint8_t state;
uint8_t timeout;
uint8_t esc_state;
uint8_t telemetry[64];
uint8_t telemetry_index;
} __attribute__((__packed__));
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
};
class afhds2 {
public:
afhds2();
virtual ~afhds2();
};
#endif /* PULSES_AFHDS2_H_ */
+496
View File
@@ -0,0 +1,496 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* 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 "../debug.h"
#include "../definitions.h"
#define FAILSAFE_HOLD 1
#define FAILSAFE_CUSTOM 2
namespace afhds3 {
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
};
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"
};
const char* afhds3::getState() {
if(data->state <= ModuleState::STATE_READY) return moduleStateText[data->state];
return "Unknown";
}
void afhds3::putByte(uint8_t byte) {
this->putBytes(&byte, 1);
}
void afhds3::putBytes(uint8_t* data, int length) {
for (int i = 0; i < length; i++) {
uint8_t byte = data[i];
this->data->crc += byte;
if (END == byte) {
*this->data->ptr++ = ESC;
*this->data->ptr++ = ESC_END;
}
else if (ESC == byte) {
*this->data->ptr++ = ESC;
*this->data->ptr++ = ESC_ESC;
}
else {
*this->data->ptr++ = byte;
}
}
}
void afhds3::putHeader(COMMAND command, FRAME_TYPE frame) {
operationState = State::SENDING_COMMAND;
data->ptr = this->data->pulses;
data->crc = 0;
*data->ptr++ = END;
uint8_t buffer[] = { FrameAddress, data->frame_index, frame, command};
putBytes(buffer, 4);
}
void afhds3::putFooter() {
putByte(data->crc ^ 0xff);
*data->ptr++ = END;
data->frame_index++;
switch((FRAME_TYPE)data->pulses[3])
{
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 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();
}
}
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) {
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 afhds3::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)
{
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);
}
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;
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_POWER_STATUS:
powerSource = (enum MODULE_POWER_SOURCE)responseFrame->value;
TRACE("AFHDS3 [MODULE_POWER_STATUS], %d", powerSource);
break;
case COMMAND::MODULE_STATE:
TRACE("AFHDS3 [MODULE_STATE] %02X", responseFrame->value);
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);
}
else addToQueue(COMMAND::MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
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;
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);
}
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;
}
}
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);
}
else if(responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA || responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) {
if(operationState == State::AWAITING_RESPONSE /* && requestFrame->command == responseFrame->command*/) {
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;
}
}
bool afhds3::syncSettings() {
if (moduleData->afhds3.runPower != cfg.config.runPower) {
cfg.config.runPower = moduleData->afhds3.runPower;
uint8_t data[] = { 0x20, 0x13, 0x01, moduleData->afhds3.runPower };
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)};
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) {
cfg.config.pulseMode = modelPulseMode;
TRACE("AFHDS3 PWM/PPM %d", modelPulseMode);
uint8_t data[] = {0x70, 0x16, 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) {
cfg.config.serialMode = modelSerialMode;
TRACE("AFHDS3 IBUS/SBUS %d", modelSerialMode);
uint8_t data[] = {0x70, 0x18, 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) {
moduleData->afhds3.failsafeTimeout = cfg.config.failSafeTimout;
uint8_t data[] = { 0x60, 0x12, 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");
return true;
}
return false;
}
void afhds3::sendChannelsData() {
uint8_t channels_start = moduleData->channelsStart;
uint8_t channelsCount = 8 + moduleData->channelsCount;
uint8_t channels_last = channels_start + channelsCount;
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;
}
putFrame(COMMAND::CHANNELS_FAILSAFE_DATA, FRAME_TYPE::REQUEST_SET_NO_RESP, channels, sizeof(channels));
}
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() {
TRACE("AFHDS3 STOP");
uint8_t cmd = MODULE_MODE_E::STANDBY;
putFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &cmd, 1);
reset(true);
}
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;
}
}
int16_t afhds3::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>(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) {
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(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;
}
}
+335
View File
@@ -0,0 +1,335 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* 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.
*/
#ifndef PULSES_AFHDS3_H_
#define PULSES_AFHDS3_H_
#include "afhds2.h"
#include "../datastructs.h"
#include <cstring>
#include <queue>
#define AFHDS3_BAUDRATE 1500000
namespace afhds3 {
typedef void (*bindCallback_t) (bool);
typedef void (*processSensor_t) (const uint8_t *, uint8_t);
typedef int32_t (*getChannelValue_t)(uint8_t);
enum DeviceAddress {
TRANSMITTER = 0x01, MODULE = 0x03,
};
enum FRAME_TYPE {
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 {
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,
};
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_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_NOT_READY = 0x0b, //virtual
STATE_READY = 0x0c, //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
};
enum CMD_RESULT {
FAILURE = 0x01,
SUCCESS = 0x02,
};
#define MIN_FREQ 50
#define MAX_FREQ 400
#define MAX_CHANNELS 18
#define FAILSAFE_KEEP_LAST 0x8000
#define FAILSAFE_MIN -15000
#define FAILSAFE_MAX 15000
enum BIND_POWER {
MIN_16dBm = 0x00,
MIN_5dBm = 0x01,
MIN_0dbm = 0x02,
PLUS_5dBm = 0x03,
PLUS_14dBm = 0x04
};
enum RUN_POWER {
PLUS_15bBm = 0x00,
PLUS_20bBm = 0x01,
PLUS_27dbm = 0x02,
PLUS_30dBm = 0x03,
PLUS_33dBm = 0x04
};
enum EMI_STANDARD {
FCC = 0x00, CE = 0x01
};
enum TELEMETRY {
TELEMETRY_DISABLED = 0x00, TELEMETRY_ENABLED = 0x01
};
enum PULSE_MODE {
PWM = 0x00, PPM_MODE = 0x01,
};
enum SERIAL_MODE {
IBUS = 0x00, SBUS_MODE = 0x02
};
struct __attribute__ ((packed)) Config_s {
uint8_t bindPower;
uint8_t runPower;
uint8_t emiStandard;
uint8_t telemetry;
uint16_t pwmFreq;
uint8_t pulseMode;
uint8_t serialMode;
uint8_t channelCount;
uint16_t failSafeTimout;
int16_t failSafeMode[MAX_CHANNELS];
};
union Config_u {
Config_s config;
uint8_t buffer[sizeof(Config_s)];
};
enum CHANNELS_DATA_MODE {
CHANNELS = 0x01, FAIL_SAFE = 0x02,
};
struct __attribute__ ((packed)) ChannelsData {
uint8_t mode;
uint8_t channelsNumber;
int16_t data[MAX_CHANNELS];
};
union ChannelsData_u {
ChannelsData data;
uint8_t buffer[sizeof(ChannelsData)];
};
struct __attribute__ ((packed)) 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,
};
struct __attribute__ ((packed)) ModuleVersion {
uint32_t productNumber;
uint32_t hardwereVersion;
uint32_t bootloaderVersion;
uint32_t firmwareVersion;
uint32_t rfVersion;
};
union AfhdsFrameData {
uint8_t value;
Config_s Config;
ChannelsData Channels;
TelemetryData Telemetry;
ModuleVersion Version;
};
struct __attribute__ ((packed)) 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
enum State {
UNKNOWN = 0,
SENDING_COMMAND,
AWAITING_RESPONSE,
IDLE
};
class request {
public:
request(COMMAND command, FRAME_TYPE frameType, const uint8_t* data = nullptr, uint8_t length = 0) {
this->command = command;
this->frameType = frameType;
if(data && length){
payload = new uint8_t[length];
std::memcpy(payload, data, length);
}
else payload = nullptr;
payloadSize = length;
}
~request() {
if(payload != nullptr) {
delete[] payload;
payload = nullptr;
}
}
enum COMMAND command;
enum FRAME_TYPE frameType;
uint8_t* payload;
uint8_t payloadSize;
};
class afhds3 {
public:
afhds3(FlySkySerialPulsesData* data, ModuleData* moduleData, int16_t* failsafeChannels, getChannelValue_t getChannelValue, processSensor_t processSensor) {
this->data = data;
this->moduleData = moduleData;
this->getChannelValue = getChannelValue;
this->processSensor = processSensor;
this->failsafeChannels = failsafeChannels;
reset();
}
virtual ~afhds3() {
clearQueue();
}
const uint32_t baudrate = AFHDS3_BAUDRATE;
const uint16_t parity = ((uint16_t)0x0000); //USART_Parity_No
const uint16_t stopBits = ((uint16_t)0x0000); //USART_StopBits_1
const uint16_t wordLength = ((uint16_t)0x0000); //USART_WordLength_8b
const uint16_t commandTimout = 5; //ms
void setupPulses();
void onDataReceived(uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount, uint8_t maxSize);
void reset(bool resetFrameCount = true);
void bind(bindCallback_t callback);
void range(bindCallback_t callback);
void cancel();
const char* getState();
void stop();
void setToDefault();
void setModelData();
private:
const uint8_t FrameAddress = DeviceAddress::TRANSMITTER | (DeviceAddress::MODULE << 4);
const uint16_t commandRepeatCount = 5;
void putByte(uint8_t byte);
void putBytes(uint8_t* data, int length);
void putHeader(COMMAND command, FRAME_TYPE frameType);
void putFooter();
void putFrame(COMMAND command, FRAME_TYPE frameType, uint8_t* data = nullptr, uint8_t dataLength = 0);
void addToQueue(COMMAND command, FRAME_TYPE frameType, uint8_t* data = nullptr, uint8_t dataLength = 0);
void parseData(uint8_t* rxBuffer, uint8_t rxBufferCount);
void setState(uint8_t state);
bool syncSettings();
void trace(const char* message);
uint8_t setFailSafe(int16_t* target);
int16_t convert(int channelValue);
void onModelSwitch();
void sendChannelsData();
void clearQueue();
//external data
FlySkySerialPulsesData* data;
ModuleData* moduleData;
int16_t* failsafeChannels;
int16_t* channelOutputs;
bindCallback_t operationCallback;
getChannelValue_t getChannelValue;
processSensor_t processSensor;
//missing ppm center!
//local config
Config_u cfg;
ModuleVersion version;
enum MODULE_POWER_SOURCE powerSource;
//buffer where the channels are
State operationState;
uint16_t repeatCount;
uint32_t idleCount;
std::queue<request*> commandQueue;
};
}
#endif /* PULSES_AFHDS3_H_ */
+94 -179
View File
@@ -21,49 +21,17 @@
*/
#include "opentx.h"
#define END 0xC0
#define ESC 0xDB
#define ESC_END 0xDC
#define ESC_ESC 0xDD
#include "flysky.h"
#define FRAME_TYPE_REQUEST_ACK 0x01
#define FRAME_TYPE_REQUEST_NACK 0x02
#define FRAME_TYPE_ANSWER 0x10
enum FlySkyModuleCommandID {
COMMAND_ID_NONE,
COMMAND_ID_RF_INIT,
COMMAND_ID_BIND,
COMMAND_ID_SET_RECEIVER_ID,
COMMAND_ID_RF_GET_CONFIG,
COMMAND_ID_SEND_CHANNEL_DATA,
COMMAND_ID_RX_SENSOR_DATA,
COMMAND_ID_SET_RX_PWM_PPM,
COMMAND_ID_SET_RX_SERVO_FREQ,
COMMAND_ID_GET_FIRMWARE_REV,
COMMAND_ID_SET_RX_IBUS_SBUS,
COMMAND_ID_SET_RX_IBUS_SERVO_EXT,
COMMAND_ID0C_UPDATE_RF_FIRMWARE = 0x0C,
COMMAND_ID0D_SET_TX_POWER = 0x0D,
COMMAND_ID_SET_RF_PROTOCOL,
COMMAND_ID_TEST_RANGE,
COMMAND_ID_TEST_RF_RESERVED,
COMMAND_ID20_UPDATE_RX_FIRMWARE = 0x20,
COMMAND_ID_LAST
};
#define IS_VALID_COMMAND_ID(id) ((id) < COMMAND_ID_LAST)
#ifndef custom_log
#define custom_log
#endif
enum DEBUG_RF_FRAME_PRINT_E {
FRAME_PRINT_OFF,// 0:OFF, 1:RF frame only, 2:TX frame only, 3:Both RF and TX frame
RF_FRAME_ONLY,
TX_FRAME_ONLY,
BOTH_FRAME_PRINT
};
#define DEBUG_RF_FRAME_PRINT FRAME_PRINT_OFF
#define FLYSKY_MODULE_TIMEOUT 155 /* ms */
#define NUM_OF_NV14_CHANNELS (14)
@@ -72,70 +40,6 @@ enum DEBUG_RF_FRAME_PRINT_E {
#define gRomData g_model.moduleData[INTERNAL_MODULE].flysky
#define SET_DIRTY() storageDirty(EE_MODEL)
enum FlySkySensorType_E {
FLYSKY_SENSOR_RX_VOLTAGE,
FLYSKY_SENSOR_RX_SIGNAL,
FLYSKY_SENSOR_RX_RSSI,
FLYSKY_SENSOR_RX_NOISE,
FLYSKY_SENSOR_RX_SNR,
FLYSKY_SENSOR_TEMP,
FLYSKY_SENSOR_EXT_VOLTAGE,
FLYSKY_SENSOR_MOTO_RPM,
FLYSKY_SENSOR_PRESURRE,
FLYSKY_SENSOR_GPS
};
enum FlySkyModuleState_E {
FLYSKY_MODULE_STATE_SET_TX_POWER,
FLYSKY_MODULE_STATE_INIT,
FLYSKY_MODULE_STATE_BIND,
FLYSKY_MODULE_STATE_SET_RECEIVER_ID,
FLYSKY_MODULE_STATE_SET_RX_PWM_PPM,
FLYSKY_MODULE_STATE_SET_RX_IBUS_SBUS,
FLYSKY_MODULE_STATE_SET_RX_FREQUENCY,
FLYSKY_MODULE_STATE_UPDATE_RF_FIRMWARE,
FLYSKY_MODULE_STATE_UPDATE_RX_FIRMWARE,
FLYSKY_MODULE_STATE_UPDATE_HALL_FIRMWARE,
FLYSKY_MODULE_STATE_UPDATE_RF_PROTOCOL,
FLYSKY_MODULE_STATE_GET_RECEIVER_CONFIG,
FLYSKY_MODULE_STATE_GET_RX_FIRMWARE_INFO,
FLYSKY_MODULE_STATE_GET_RX_FW_VERSION,
FLYSKY_MODULE_STATE_GET_RF_FIRMWARE_INFO,
FLYSKY_MODULE_STATE_GET_RF_FW_VERSION,
FLYSKY_MODULE_STATE_IDLE,
FLYSKY_MODULE_STATE_DEFAULT,
};
enum FlySkyBindState_E {
BIND_LOW_POWER,
BIND_NORMAL_POWER,
BIND_EXIT,
};
enum FlySkyRxPulse_E {
FLYSKY_PWM,
FLYSKY_PPM
};
enum FlySkyRxPort_E {
FLYSKY_IBUS,
FLYSKY_SBUS
};
enum FlySkyFirmwareType_E {
FLYSKY_RX_FIRMWARE,
FLYSKY_RF_FIRMWARE
};
enum FlySkyChannelDataType_E {
FLYSKY_CHANNEL_DATA_NORMAL,
FLYSKY_CHANNEL_DATA_FAILSAFE
};
enum FlySkyPulseModeValue_E {
PWM_IBUS, PWM_SBUS,
PPM_IBUS, PPM_SBUS
};
#define GET_FLYSKY_PWM_PPM (gRomData.mode < 2 ? FLYSKY_PWM: FLYSKY_PPM)
#define GET_FLYSKY_IBUS_SBUS (gRomData.mode & 1 ? FLYSKY_SBUS: FLYSKY_IBUS)
@@ -197,7 +101,7 @@ typedef struct RX_INFO_S {
static rx_sensor_t rx_sensor_info;
static uint8_t tx_working_power = 90;
static uint8_t lastState = FLYSKY_MODULE_STATE_IDLE;
static rf_info_t rf_info = {
.id = {8, 8, 8, 8},
.bind_power = BIND_NORMAL_POWER,
@@ -474,15 +378,15 @@ void setFlySkyChannelData(int channel, int16_t servoValue)
void sendPulsesFrameByState(uint8_t port, uint8_t frameState)
{
modulePulsesData[port].flysky.state = frameState;
modulePulsesData[port].flysky.state_index = FLYSKY_MODULE_TIMEOUT;
intmodulePulsesData.flysky.state = frameState;
intmodulePulsesData.flysky.timeout = FLYSKY_MODULE_TIMEOUT;
//setupPulsesFlySky(port); // effect immediately
}
bool isRxBindingState(uint8_t port)
{
return moduleFlag[port] == MODULE_BIND;
return moduleState[port].mode == MODULE_MODE_BIND;
}
bool isFlySkyUsbDownload(void)
@@ -501,22 +405,24 @@ void onIntmoduleSetPower(bool isPowerOn)
INTERNAL_MODULE_OFF();
}
}
//REMOVED FROM OPENTX
void resetPulsesFlySky(uint8_t port);
void onIntmoduleBindReceiver(uint8_t port)
{
resetPulsesFlySky(port);
moduleFlag[port] = MODULE_BIND;
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_INIT;
moduleState[port].mode = MODULE_MODE_BIND;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_INIT;
}
void onFlySkyReceiverPulseMode(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_RX_PWM_PPM;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_RX_PWM_PPM;
}
void onFlySkyReceiverPulsePort(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_RX_IBUS_SBUS;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_RX_IBUS_SBUS;
}
void onIntmoduleReceiverSetFrequency(uint8_t port)
@@ -533,96 +439,105 @@ void onIntmoduleReceiverSetPulse(uint8_t port, uint8_t mode_and_port) // mode_an
void onFlySkyTransmitterPower(uint8_t port, uint8_t dBmValue)
{
tx_working_power = dBmValue;
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_TX_POWER;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_TX_POWER;
}
void onFlySkyUpdateReceiverFirmwareStart(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_UPDATE_RX_FIRMWARE;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_UPDATE_RX_FIRMWARE;
}
void setFlyskyState(uint8_t port, uint8_t state) {
intmodulePulsesData.flysky.state = state;
}
void onIntmoduleUsbDownloadStart(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_UPDATE_RF_FIRMWARE;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_UPDATE_RF_FIRMWARE;
}
void onFlySkyUsbDownloadStart(uint8_t fw_state)
{
rf_info.fw_state = fw_state;
}
void onFlySkyGetVersionInfoStart(uint8_t port, uint8_t isRfTransfer)
{
lastState = intmodulePulsesData.flysky.state;
if ( isRfTransfer != 0 )
setFlyskyState(port, FLYSKY_MODULE_STATE_GET_RF_FW_VERSION);
else setFlyskyState(port, FLYSKY_MODULE_STATE_GET_RX_FW_VERSION);
}
void onFlySkyUpdateTransmitterProtocol(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_UPDATE_RF_PROTOCOL;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_UPDATE_RF_PROTOCOL;
}
void onFlySkyGetReceiverFirmwareInfo(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_GET_RX_FIRMWARE_INFO;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_GET_RX_FIRMWARE_INFO;
}
void onFlySkyGetTransmitterFirmwareInfo(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_GET_RF_FIRMWARE_INFO;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_GET_RF_FIRMWARE_INFO;
}
void onFlySkyGetReceiverFirmwareVersion(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_GET_RX_FW_VERSION;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_GET_RX_FW_VERSION;
}
void onFlySkyGetTransmitterFirmwareVersion(uint8_t port)
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_GET_RF_FW_VERSION;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_GET_RF_FW_VERSION;
}
void initFlySkyArray(uint8_t port)
{
modulePulsesData[port].flysky.ptr = modulePulsesData[port].flysky.pulses;
modulePulsesData[port].flysky.crc = 0;
intmodulePulsesData.flysky.ptr = intmodulePulsesData.flysky.pulses;
intmodulePulsesData.flysky.crc = 0;
}
inline void putFlySkyByte(uint8_t port, uint8_t byte)
{
if (END == byte) {
*modulePulsesData[port].flysky.ptr++ = ESC;
*modulePulsesData[port].flysky.ptr++ = ESC_END;
*intmodulePulsesData.flysky.ptr++ = ESC;
*intmodulePulsesData.flysky.ptr++ = ESC_END;
}
else if (ESC == byte) {
*modulePulsesData[port].flysky.ptr++ = ESC;
*modulePulsesData[port].flysky.ptr++ = ESC_ESC;
*intmodulePulsesData.flysky.ptr++ = ESC;
*intmodulePulsesData.flysky.ptr++ = ESC_ESC;
}
else {
*modulePulsesData[port].flysky.ptr++ = byte;
*intmodulePulsesData.flysky.ptr++ = byte;
}
}
void putFlySkyFrameByte(uint8_t port, uint8_t byte)
{
modulePulsesData[port].flysky.crc += byte;
intmodulePulsesData.flysky.crc += byte;
putFlySkyByte(port, byte);
}
void putFlySkyFrameHead(uint8_t port)
{
*modulePulsesData[port].flysky.ptr++ = END;
*intmodulePulsesData.flysky.ptr++ = END;
}
void putFlySkyFrameIndex(uint8_t port)
{
putFlySkyFrameByte(port, modulePulsesData[port].flysky.frame_index);
putFlySkyFrameByte(port, intmodulePulsesData.flysky.frame_index);
}
void putFlySkyFrameCrc(uint8_t port)
{
putFlySkyByte(port, modulePulsesData[port].flysky.crc ^ 0xff);
putFlySkyByte(port, intmodulePulsesData.flysky.crc ^ 0xff);
}
void putFlySkyFrameTail(uint8_t port)
{
*modulePulsesData[port].flysky.ptr++ = END;
*intmodulePulsesData.flysky.ptr++ = END;
}
@@ -719,7 +634,7 @@ void putFlySkySendChannelData(uint8_t port)
uint8_t channels_count = min<unsigned int>(NUM_OF_NV14_CHANNELS, channels_start + 8 + g_model.moduleData[port].channelsCount);
putFlySkyFrameByte(port, channels_count);
for (uint8_t channel = channels_start; channel < channels_count; channel++) {
int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[channel];
int16_t failsafeValue = g_model.failsafeChannels[channel];
uint16_t pulseValue = limit<uint16_t>(900, 900 + ((2100 - 900) * (failsafeValue + 1024) / 2048), 2100);
putFlySkyFrameByte(port, pulseValue & 0xff);
putFlySkyFrameByte(port, pulseValue >> 8);
@@ -761,8 +676,8 @@ void putFlySkyUpdateRfProtocol(uint8_t port)
void incrFlySkyFrame(uint8_t port)
{
if (++modulePulsesData[port].flysky.frame_index == 0)
modulePulsesData[port].flysky.frame_index = 1;
if (++intmodulePulsesData.flysky.frame_index == 0)
intmodulePulsesData.flysky.frame_index = 1;
}
bool checkFlySkyFrameCrc(const uint8_t * ptr, uint8_t size)
@@ -792,7 +707,7 @@ bool checkFlySkyFrameCrc(const uint8_t * ptr, uint8_t size)
void parseFlySkyFeedbackFrame(uint8_t port)
{
const uint8_t * ptr = modulePulsesData[port].flysky.telemetry;
const uint8_t * ptr = intmodulePulsesData.flysky.telemetry;
if (*ptr++ != END)
return;
@@ -802,15 +717,15 @@ void parseFlySkyFeedbackFrame(uint8_t port)
uint8_t first_para = *ptr++;
uint8_t * p_data = NULL;
if (!checkFlySkyFrameCrc(modulePulsesData[port].flysky.telemetry + 1, modulePulsesData[port].flysky.telemetry_index - 2)) {
if (!checkFlySkyFrameCrc(intmodulePulsesData.flysky.telemetry + 1, intmodulePulsesData.flysky.telemetry_index - 2)) {
return;
}
switch (command_id) {
default:
if (moduleFlag[port] == MODULE_NORMAL_MODE
&& modulePulsesData[port].flysky.state >= FLYSKY_MODULE_STATE_IDLE) {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
if (moduleState[port].mode == MODULE_MODE_NORMAL
&& intmodulePulsesData.flysky.state >= FLYSKY_MODULE_STATE_IDLE) {
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
if (DEBUG_RF_FRAME_PRINT & RF_FRAME_ONLY) TRACE("State back to channel data");
}
break;
@@ -818,27 +733,27 @@ void parseFlySkyFeedbackFrame(uint8_t port)
case COMMAND_ID_RF_INIT: {
if (first_para == 0x01) { // action only RF ready
if (moduleFlag[port] == MODULE_BIND) {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_BIND;
if (moduleState[port].mode == MODULE_MODE_BIND) {
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_BIND;
incrFlySkyFrame(port);
}
/*else if (moduleFlag[port] == MODULE_RANGECHECK) {
/*else if (moduleState[port].mode == MODULE_MODE_RANGECHECK) {
}*/
if (modulePulsesData[port].flysky.state == FLYSKY_MODULE_STATE_INIT) {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_RECEIVER_ID;
if (intmodulePulsesData.flysky.state == FLYSKY_MODULE_STATE_INIT) {
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_RECEIVER_ID;
incrFlySkyFrame(port);
}
}
else modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_IDLE;
else intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_IDLE;
break;
}
case COMMAND_ID_BIND: {
if (frame_type != FRAME_TYPE_ANSWER) {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_IDLE;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_IDLE;
return;
}
g_model.header.modelId[port] = ptr[2];
@@ -850,11 +765,11 @@ void parseFlySkyFeedbackFrame(uint8_t port)
TRACE("New Rx ID: %02X %02X %02X %02X", gRomData.rx_id[0], gRomData.rx_id[1], gRomData.rx_id[2], gRomData.rx_id[3]);
SET_DIRTY();
resetPulsesFlySky(port);
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_INIT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_INIT;
break;
case COMMAND_ID_RF_GET_CONFIG:
modulePulsesData[port].flysky.frame_index = frame_number;
intmodulePulsesData.flysky.frame_index = frame_number;
sendPulsesFrameByState(port, FLYSKY_MODULE_STATE_GET_RECEIVER_CONFIG);
break;
}
@@ -883,46 +798,46 @@ void parseFlySkyFeedbackFrame(uint8_t port)
p_data[1] = *ptr++;
}
if (moduleFlag[port] == MODULE_RANGECHECK) {
moduleFlag[port] = MODULE_NORMAL_MODE;
if (moduleState[port].mode == MODULE_MODE_RANGECHECK) {
moduleState[port].mode = MODULE_MODE_NORMAL;
onFlySkyTransmitterPower(port, 0); // set power 0
break;
}
if (moduleFlag[port] == MODULE_NORMAL_MODE && modulePulsesData[port].flysky.state >= FLYSKY_MODULE_STATE_IDLE) {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
if (moduleState[port].mode == MODULE_MODE_NORMAL && intmodulePulsesData.flysky.state >= FLYSKY_MODULE_STATE_IDLE) {
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
}
break;
}
case COMMAND_ID_SET_RECEIVER_ID: {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
return;
}
case COMMAND_ID0D_SET_TX_POWER: {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_INIT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_INIT;
break;
}
case COMMAND_ID_SET_RX_PWM_PPM: {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_RX_IBUS_SBUS;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_RX_IBUS_SBUS;
break;
}
case COMMAND_ID_SET_RX_IBUS_SBUS: {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_RX_FREQUENCY;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_RX_FREQUENCY;
break;
}
case COMMAND_ID_SET_RX_SERVO_FREQ: {
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_DEFAULT;
break;
}
case COMMAND_ID0C_UPDATE_RF_FIRMWARE: {
rf_info.fw_state = FLYSKY_MODULE_STATE_UPDATE_RF_FIRMWARE;
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_IDLE;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_IDLE;
break;
}
}
@@ -946,7 +861,7 @@ void checkFlySkyFeedback(uint8_t port)
uint8_t byte;
while (intmoduleGetByte(&byte)) {
//if ( modulePulsesData[port].flysky.state == FLYSKY_MODULE_STATE_IDLE
//if ( intmodulePulsesData.flysky.state == FLYSKY_MODULE_STATE_IDLE
// && rf_info.fw_state == FLYSKY_MODULE_STATE_UPDATE_RF_FIRMWARE )
{
Parse_Character(&rfProtocolRx, byte );
@@ -966,7 +881,7 @@ void checkFlySkyFeedback(uint8_t port)
if ( 0x01 == rfProtocolRx.length &&
( 0x05 == rfProtocolRx.data[0] || 0x06 == rfProtocolRx.data[0]) )
{
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_INIT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_INIT;
rf_info.fw_state = 0;
}
@@ -975,26 +890,26 @@ void checkFlySkyFeedback(uint8_t port)
//continue;
}
if (byte == END && modulePulsesData[port].flysky.telemetry_index > 0) {
if (byte == END && intmodulePulsesData.flysky.telemetry_index > 0) {
parseFlySkyFeedbackFrame(port);
modulePulsesData[port].flysky.telemetry_index = 0;
intmodulePulsesData.flysky.telemetry_index = 0;
}
else {
if (byte == ESC) {
modulePulsesData[port].flysky.esc_state = 1;
intmodulePulsesData.flysky.esc_state = 1;
}
else {
if (modulePulsesData[port].flysky.esc_state) {
modulePulsesData[port].flysky.esc_state = 0;
if (intmodulePulsesData.flysky.esc_state) {
intmodulePulsesData.flysky.esc_state = 0;
if (byte == ESC_END)
byte = END;
else if (byte == ESC_ESC)
byte = ESC;
}
modulePulsesData[port].flysky.telemetry[modulePulsesData[port].flysky.telemetry_index++] = byte;
if (modulePulsesData[port].flysky.telemetry_index >= sizeof(modulePulsesData[port].flysky.telemetry)) {
intmodulePulsesData.flysky.telemetry[intmodulePulsesData.flysky.telemetry_index++] = byte;
if (intmodulePulsesData.flysky.telemetry_index >= sizeof(intmodulePulsesData.flysky.telemetry)) {
// TODO buffer is full, log an error?
modulePulsesData[port].flysky.telemetry_index = 0;
intmodulePulsesData.flysky.telemetry_index = 0;
}
}
}
@@ -1004,11 +919,11 @@ void checkFlySkyFeedback(uint8_t port)
void resetPulsesFlySky(uint8_t port)
{
modulePulsesData[port].flysky.frame_index = 1;
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_SET_TX_POWER;
modulePulsesData[port].flysky.state_index = 0;
modulePulsesData[port].flysky.esc_state = 0;
moduleFlag[port] = MODULE_NORMAL_MODE;
intmodulePulsesData.flysky.frame_index = 1;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_SET_TX_POWER;
intmodulePulsesData.flysky.timeout = 0;
intmodulePulsesData.flysky.esc_state = 0;
moduleState[port].mode = MODULE_MODE_NORMAL;
uint16_t rx_freq = g_model.moduleData[port].flysky.rx_freq[0];
rx_freq += (g_model.moduleData[port].flysky.rx_freq[1] * 256);
if (50 > rx_freq || 400 < rx_freq) {
@@ -1026,13 +941,13 @@ void setupPulsesFlySky(uint8_t port)
putFlySkyFrameHead(port);
putFlySkyFrameIndex(port);
if (modulePulsesData[port].flysky.state < FLYSKY_MODULE_STATE_DEFAULT) {
if (intmodulePulsesData.flysky.state < FLYSKY_MODULE_STATE_DEFAULT) {
if (++modulePulsesData[port].flysky.state_index >= FLYSKY_MODULE_TIMEOUT / PXX_PERIOD_DURATION) {
if (++intmodulePulsesData.flysky.timeout >= FLYSKY_MODULE_TIMEOUT / 9) {
modulePulsesData[port].flysky.state_index = 0;
intmodulePulsesData.flysky.timeout = 0;
switch (modulePulsesData[port].flysky.state) {
switch (intmodulePulsesData.flysky.state) {
case FLYSKY_MODULE_STATE_INIT:
putFlySkyRfInit(port);
@@ -1048,7 +963,7 @@ void setupPulsesFlySky(uint8_t port)
case FLYSKY_MODULE_STATE_GET_RECEIVER_CONFIG:
putFlySkyGetReceiverConfig(port);
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_INIT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_INIT;
break;
case FLYSKY_MODULE_STATE_SET_TX_POWER:
@@ -1096,11 +1011,11 @@ void setupPulsesFlySky(uint8_t port)
break;
case FLYSKY_MODULE_STATE_IDLE:
modulePulsesData[port].pxx_uart.ptr = modulePulsesData[port].pxx_uart.pulses;
intmodulePulsesData.flysky.ptr = intmodulePulsesData.flysky.pulses;
return;
default:
modulePulsesData[port].flysky.state = FLYSKY_MODULE_STATE_INIT;
intmodulePulsesData.flysky.state = FLYSKY_MODULE_STATE_INIT;
initFlySkyArray(port);
if ((DEBUG_RF_FRAME_PRINT & TX_FRAME_ONLY)) {
TRACE("State back to INIT\r\n");
@@ -1123,10 +1038,10 @@ void setupPulsesFlySky(uint8_t port)
if ((DEBUG_RF_FRAME_PRINT & TX_FRAME_ONLY)) {
/* print each command, except channel data by interval */
uint8_t * data = modulePulsesData[port].pxx_uart.pulses;
uint8_t * data = intmodulePulsesData.flysky.pulses;
if (data[3] != COMMAND_ID_SEND_CHANNEL_DATA || (set_loop_cnt++ % 100 == 0)) {
uint8_t size = modulePulsesData[port].pxx_uart.ptr - data;
TRACE_NOCRLF("TX(State%0d)%0dB:", modulePulsesData[port].flysky.state, size);
uint8_t size = intmodulePulsesData.flysky.ptr - data;
TRACE_NOCRLF("TX(State%0d)%0dB:", intmodulePulsesData.flysky.state, size);
for (int idx = 0; idx < size; idx++) {
TRACE_NOCRLF(" %02X", data[idx]);
}
+7 -1
View File
@@ -27,6 +27,7 @@
#include "pxx1.h"
#include "pxx2.h"
#include "multi.h"
#include "flysky.h"
#include "modules_helpers.h"
#include "ff.h"
@@ -272,7 +273,9 @@ union InternalModulePulsesData {
PwmPxx1Pulses pxx;
#endif
#endif
#if defined(AFHDS2)
FlySkySerialPulsesData flysky;
#endif
#if defined(PXX2)
Pxx2Pulses pxx2;
#endif
@@ -305,6 +308,9 @@ union ExternalModulePulsesData {
#if defined(DSM2) || defined(MULTIMODULE) || defined(SBUS)
Dsm2PulsesData dsm2;
#endif
#if defined(AFHDS3)
FlySkySerialPulsesData flysky;
#endif
PpmPulsesData<pulse_duration_t> ppm;