Crossfire telemetry protocol start
This commit is contained in:
@@ -0,0 +1,141 @@
|
||||
/*
|
||||
* 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 "opentx.h"
|
||||
|
||||
struct CrossfireSensor {
|
||||
const uint8_t id;
|
||||
const uint8_t subId;
|
||||
const char * name;
|
||||
const TelemetryUnit unit;
|
||||
const uint8_t prec;
|
||||
};
|
||||
|
||||
const CrossfireSensor crossfireSensors[] = {
|
||||
{LINK_STATS_ID, 0, ZSTR_RX_RSSI1, UNIT_DB, 0},
|
||||
{LINK_STATS_ID, 1, ZSTR_RX_RSSI2, UNIT_DB, 0},
|
||||
{LINK_STATS_ID, 2, ZSTR_RX_QUALITY, UNIT_PERCENT, 0},
|
||||
{LINK_STATS_ID, 3, ZSTR_RX_SNR, UNIT_DB, 0},
|
||||
{LINK_STATS_ID, 4, ZSTR_ANTENNA, UNIT_RAW, 0},
|
||||
{LINK_STATS_ID, 5, ZSTR_RF_MODE, UNIT_RAW, 0},
|
||||
{LINK_STATS_ID, 6, ZSTR_TX_POWER, UNIT_MILLIWATTS, 0},
|
||||
{LINK_STATS_ID, 7, ZSTR_TX_RSSI, UNIT_DB, 0},
|
||||
{LINK_STATS_ID, 8, ZSTR_TX_QUALITY, UNIT_PERCENT, 0},
|
||||
{LINK_STATS_ID, 9, ZSTR_TX_SNR, UNIT_DB, 0},
|
||||
{ 0, 0, NULL, UNIT_RAW, 0} // sentinel
|
||||
};
|
||||
|
||||
const CrossfireSensor * getCrossfireSensor(uint8_t id, uint8_t subId=0)
|
||||
{
|
||||
const CrossfireSensor * result = NULL;
|
||||
for (const CrossfireSensor * sensor = crossfireSensors; sensor->id; sensor++) {
|
||||
if (id == sensor->id && subId == sensor->subId) {
|
||||
result = sensor;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void processCrossfireTelemetryFrame(uint8_t id, uint8_t subId, uint32_t value)
|
||||
{
|
||||
const CrossfireSensor * sensor = getCrossfireSensor(id, subId);
|
||||
TelemetryUnit unit = UNIT_RAW;
|
||||
uint8_t precision = 0;
|
||||
if (sensor) {
|
||||
unit = sensor->unit;
|
||||
precision = sensor->prec;
|
||||
}
|
||||
setTelemetryValue(TELEM_PROTO_CROSSFIRE, id, 0, subId, value, unit, precision);
|
||||
}
|
||||
|
||||
bool checkCrossfireTelemetryFrameCRC()
|
||||
{
|
||||
// uint8_t length = telemetryRxBuffer[1];
|
||||
return true;
|
||||
}
|
||||
|
||||
void processCrossfireTelemetryFrame()
|
||||
{
|
||||
if (!checkCrossfireTelemetryFrameCRC()) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t id = telemetryRxBuffer[2];
|
||||
switch(id) {
|
||||
case LINK_STATS_ID:
|
||||
for (int i=0; i<10; i++) {
|
||||
processCrossfireTelemetryFrame(id, i, telemetryRxBuffer[3+i]);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void processCrossfireTelemetryData(uint8_t data)
|
||||
{
|
||||
if (telemetryRxBufferCount == 0 && data != 0x00) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (telemetryRxBufferCount == 1 && (data < || data > TELEMETRY_RX_PACKET_SIZE-2)) {
|
||||
telemetryRxBufferCount = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||
}
|
||||
else {
|
||||
TRACE("processCrossfirePacket(): length error ");
|
||||
DUMP(telemetryRxBuffer, TELEMETRY_RX_PACKET_SIZE);
|
||||
telemetryRxBufferCount = 0;
|
||||
}
|
||||
|
||||
if (telemetryRxBufferCount > 4) {
|
||||
uint8_t length = telemetryRxBuffer[1];
|
||||
if (length + 2 == telemetryRxBufferCount) {
|
||||
processCrossfireTelemetryFrame();
|
||||
telemetryRxBufferCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
|
||||
{
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||
|
||||
telemetrySensor.id = id;
|
||||
telemetrySensor.instance = subId;
|
||||
|
||||
const CrossfireSensor * sensor = getCrossfireSensor(id, subId);
|
||||
if (sensor) {
|
||||
TelemetryUnit unit = sensor->unit;
|
||||
uint8_t prec = min<uint8_t>(2, sensor->prec);
|
||||
telemetrySensor.init(sensor->name, unit, prec);
|
||||
if (id == LINK_STATS_ID) {
|
||||
telemetrySensor.logs = true;
|
||||
}
|
||||
}
|
||||
else {
|
||||
telemetrySensor.init(id);
|
||||
}
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
* 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 _CROSSFIRE_H_
|
||||
#define _CROSSFIRE_H_
|
||||
|
||||
#define LINK_STATS_ID 0x14
|
||||
|
||||
void processCrossfireTelemetryData(uint8_t data);
|
||||
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
|
||||
|
||||
#endif // _CROSSFIRE_H_
|
||||
@@ -20,9 +20,6 @@
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
||||
uint8_t frskyRxBufferCount = 0;
|
||||
|
||||
#if !defined(CPUARM)
|
||||
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
|
||||
uint8_t frskyTxBufferCount = 0;
|
||||
@@ -39,6 +36,18 @@ extern uint8_t TrotCount;
|
||||
extern uint8_t TezRotary;
|
||||
#endif
|
||||
|
||||
// Receive buffer state machine state enum
|
||||
enum FrSkyDataState {
|
||||
STATE_DATA_IDLE,
|
||||
STATE_DATA_START,
|
||||
STATE_DATA_IN_FRAME,
|
||||
STATE_DATA_XOR,
|
||||
#if defined(TELEMETREZ)
|
||||
STATE_DATA_PRIVATE_LEN,
|
||||
STATE_DATA_PRIVATE_VALUE
|
||||
#endif
|
||||
};
|
||||
|
||||
NOINLINE void processFrskyTelemetryData(uint8_t data)
|
||||
{
|
||||
static uint8_t dataState = STATE_DATA_IDLE;
|
||||
@@ -72,12 +81,12 @@ NOINLINE void processFrskyTelemetryData(uint8_t data)
|
||||
if (data == START_STOP) {
|
||||
if (IS_FRSKY_SPORT_PROTOCOL()) {
|
||||
dataState = STATE_DATA_IN_FRAME ;
|
||||
frskyRxBufferCount = 0;
|
||||
telemetryRxBufferCount = 0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (frskyRxBufferCount < FRSKY_RX_PACKET_SIZE) {
|
||||
frskyRxBuffer[frskyRxBufferCount++] = data;
|
||||
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||
}
|
||||
dataState = STATE_DATA_IN_FRAME;
|
||||
}
|
||||
@@ -90,30 +99,30 @@ NOINLINE void processFrskyTelemetryData(uint8_t data)
|
||||
else if (data == START_STOP) {
|
||||
if (IS_FRSKY_SPORT_PROTOCOL()) {
|
||||
dataState = STATE_DATA_IN_FRAME ;
|
||||
frskyRxBufferCount = 0;
|
||||
telemetryRxBufferCount = 0;
|
||||
}
|
||||
else {
|
||||
// end of frame detected
|
||||
frskyDProcessPacket(frskyRxBuffer);
|
||||
frskyDProcessPacket(telemetryRxBuffer);
|
||||
dataState = STATE_DATA_IDLE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
else if (frskyRxBufferCount < FRSKY_RX_PACKET_SIZE) {
|
||||
frskyRxBuffer[frskyRxBufferCount++] = data;
|
||||
else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||
}
|
||||
break;
|
||||
|
||||
case STATE_DATA_XOR:
|
||||
if (frskyRxBufferCount < FRSKY_RX_PACKET_SIZE) {
|
||||
frskyRxBuffer[frskyRxBufferCount++] = data ^ STUFF_MASK;
|
||||
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
|
||||
telemetryRxBuffer[telemetryRxBufferCount++] = data ^ STUFF_MASK;
|
||||
}
|
||||
dataState = STATE_DATA_IN_FRAME;
|
||||
break;
|
||||
|
||||
case STATE_DATA_IDLE:
|
||||
if (data == START_STOP) {
|
||||
frskyRxBufferCount = 0;
|
||||
telemetryRxBufferCount = 0;
|
||||
dataState = STATE_DATA_START;
|
||||
}
|
||||
#if defined(TELEMETREZ)
|
||||
@@ -155,8 +164,8 @@ NOINLINE void processFrskyTelemetryData(uint8_t data)
|
||||
} // switch
|
||||
|
||||
#if defined(FRSKY_SPORT)
|
||||
if (IS_FRSKY_SPORT_PROTOCOL() && frskyRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
|
||||
processSportPacket(frskyRxBuffer);
|
||||
if (IS_FRSKY_SPORT_PROTOCOL() && telemetryRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
|
||||
processSportPacket(telemetryRxBuffer);
|
||||
dataState = STATE_DATA_IDLE;
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -388,20 +388,6 @@ enum AlarmLevel {
|
||||
#define BYTESTUFF 0x7d
|
||||
#define STUFF_MASK 0x20
|
||||
|
||||
// Receive buffer state machine state enum
|
||||
enum FrSkyDataState {
|
||||
STATE_DATA_IDLE,
|
||||
STATE_DATA_START,
|
||||
STATE_DATA_IN_FRAME,
|
||||
STATE_DATA_XOR,
|
||||
#if defined(TELEMETREZ)
|
||||
STATE_DATA_PRIVATE_LEN,
|
||||
STATE_DATA_PRIVATE_VALUE
|
||||
#endif
|
||||
};
|
||||
|
||||
#define FRSKY_RX_PACKET_SIZE 19
|
||||
|
||||
#if defined(CPUARM)
|
||||
#define frskySendAlarms()
|
||||
#else
|
||||
@@ -453,6 +439,7 @@ enum TelemetryProtocol
|
||||
{
|
||||
TELEM_PROTO_FRSKY_D,
|
||||
TELEM_PROTO_FRSKY_SPORT,
|
||||
TELEM_PROTO_CROSSFIRE,
|
||||
};
|
||||
|
||||
enum TelemAnas {
|
||||
@@ -496,23 +483,6 @@ void frskyUpdateCells();
|
||||
|
||||
void processFrskyTelemetryData(uint8_t data);
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
inline uint8_t modelTelemetryProtocol()
|
||||
{
|
||||
#if defined(CROSSFIRE)
|
||||
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_CROSSFIRE)
|
||||
return PROTOCOL_PULSES_CROSSFIRE;
|
||||
#endif
|
||||
if (g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_OFF && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM)
|
||||
return g_model.telemetryProtocol;
|
||||
else
|
||||
return PROTOCOL_FRSKY_SPORT;
|
||||
}
|
||||
#define MODEL_TELEMETRY_PROTOCOL() modelTelemetryProtocol()
|
||||
#elif defined(CPUARM)
|
||||
#define MODEL_TELEMETRY_PROTOCOL() g_model.telemetryProtocol
|
||||
#endif
|
||||
|
||||
#if defined(LUA)
|
||||
struct LuaTelemetryValue
|
||||
{
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#include "opentx.h"
|
||||
|
||||
uint8_t telemetryStreaming = 0;
|
||||
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
||||
uint8_t telemetryRxBufferCount = 0;
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
uint8_t wshhStreaming = 0;
|
||||
@@ -60,6 +62,10 @@ lcdint_t applyChannelRatio(source_t channel, lcdint_t val)
|
||||
#if defined(CPUSTM32)
|
||||
void processTelemetryData(uint8_t data)
|
||||
{
|
||||
#if defined(CROSSFIRE)
|
||||
if (telemetryProtocol == PROTOCOL_PULSES_CROSSFIRE)
|
||||
processCrossfireTelemetryData(data);
|
||||
#endif
|
||||
processFrskyTelemetryData(data);
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -21,7 +21,7 @@
|
||||
#ifndef _TELEMETRY_H_
|
||||
#define _TELEMETRY_H_
|
||||
|
||||
#if defined (FRSKY)
|
||||
#if defined(FRSKY)
|
||||
// FrSky Telemetry
|
||||
#include "frsky.h"
|
||||
#elif defined(JETI)
|
||||
@@ -38,6 +38,10 @@
|
||||
#include "mavlink.h"
|
||||
#endif
|
||||
|
||||
#if defined(CROSSFIRE)
|
||||
#include "crossfire.h"
|
||||
#endif
|
||||
|
||||
extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
@@ -55,6 +59,10 @@ enum TelemetryStates {
|
||||
extern uint8_t telemetryState;
|
||||
#endif
|
||||
|
||||
#define TELEMETRY_RX_PACKET_SIZE 19 // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
||||
extern uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
|
||||
extern uint8_t telemetryRxBufferCount;
|
||||
|
||||
#if defined(CPUARM)
|
||||
#define TELEMETRY_VALUE_TIMER_CYCLE 200 /*20 seconds*/
|
||||
#define TELEMETRY_VALUE_OLD_THRESHOLD 150 /*15 seconds*/
|
||||
@@ -113,6 +121,23 @@ extern uint8_t telemetryProtocol;
|
||||
#define IS_FRSKY_SPORT_PROTOCOL() (false)
|
||||
#endif
|
||||
|
||||
#if defined(CPUSTM32)
|
||||
inline uint8_t modelTelemetryProtocol()
|
||||
{
|
||||
#if defined(CROSSFIRE)
|
||||
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_CROSSFIRE)
|
||||
return PROTOCOL_PULSES_CROSSFIRE;
|
||||
#endif
|
||||
if (g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_OFF && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM)
|
||||
return g_model.telemetryProtocol;
|
||||
else
|
||||
return PROTOCOL_FRSKY_SPORT;
|
||||
}
|
||||
#define MODEL_TELEMETRY_PROTOCOL() modelTelemetryProtocol()
|
||||
#elif defined(CPUARM)
|
||||
#define MODEL_TELEMETRY_PROTOCOL() g_model.telemetryProtocol
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
#include "telemetry_sensors.h"
|
||||
#endif
|
||||
|
||||
@@ -529,6 +529,11 @@ void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, u
|
||||
case TELEM_PROTO_FRSKY_D:
|
||||
frskyDSetDefault(index, id);
|
||||
break;
|
||||
#endif
|
||||
#if defined(CROSSFIRE)
|
||||
case TELEM_PROTO_CROSSFIRE:
|
||||
crossfireSetDefault(index, id, instance);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
return;
|
||||
|
||||
Reference in New Issue
Block a user