mirror of
https://github.com/opentx/opentx.git
synced 2025-07-26 17:55:19 +03:00
Re #2908: frsky_d.cpp split into ARM and nonARM files, ARM Frsky alarms code removed completely
This commit is contained in:
Damjan Adamic
parent
18ffe9f503
commit
bfc01f9504
5 changed files with 344 additions and 360 deletions
|
|
@ -1284,13 +1284,18 @@ endif
|
||||||
|
|
||||||
ifeq ($(EXT), $(filter $(EXT), FRSKY FRSKY_SPORT TELEMETREZ))
|
ifeq ($(EXT), $(filter $(EXT), FRSKY FRSKY_SPORT TELEMETREZ))
|
||||||
CPPDEFS += -DFRSKY
|
CPPDEFS += -DFRSKY
|
||||||
ifeq ($(EXT), FRSKY_SPORT)
|
|
||||||
CPPSRC += telemetry/frsky.cpp telemetry/frsky_sport.cpp telemetry/frsky_d.cpp
|
ifeq ($(ARCH), ARM)
|
||||||
CPPDEFS += -DFRSKY_SPORT
|
CPPSRC += telemetry/frsky.cpp telemetry/frsky_d_arm.cpp
|
||||||
else
|
else
|
||||||
CPPSRC += telemetry/frsky.cpp telemetry/frsky_d.cpp
|
CPPSRC += telemetry/frsky.cpp telemetry/frsky_d.cpp
|
||||||
endif
|
endif
|
||||||
|
|
||||||
|
ifeq ($(EXT), FRSKY_SPORT)
|
||||||
|
CPPSRC += telemetry/frsky_sport.cpp
|
||||||
|
CPPDEFS += -DFRSKY_SPORT
|
||||||
|
endif
|
||||||
|
|
||||||
CPPSRC += gui/$(GUIDIRECTORY)/view_telemetry.cpp
|
CPPSRC += gui/$(GUIDIRECTORY)/view_telemetry.cpp
|
||||||
|
|
||||||
ifeq ($(FRSKY_HUB), YES)
|
ifeq ($(FRSKY_HUB), YES)
|
||||||
|
|
|
||||||
|
|
@ -47,7 +47,7 @@ uint8_t link_counter = 0;
|
||||||
#define FRSKY_RX_PACKET_SIZE 19
|
#define FRSKY_RX_PACKET_SIZE 19
|
||||||
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
||||||
|
|
||||||
#if !defined(PCBTARANIS)
|
#if !defined(CPUARM)
|
||||||
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
|
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
@ -323,7 +323,7 @@ void telemetryWakeup()
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if !defined(PCBTARANIS)
|
#if !defined(CPUARM)
|
||||||
if (IS_FRSKY_D_PROTOCOL()) {
|
if (IS_FRSKY_D_PROTOCOL()) {
|
||||||
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
|
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
|
||||||
static uint8_t frskyTxDelay = 5;
|
static uint8_t frskyTxDelay = 5;
|
||||||
|
|
|
||||||
|
|
@ -50,6 +50,18 @@
|
||||||
#define FRSKY_SPORT_AVERAGING 4
|
#define FRSKY_SPORT_AVERAGING 4
|
||||||
#define FRSKY_D_AVERAGING 8
|
#define FRSKY_D_AVERAGING 8
|
||||||
|
|
||||||
|
// Enumerate FrSky packet codes
|
||||||
|
#define LINKPKT 0xfe
|
||||||
|
#define USRPKT 0xfd
|
||||||
|
#define A11PKT 0xfc
|
||||||
|
#define A12PKT 0xfb
|
||||||
|
#define A21PKT 0xfa
|
||||||
|
#define A22PKT 0xf9
|
||||||
|
#define ALRM_REQUEST 0xf8
|
||||||
|
#define RSSI1PKT 0xf7
|
||||||
|
#define RSSI2PKT 0xf6
|
||||||
|
#define RSSI_REQUEST 0xf1
|
||||||
|
|
||||||
|
|
||||||
// FrSky PRIM IDs (1 byte)
|
// FrSky PRIM IDs (1 byte)
|
||||||
#define DATA_FRAME 0x10
|
#define DATA_FRAME 0x10
|
||||||
|
|
@ -471,7 +483,7 @@ enum FrSkyDataState {
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
#if defined(PCBTARANIS)
|
#if defined(CPUARM)
|
||||||
#define frskySendAlarms()
|
#define frskySendAlarms()
|
||||||
#else
|
#else
|
||||||
#define SEND_RSSI_ALARMS 6
|
#define SEND_RSSI_ALARMS 6
|
||||||
|
|
@ -486,6 +498,16 @@ enum FrSkyDataState {
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(FRSKY_HUB)
|
||||||
|
typedef enum {
|
||||||
|
TS_IDLE = 0, // waiting for 0x5e frame marker
|
||||||
|
TS_DATA_ID, // waiting for dataID
|
||||||
|
TS_DATA_LOW, // waiting for data low byte
|
||||||
|
TS_DATA_HIGH, // waiting for data high byte
|
||||||
|
TS_XOR = 0x80 // decode stuffed byte
|
||||||
|
} TS_STATE;
|
||||||
|
#endif
|
||||||
|
|
||||||
// FrSky D Protocol
|
// FrSky D Protocol
|
||||||
void processHubPacket(uint8_t id, int16_t value);
|
void processHubPacket(uint8_t id, int16_t value);
|
||||||
void frskyDSendNextAlarm(void);
|
void frskyDSendNextAlarm(void);
|
||||||
|
|
|
||||||
|
|
@ -36,19 +36,7 @@
|
||||||
|
|
||||||
#include "../opentx.h"
|
#include "../opentx.h"
|
||||||
|
|
||||||
// Enumerate FrSky packet codes
|
#if (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
|
||||||
#define LINKPKT 0xfe
|
|
||||||
#define USRPKT 0xfd
|
|
||||||
#define A11PKT 0xfc
|
|
||||||
#define A12PKT 0xfb
|
|
||||||
#define A21PKT 0xfa
|
|
||||||
#define A22PKT 0xf9
|
|
||||||
#define ALRM_REQUEST 0xf8
|
|
||||||
#define RSSI1PKT 0xf7
|
|
||||||
#define RSSI2PKT 0xf6
|
|
||||||
#define RSSI_REQUEST 0xf1
|
|
||||||
|
|
||||||
#if !defined(CPUARM) && (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
|
|
||||||
void checkMinMaxAltitude()
|
void checkMinMaxAltitude()
|
||||||
{
|
{
|
||||||
if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
|
if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
|
||||||
|
|
@ -59,14 +47,6 @@ void checkMinMaxAltitude()
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(FRSKY_HUB)
|
#if defined(FRSKY_HUB)
|
||||||
typedef enum {
|
|
||||||
TS_IDLE = 0, // waiting for 0x5e frame marker
|
|
||||||
TS_DATA_ID, // waiting for dataID
|
|
||||||
TS_DATA_LOW, // waiting for data low byte
|
|
||||||
TS_DATA_HIGH, // waiting for data high byte
|
|
||||||
TS_XOR = 0x80 // decode stuffed byte
|
|
||||||
} TS_STATE;
|
|
||||||
|
|
||||||
void parseTelemHubByte(uint8_t byte)
|
void parseTelemHubByte(uint8_t byte)
|
||||||
{
|
{
|
||||||
static int8_t structPos;
|
static int8_t structPos;
|
||||||
|
|
@ -93,11 +73,7 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
state = TS_IDLE;
|
state = TS_IDLE;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
#if defined(CPUARM)
|
|
||||||
structPos = byte;
|
|
||||||
#else
|
|
||||||
structPos = byte * 2;
|
structPos = byte * 2;
|
||||||
#endif
|
|
||||||
state = TS_DATA_LOW;
|
state = TS_DATA_LOW;
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
|
|
@ -110,10 +86,6 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
|
|
||||||
state = TS_IDLE;
|
state = TS_IDLE;
|
||||||
|
|
||||||
#if defined(CPUARM)
|
|
||||||
processHubPacket(structPos, (byte << 8) + lowByte);
|
|
||||||
#else
|
|
||||||
|
|
||||||
#if defined(GPS)
|
#if defined(GPS)
|
||||||
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) {
|
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) {
|
||||||
if (lowByte || byte)
|
if (lowByte || byte)
|
||||||
|
|
@ -134,11 +106,7 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
if (frskyData.hub.gpsFix <= 0)
|
if (frskyData.hub.gpsFix <= 0)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
#endif
|
#endif // #if defined(GPS)
|
||||||
|
|
||||||
#if 0
|
|
||||||
uint16_t previousValue = *((uint16_t *)(((uint8_t*)&frskyData.hub) + structPos));
|
|
||||||
#endif
|
|
||||||
|
|
||||||
((uint8_t*)&frskyData.hub)[structPos] = lowByte;
|
((uint8_t*)&frskyData.hub)[structPos] = lowByte;
|
||||||
((uint8_t*)&frskyData.hub)[structPos+1] = byte;
|
((uint8_t*)&frskyData.hub)[structPos+1] = byte;
|
||||||
|
|
@ -171,7 +139,7 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
if (frskyData.hub.current > frskyData.hub.maxCurrent)
|
if (frskyData.hub.current > frskyData.hub.maxCurrent)
|
||||||
frskyData.hub.maxCurrent = frskyData.hub.current;
|
frskyData.hub.maxCurrent = frskyData.hub.current;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case offsetof(FrskySerialData, currentConsumption):
|
case offsetof(FrskySerialData, currentConsumption):
|
||||||
// we receive data from openXsensor. stops the calculation of consumption and power
|
// we receive data from openXsensor. stops the calculation of consumption and power
|
||||||
frskyData.hub.openXsensor = 1;
|
frskyData.hub.openXsensor = 1;
|
||||||
|
|
@ -264,11 +232,10 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif // #if defined(FRSKY_HUB)
|
||||||
|
|
||||||
#if defined(WS_HOW_HIGH) && !defined(CPUARM)
|
#if defined(WS_HOW_HIGH)
|
||||||
void parseTelemWSHowHighByte(uint8_t byte)
|
void parseTelemWSHowHighByte(uint8_t byte)
|
||||||
{
|
{
|
||||||
if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) {
|
if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) {
|
||||||
|
|
@ -282,33 +249,8 @@ void parseTelemWSHowHighByte(uint8_t byte)
|
||||||
// baroAltitude_bp unit here is feet!
|
// baroAltitude_bp unit here is feet!
|
||||||
frskyUsrStreaming = WSHH_TIMEOUT10ms; // reset counter
|
frskyUsrStreaming = WSHH_TIMEOUT10ms; // reset counter
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(CPUARM)
|
|
||||||
void frskyDProcessPacket(uint8_t *packet)
|
|
||||||
{
|
|
||||||
// What type of packet?
|
|
||||||
switch (packet[0])
|
|
||||||
{
|
|
||||||
case LINKPKT: // A1/A2/RSSI values
|
|
||||||
{
|
|
||||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, packet[1], UNIT_VOLTS, 0);
|
|
||||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, packet[2], UNIT_VOLTS, 0);
|
|
||||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, packet[3], UNIT_RAW, 0);
|
|
||||||
frskyData.rssi.set(packet[3]);
|
|
||||||
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case USRPKT: // User Data packet
|
|
||||||
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
|
|
||||||
for (uint8_t i=3; i<numBytes; i++) {
|
|
||||||
parseTelemHubByte(packet[i]);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
void frskyDProcessPacket(uint8_t *packet)
|
void frskyDProcessPacket(uint8_t *packet)
|
||||||
{
|
{
|
||||||
// What type of packet?
|
// What type of packet?
|
||||||
|
|
@ -346,79 +288,14 @@ void frskyDProcessPacket(uint8_t *packet)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif // #if defined(FRSKY_HUB) || defined (WS_HOW_HIGH)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
#if !defined(PCBTARANIS)
|
|
||||||
|
|
||||||
// Alarms level sent to the FrSky module
|
// Alarms level sent to the FrSky module
|
||||||
|
|
||||||
uint8_t frskyAlarmsSendState = 0 ;
|
uint8_t frskyAlarmsSendState = 0 ;
|
||||||
|
|
||||||
#if defined(CPUARM)
|
|
||||||
void frskyPushValue(uint8_t *&ptr, uint8_t value)
|
|
||||||
{
|
|
||||||
// byte stuff the only byte than might need it
|
|
||||||
if (value == START_STOP) {
|
|
||||||
*ptr++ = BYTESTUFF;
|
|
||||||
*ptr++ = 0x5e;
|
|
||||||
}
|
|
||||||
else if (value == BYTESTUFF) {
|
|
||||||
*ptr++ = BYTESTUFF;
|
|
||||||
*ptr++ = 0x5d;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
*ptr++ = value;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void frskyDSendNextAlarm(void)
|
|
||||||
{
|
|
||||||
#if defined(PCBSKY9X)
|
|
||||||
if (telemetryTransmitPending()) {
|
|
||||||
return; // we only have one buffer. If it's in use, then we can't send yet.
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if 0
|
|
||||||
uint8_t *ptr = &frskyTxBuffer[0];
|
|
||||||
|
|
||||||
*ptr++ = START_STOP; // Start of packet
|
|
||||||
|
|
||||||
// Now send a packet
|
|
||||||
frskyAlarmsSendState -= 1;
|
|
||||||
uint8_t alarm = 1 - (frskyAlarmsSendState % 2);
|
|
||||||
if (frskyAlarmsSendState < SEND_MODEL_ALARMS) {
|
|
||||||
uint8_t channel = 1 - (frskyAlarmsSendState / 2);
|
|
||||||
*ptr++ = (A22PKT + frskyAlarmsSendState); // fc - fb - fa - f9
|
|
||||||
frskyPushValue(ptr, g_model.frsky.channels[channel].alarms_value[alarm]);
|
|
||||||
*ptr++ = ALARM_GREATER(channel, alarm);
|
|
||||||
*ptr++ = (IS_SOUND_OFF() ? alarm_off : ALARM_LEVEL(channel, alarm));
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
*ptr++ = (RSSI1PKT-alarm); // f7 - f6
|
|
||||||
frskyPushValue(ptr, getRssiAlarmValue(alarm));
|
|
||||||
*ptr++ = 0x00;
|
|
||||||
*ptr++ = (IS_SOUND_OFF() ? alarm_off : ((2+alarm+g_model.frsky.rssiAlarms[alarm].level) % 4));
|
|
||||||
}
|
|
||||||
|
|
||||||
*ptr++ = 0x00;
|
|
||||||
*ptr++ = 0x00;
|
|
||||||
*ptr++ = 0x00;
|
|
||||||
*ptr++ = 0x00;
|
|
||||||
*ptr++ = 0x00;
|
|
||||||
*ptr++ = START_STOP; // End of packet
|
|
||||||
|
|
||||||
telemetryTransmitBuffer(frskyTxBuffer, ptr - &frskyTxBuffer[0]);
|
|
||||||
#elif !defined(WIN32)
|
|
||||||
#warning "FrSky module alarms removed!"
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#else
|
|
||||||
|
|
||||||
void frskyPushValue(uint8_t *&ptr, uint8_t value)
|
void frskyPushValue(uint8_t *&ptr, uint8_t value)
|
||||||
{
|
{
|
||||||
// byte stuff the only byte than might need it
|
// byte stuff the only byte than might need it
|
||||||
|
|
@ -476,227 +353,3 @@ inline void frskyDSendNextAlarm(void)
|
||||||
frskySendPacket(RSSI1PKT-alarm, getRssiAlarmValue(alarm), 0, (2+alarm+g_model.frsky.rssiAlarms[alarm].level) % 4);
|
frskySendPacket(RSSI1PKT-alarm, getRssiAlarmValue(alarm), 0, (2+alarm+g_model.frsky.rssiAlarms[alarm].level) % 4);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// checks if an alarm is raised, not used any more
|
|
||||||
#if 0
|
|
||||||
bool isFrskyAlarmRaised(uint8_t idx)
|
|
||||||
{
|
|
||||||
for (int i=0; i<2; i++) {
|
|
||||||
if (ALARM_LEVEL(idx, i) != alarm_off) {
|
|
||||||
if (ALARM_GREATER(idx, i)) {
|
|
||||||
if (frskyData.analog[idx].value > g_model.frsky.channels[idx].alarms_value[i])
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (frskyData.analog[idx].value < g_model.frsky.channels[idx].alarms_value[i])
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(CPUARM)
|
|
||||||
struct FrSkyDSensor {
|
|
||||||
const uint8_t id;
|
|
||||||
const char * name;
|
|
||||||
const TelemetryUnit unit;
|
|
||||||
const uint8_t prec;
|
|
||||||
};
|
|
||||||
|
|
||||||
const FrSkyDSensor frskyDSensors[] = {
|
|
||||||
{ D_RSSI_ID, ZSTR_RSSI, UNIT_RAW, 0 },
|
|
||||||
{ D_A1_ID, ZSTR_A1, UNIT_VOLTS, 1 },
|
|
||||||
{ D_A2_ID, ZSTR_A2, UNIT_VOLTS, 1 },
|
|
||||||
{ RPM_ID, ZSTR_RPM, UNIT_RPMS, 0 },
|
|
||||||
{ FUEL_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
|
|
||||||
{ TEMP1_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
|
||||||
{ TEMP2_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
|
||||||
{ CURRENT_ID, ZSTR_CURR, UNIT_AMPS, 1 },
|
|
||||||
{ ACCEL_X_ID, ZSTR_ACCX, UNIT_G, 3 },
|
|
||||||
{ ACCEL_Y_ID, ZSTR_ACCY, UNIT_G, 3 },
|
|
||||||
{ ACCEL_Z_ID, ZSTR_ACCZ, UNIT_G, 3 },
|
|
||||||
{ VARIO_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
|
|
||||||
{ VFAS_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
|
|
||||||
{ BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 1 }, // we map hi precision vario into PREC1!
|
|
||||||
{ VOLTS_AP_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
|
|
||||||
{ GPS_SPEED_BP_ID, ZSTR_GSPD, UNIT_KTS, 0 },
|
|
||||||
{ GPS_COURS_BP_ID, ZSTR_HDG, UNIT_DEGREE, 0 },
|
|
||||||
{ VOLTS_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
|
|
||||||
{ GPS_ALT_BP_ID, ZSTR_GPSALT, UNIT_METERS, 0 },
|
|
||||||
{ GPS_HOUR_MIN_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
|
|
||||||
{ GPS_LAT_AP_ID, ZSTR_GPS, UNIT_GPS, 0 },
|
|
||||||
{ 0, NULL, UNIT_RAW, 0 } // sentinel
|
|
||||||
};
|
|
||||||
|
|
||||||
const FrSkyDSensor * getFrSkyDSensor(uint8_t id)
|
|
||||||
{
|
|
||||||
const FrSkyDSensor * result = NULL;
|
|
||||||
for (const FrSkyDSensor * sensor = frskyDSensors; sensor->id; sensor++) {
|
|
||||||
if (id == sensor->id) {
|
|
||||||
result = sensor;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
void processHubPacket(uint8_t id, int16_t value)
|
|
||||||
{
|
|
||||||
static uint8_t lastId = 0;
|
|
||||||
static uint16_t lastValue = 0;
|
|
||||||
TelemetryUnit unit = UNIT_RAW;
|
|
||||||
uint8_t precision = 0;
|
|
||||||
int32_t data = value;
|
|
||||||
|
|
||||||
if (id > FRSKY_LAST_ID || id == GPS_SPEED_AP_ID || id == GPS_ALT_AP_ID || id == GPS_COURS_AP_ID) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (id == GPS_LAT_BP_ID || id == GPS_LONG_BP_ID || id == BARO_ALT_BP_ID || id == VOLTS_BP_ID) {
|
|
||||||
lastId = id;
|
|
||||||
lastValue = value;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (id == GPS_LAT_AP_ID) {
|
|
||||||
if (lastId == GPS_LAT_BP_ID) {
|
|
||||||
data += lastValue << 16;
|
|
||||||
unit = UNIT_GPS_LATITUDE;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (id == GPS_LONG_AP_ID) {
|
|
||||||
if (lastId == GPS_LONG_BP_ID) {
|
|
||||||
data += lastValue << 16;
|
|
||||||
id = GPS_LAT_AP_ID;
|
|
||||||
unit = UNIT_GPS_LONGITUDE;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (id == GPS_LAT_NS_ID) {
|
|
||||||
id = GPS_LAT_AP_ID;
|
|
||||||
unit = UNIT_GPS_LATITUDE_NS;
|
|
||||||
}
|
|
||||||
else if (id == GPS_LONG_EW_ID) {
|
|
||||||
id = GPS_LAT_AP_ID;
|
|
||||||
unit = UNIT_GPS_LONGITUDE_EW;
|
|
||||||
}
|
|
||||||
else if (id == BARO_ALT_AP_ID) {
|
|
||||||
if (lastId == BARO_ALT_BP_ID) {
|
|
||||||
if (data > 9 || frskyData.varioHighPrecision) {
|
|
||||||
frskyData.varioHighPrecision = true;
|
|
||||||
data /= 10; // map hi precision vario into low precision. Altitude is stored in 0.1m anyways
|
|
||||||
}
|
|
||||||
data = (int16_t)lastValue * 10 + (((int16_t)lastValue < 0) ? -data : data);
|
|
||||||
unit = UNIT_METERS;
|
|
||||||
precision = 1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (id == VOLTS_AP_ID) {
|
|
||||||
if (lastId == VOLTS_BP_ID) {
|
|
||||||
data = ((lastValue * 100 + value * 10) * 210) / 110;
|
|
||||||
unit = UNIT_VOLTS;
|
|
||||||
precision = 2;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (id == VOLTS_ID) {
|
|
||||||
unit = UNIT_CELLS;
|
|
||||||
uint32_t cellData = (uint32_t)data;
|
|
||||||
data = ((cellData & 0x00F0) << 12) + (((((cellData & 0xFF00) >> 8) + ((cellData & 0x000F) << 8))) / 5);
|
|
||||||
}
|
|
||||||
else if (id == GPS_DAY_MONTH_ID) {
|
|
||||||
id = GPS_HOUR_MIN_ID;
|
|
||||||
unit = UNIT_DATETIME_DAY_MONTH;
|
|
||||||
}
|
|
||||||
else if (id == GPS_HOUR_MIN_ID) {
|
|
||||||
unit = UNIT_DATETIME_HOUR_MIN;
|
|
||||||
}
|
|
||||||
else if (id == GPS_SEC_ID) {
|
|
||||||
id = GPS_HOUR_MIN_ID;
|
|
||||||
unit = UNIT_DATETIME_SEC;
|
|
||||||
}
|
|
||||||
else if (id == GPS_YEAR_ID) {
|
|
||||||
id = GPS_HOUR_MIN_ID;
|
|
||||||
unit = UNIT_DATETIME_YEAR;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
const FrSkyDSensor * sensor = getFrSkyDSensor(id);
|
|
||||||
if (sensor) {
|
|
||||||
unit = sensor->unit;
|
|
||||||
precision = sensor->prec;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (id == RPM_ID) {
|
|
||||||
data = data * 60;
|
|
||||||
}
|
|
||||||
else if (id == VFAS_ID) {
|
|
||||||
if (data >= VFAS_D_HIPREC_OFFSET) {
|
|
||||||
// incoming value has a resolution of 0.01V and added offset of VFAS_D_HIPREC_OFFSET
|
|
||||||
data -= VFAS_D_HIPREC_OFFSET;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
// incoming value has a resolution of 0.1V
|
|
||||||
data *= 10;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, id, 0, data, unit, precision);
|
|
||||||
}
|
|
||||||
|
|
||||||
void frskyDSetDefault(int index, uint16_t id)
|
|
||||||
{
|
|
||||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
|
||||||
|
|
||||||
telemetrySensor.id = id;
|
|
||||||
telemetrySensor.instance = 0;
|
|
||||||
|
|
||||||
const FrSkyDSensor * sensor = getFrSkyDSensor(id);
|
|
||||||
if (sensor) {
|
|
||||||
TelemetryUnit unit = sensor->unit;
|
|
||||||
uint8_t prec = min<uint8_t>(2, sensor->prec);
|
|
||||||
telemetrySensor.init(sensor->name, unit, prec);
|
|
||||||
if (id == D_RSSI_ID) {
|
|
||||||
telemetrySensor.filter = 1;
|
|
||||||
telemetrySensor.logs = true;
|
|
||||||
}
|
|
||||||
else if (id >= D_A1_ID && id <= D_A2_ID) {
|
|
||||||
telemetrySensor.custom.ratio = 132;
|
|
||||||
telemetrySensor.filter = 1;
|
|
||||||
}
|
|
||||||
else if (id == CURRENT_ID) {
|
|
||||||
telemetrySensor.onlyPositive = 1;
|
|
||||||
}
|
|
||||||
else if (id == BARO_ALT_AP_ID) {
|
|
||||||
telemetrySensor.autoOffset = 1;
|
|
||||||
}
|
|
||||||
if (unit == UNIT_RPMS) {
|
|
||||||
telemetrySensor.custom.ratio = 1;
|
|
||||||
telemetrySensor.custom.offset = 1;
|
|
||||||
}
|
|
||||||
else if (unit == UNIT_METERS) {
|
|
||||||
if (IS_IMPERIAL_ENABLE()) {
|
|
||||||
telemetrySensor.unit = UNIT_FEET;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
telemetrySensor.init(id);
|
|
||||||
}
|
|
||||||
|
|
||||||
eeDirty(EE_MODEL);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
|
||||||
304
radio/src/telemetry/frsky_d_arm.cpp
Normal file
304
radio/src/telemetry/frsky_d_arm.cpp
Normal file
|
|
@ -0,0 +1,304 @@
|
||||||
|
/*
|
||||||
|
* Authors (alphabetical order)
|
||||||
|
* - Andre Bernet <bernet.andre@gmail.com>
|
||||||
|
* - Andreas Weitl
|
||||||
|
* - Bertrand Songis <bsongis@gmail.com>
|
||||||
|
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
|
||||||
|
* - Cameron Weeks <th9xer@gmail.com>
|
||||||
|
* - Erez Raviv
|
||||||
|
* - Gabriel Birkus
|
||||||
|
* - Jean-Pierre Parisy
|
||||||
|
* - Karl Szmutny
|
||||||
|
* - Michael Blandford
|
||||||
|
* - Michal Hlavinka
|
||||||
|
* - Pat Mackenzie
|
||||||
|
* - Philip Moss
|
||||||
|
* - Rob Thomson
|
||||||
|
* - Romolo Manfredini <romolo.manfredini@gmail.com>
|
||||||
|
* - Thomas Husterer
|
||||||
|
*
|
||||||
|
* opentx is based on code named
|
||||||
|
* gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
|
||||||
|
* er9x by Erez Raviv: http://code.google.com/p/er9x/,
|
||||||
|
* and the original (and ongoing) project by
|
||||||
|
* Thomas Husterer, th9x: http://code.google.com/p/th9x/
|
||||||
|
*
|
||||||
|
* 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"
|
||||||
|
|
||||||
|
#if defined(FRSKY_HUB)
|
||||||
|
void parseTelemHubByte(uint8_t byte)
|
||||||
|
{
|
||||||
|
static int8_t structPos;
|
||||||
|
static uint8_t lowByte;
|
||||||
|
static TS_STATE state = TS_IDLE;
|
||||||
|
|
||||||
|
if (byte == 0x5e) {
|
||||||
|
state = TS_DATA_ID;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (state == TS_IDLE) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (state & TS_XOR) {
|
||||||
|
byte = byte ^ 0x60;
|
||||||
|
state = (TS_STATE)(state - TS_XOR);
|
||||||
|
}
|
||||||
|
else if (byte == 0x5d) {
|
||||||
|
state = (TS_STATE)(state | TS_XOR);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (state == TS_DATA_ID) {
|
||||||
|
if (byte > 0x3f) {
|
||||||
|
state = TS_IDLE;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
structPos = byte;
|
||||||
|
state = TS_DATA_LOW;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (state == TS_DATA_LOW) {
|
||||||
|
lowByte = byte;
|
||||||
|
state = TS_DATA_HIGH;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
state = TS_IDLE;
|
||||||
|
processHubPacket(structPos, (byte << 8) + lowByte);
|
||||||
|
}
|
||||||
|
#endif // #if defined(FRSKY_HUB)
|
||||||
|
|
||||||
|
void frskyDProcessPacket(uint8_t *packet)
|
||||||
|
{
|
||||||
|
// What type of packet?
|
||||||
|
switch (packet[0])
|
||||||
|
{
|
||||||
|
case LINKPKT: // A1/A2/RSSI values
|
||||||
|
{
|
||||||
|
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, packet[1], UNIT_VOLTS, 0);
|
||||||
|
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, packet[2], UNIT_VOLTS, 0);
|
||||||
|
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, packet[3], UNIT_RAW, 0);
|
||||||
|
frskyData.rssi.set(packet[3]);
|
||||||
|
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case USRPKT: // User Data packet
|
||||||
|
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
|
||||||
|
for (uint8_t i=3; i<numBytes; i++) {
|
||||||
|
parseTelemHubByte(packet[i]);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
struct FrSkyDSensor {
|
||||||
|
const uint8_t id;
|
||||||
|
const char * name;
|
||||||
|
const TelemetryUnit unit;
|
||||||
|
const uint8_t prec;
|
||||||
|
};
|
||||||
|
|
||||||
|
const FrSkyDSensor frskyDSensors[] = {
|
||||||
|
{ D_RSSI_ID, ZSTR_RSSI, UNIT_RAW, 0 },
|
||||||
|
{ D_A1_ID, ZSTR_A1, UNIT_VOLTS, 1 },
|
||||||
|
{ D_A2_ID, ZSTR_A2, UNIT_VOLTS, 1 },
|
||||||
|
{ RPM_ID, ZSTR_RPM, UNIT_RPMS, 0 },
|
||||||
|
{ FUEL_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
|
||||||
|
{ TEMP1_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
||||||
|
{ TEMP2_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
||||||
|
{ CURRENT_ID, ZSTR_CURR, UNIT_AMPS, 1 },
|
||||||
|
{ ACCEL_X_ID, ZSTR_ACCX, UNIT_G, 3 },
|
||||||
|
{ ACCEL_Y_ID, ZSTR_ACCY, UNIT_G, 3 },
|
||||||
|
{ ACCEL_Z_ID, ZSTR_ACCZ, UNIT_G, 3 },
|
||||||
|
{ VARIO_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
|
||||||
|
{ VFAS_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
|
||||||
|
{ BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 1 }, // we map hi precision vario into PREC1!
|
||||||
|
{ VOLTS_AP_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
|
||||||
|
{ GPS_SPEED_BP_ID, ZSTR_GSPD, UNIT_KTS, 0 },
|
||||||
|
{ GPS_COURS_BP_ID, ZSTR_HDG, UNIT_DEGREE, 0 },
|
||||||
|
{ VOLTS_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
|
||||||
|
{ GPS_ALT_BP_ID, ZSTR_GPSALT, UNIT_METERS, 0 },
|
||||||
|
{ GPS_HOUR_MIN_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
|
||||||
|
{ GPS_LAT_AP_ID, ZSTR_GPS, UNIT_GPS, 0 },
|
||||||
|
{ 0, NULL, UNIT_RAW, 0 } // sentinel
|
||||||
|
};
|
||||||
|
|
||||||
|
const FrSkyDSensor * getFrSkyDSensor(uint8_t id)
|
||||||
|
{
|
||||||
|
const FrSkyDSensor * result = NULL;
|
||||||
|
for (const FrSkyDSensor * sensor = frskyDSensors; sensor->id; sensor++) {
|
||||||
|
if (id == sensor->id) {
|
||||||
|
result = sensor;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
void processHubPacket(uint8_t id, int16_t value)
|
||||||
|
{
|
||||||
|
static uint8_t lastId = 0;
|
||||||
|
static uint16_t lastValue = 0;
|
||||||
|
TelemetryUnit unit = UNIT_RAW;
|
||||||
|
uint8_t precision = 0;
|
||||||
|
int32_t data = value;
|
||||||
|
|
||||||
|
if (id > FRSKY_LAST_ID || id == GPS_SPEED_AP_ID || id == GPS_ALT_AP_ID || id == GPS_COURS_AP_ID) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (id == GPS_LAT_BP_ID || id == GPS_LONG_BP_ID || id == BARO_ALT_BP_ID || id == VOLTS_BP_ID) {
|
||||||
|
lastId = id;
|
||||||
|
lastValue = value;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (id == GPS_LAT_AP_ID) {
|
||||||
|
if (lastId == GPS_LAT_BP_ID) {
|
||||||
|
data += lastValue << 16;
|
||||||
|
unit = UNIT_GPS_LATITUDE;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (id == GPS_LONG_AP_ID) {
|
||||||
|
if (lastId == GPS_LONG_BP_ID) {
|
||||||
|
data += lastValue << 16;
|
||||||
|
id = GPS_LAT_AP_ID;
|
||||||
|
unit = UNIT_GPS_LONGITUDE;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (id == GPS_LAT_NS_ID) {
|
||||||
|
id = GPS_LAT_AP_ID;
|
||||||
|
unit = UNIT_GPS_LATITUDE_NS;
|
||||||
|
}
|
||||||
|
else if (id == GPS_LONG_EW_ID) {
|
||||||
|
id = GPS_LAT_AP_ID;
|
||||||
|
unit = UNIT_GPS_LONGITUDE_EW;
|
||||||
|
}
|
||||||
|
else if (id == BARO_ALT_AP_ID) {
|
||||||
|
if (lastId == BARO_ALT_BP_ID) {
|
||||||
|
if (data > 9 || frskyData.varioHighPrecision) {
|
||||||
|
frskyData.varioHighPrecision = true;
|
||||||
|
data /= 10; // map hi precision vario into low precision. Altitude is stored in 0.1m anyways
|
||||||
|
}
|
||||||
|
data = (int16_t)lastValue * 10 + (((int16_t)lastValue < 0) ? -data : data);
|
||||||
|
unit = UNIT_METERS;
|
||||||
|
precision = 1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (id == VOLTS_AP_ID) {
|
||||||
|
if (lastId == VOLTS_BP_ID) {
|
||||||
|
data = ((lastValue * 100 + value * 10) * 210) / 110;
|
||||||
|
unit = UNIT_VOLTS;
|
||||||
|
precision = 2;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (id == VOLTS_ID) {
|
||||||
|
unit = UNIT_CELLS;
|
||||||
|
uint32_t cellData = (uint32_t)data;
|
||||||
|
data = ((cellData & 0x00F0) << 12) + (((((cellData & 0xFF00) >> 8) + ((cellData & 0x000F) << 8))) / 5);
|
||||||
|
}
|
||||||
|
else if (id == GPS_DAY_MONTH_ID) {
|
||||||
|
id = GPS_HOUR_MIN_ID;
|
||||||
|
unit = UNIT_DATETIME_DAY_MONTH;
|
||||||
|
}
|
||||||
|
else if (id == GPS_HOUR_MIN_ID) {
|
||||||
|
unit = UNIT_DATETIME_HOUR_MIN;
|
||||||
|
}
|
||||||
|
else if (id == GPS_SEC_ID) {
|
||||||
|
id = GPS_HOUR_MIN_ID;
|
||||||
|
unit = UNIT_DATETIME_SEC;
|
||||||
|
}
|
||||||
|
else if (id == GPS_YEAR_ID) {
|
||||||
|
id = GPS_HOUR_MIN_ID;
|
||||||
|
unit = UNIT_DATETIME_YEAR;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
const FrSkyDSensor * sensor = getFrSkyDSensor(id);
|
||||||
|
if (sensor) {
|
||||||
|
unit = sensor->unit;
|
||||||
|
precision = sensor->prec;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (id == RPM_ID) {
|
||||||
|
data = data * 60;
|
||||||
|
}
|
||||||
|
else if (id == VFAS_ID) {
|
||||||
|
if (data >= VFAS_D_HIPREC_OFFSET) {
|
||||||
|
// incoming value has a resolution of 0.01V and added offset of VFAS_D_HIPREC_OFFSET
|
||||||
|
data -= VFAS_D_HIPREC_OFFSET;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// incoming value has a resolution of 0.1V
|
||||||
|
data *= 10;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
setTelemetryValue(TELEM_PROTO_FRSKY_D, id, 0, data, unit, precision);
|
||||||
|
}
|
||||||
|
|
||||||
|
void frskyDSetDefault(int index, uint16_t id)
|
||||||
|
{
|
||||||
|
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||||
|
|
||||||
|
telemetrySensor.id = id;
|
||||||
|
telemetrySensor.instance = 0;
|
||||||
|
|
||||||
|
const FrSkyDSensor * sensor = getFrSkyDSensor(id);
|
||||||
|
if (sensor) {
|
||||||
|
TelemetryUnit unit = sensor->unit;
|
||||||
|
uint8_t prec = min<uint8_t>(2, sensor->prec);
|
||||||
|
telemetrySensor.init(sensor->name, unit, prec);
|
||||||
|
if (id == D_RSSI_ID) {
|
||||||
|
telemetrySensor.filter = 1;
|
||||||
|
telemetrySensor.logs = true;
|
||||||
|
}
|
||||||
|
else if (id >= D_A1_ID && id <= D_A2_ID) {
|
||||||
|
telemetrySensor.custom.ratio = 132;
|
||||||
|
telemetrySensor.filter = 1;
|
||||||
|
}
|
||||||
|
else if (id == CURRENT_ID) {
|
||||||
|
telemetrySensor.onlyPositive = 1;
|
||||||
|
}
|
||||||
|
else if (id == BARO_ALT_AP_ID) {
|
||||||
|
telemetrySensor.autoOffset = 1;
|
||||||
|
}
|
||||||
|
if (unit == UNIT_RPMS) {
|
||||||
|
telemetrySensor.custom.ratio = 1;
|
||||||
|
telemetrySensor.custom.offset = 1;
|
||||||
|
}
|
||||||
|
else if (unit == UNIT_METERS) {
|
||||||
|
if (IS_IMPERIAL_ENABLE()) {
|
||||||
|
telemetrySensor.unit = UNIT_FEET;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
telemetrySensor.init(id);
|
||||||
|
}
|
||||||
|
|
||||||
|
eeDirty(EE_MODEL);
|
||||||
|
}
|
||||||
Loading…
Add table
Add a link
Reference in a new issue