NV14 implementation partially merged with actual Flysky version.
This commit is contained in:
@@ -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
|
||||
}
|
||||
|
||||
@@ -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_ */
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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
@@ -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]);
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user