Crossfire telemetry protocol start

This commit is contained in:
Bertrand Songis
2016-03-28 19:07:28 +02:00
parent 5a5f192451
commit a3c175f537
21 changed files with 396 additions and 73 deletions
+141
View File
@@ -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);
}
+29
View File
@@ -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_
+24 -15
View File
@@ -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
+1 -31
View File
@@ -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
{
+6
View File
@@ -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
+26 -1
View File
@@ -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;