piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

fix(DSM telemetry): GPS Sensor, FM for AS3X and TZ adjustment (#3366)

Browse Source 
* #3290 Fix for GSP

* Fix GPS Telemetry problem.

* Added FlightMode for some RX   (AS3X and ALPHA6-BLADE)

* #3290

Fix small problems.

* Add adjustment for TimeZone

Add adjustment for TimeZone

* chore: Mostly formatting, unused code removal

---------

Co-authored-by: Peter Feerick <peter.feerick@gmail.com>
This commit is contained in:
Frankie Arzu
2023-03-23 11:52:39 +10:00
committed by GitHub
co-authored by Peter Feerick
parent 623704f8b7
commit edd469645f
1 changed files with 313 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/spektrum.cpp
+313 -61
View File
@@ -46,18 +46,25 @@
#define I2C_PSEUDO_TX 0xf0
#define I2C_PSEUDO_TX_RSSI (I2C_PSEUDO_TX << 8 | 0)
#define I2C_PSEUDO_TX_BIND (I2C_PSEUDO_TX << 8 | 4)
#define I2C_PSEUDO_TX_FM (I2C_PSEUDO_TX << 8 | 8)
#define SPEKTRUM_TELEMETRY_LENGTH 18
#define DSM_BIND_PACKET_LENGTH 12
#define I2C_HIGH_CURRENT 0x03
#define I2C_FWD_PGM 0x09
#define I2C_TEXTGEN 0x0c
#define I2C_GPS 0x17
#define I2C_GPS2 0x17
#define I2C_ESC 0x20
#define I2C_CELLS 0x3a
#define I2C_HIGH_CURRENT 0x03
#define I2C_FLITECTRL 0x05
#define I2C_FWD_PGM 0x09
#define I2C_TEXTGEN 0x0c
#define I2C_GPS_LOC 0x16
#define I2C_GPS_STAT 0x17
#define I2C_ESC 0x20
#define I2C_ALPHA6 0x24
#define I2C_CELLS 0x3a
// SMART_BAT is using fake I2C adresses compared to official Spektrum address because of subtype used only for this I2C address
// SMART_BAT is using fake I2C adresses compared to official Spektrum address
// because of subtype used only for this I2C address
#define I2C_SMART_BAT_BASE_ADDRESS 0x42
#define I2C_SMART_BAT_REALTIME 0x42
#define I2C_SMART_BAT_CELLS_1_6 0x43
@@ -68,6 +75,28 @@
#define I2C_QOS 0x7f
// GPS flags definitions:
// Example: B9 (1100 1001) = IS_NORTH, !IS_EAST, !GREATER_99, IS_FIX_VALID,
// !DATA_RECEIVED, NEGATIVE_ALT
#define GPS_INFO_FLAGS_IS_NORTH 0x01
#define GPS_INFO_FLAGS_IS_EAST 0x02
#define GPS_INFO_FLAGS_LONGITUDE_GREATER_99 0x04
#define GPS_INFO_FLAGS_GPS_FIX_VALID 0x08
#define GPS_INFO_FLAGS_GPS_DATA_RECEIVED 0x10
#define GPS_INFO_FLAGS_3D_FIX 0x20
// #define GPS_INFO_FLAGS_BIT6 0x40 // ??
#define GPS_INFO_FLAGS_NEGATIVE_ALT 0x80
// FLITECTRL FLAGS
#define FLITECTRL_FLAGS_IS_AS3X_STAB 0x01
#define FLITECTRL_FLAGS_IS_ANGLE_DEMAND 0x02
#define FLITECTRL_FLAGS_IS_AS3X_HEADING 0x04
#define FLITECTRL_FLAGS_IS_SAFE_ENVELOPE 0x08
// 1= Test sensor with captured telementry date
// See replaceForTestingPackage(const uint8_t *packet) at the end of this file
#define TEST_CAPTURED_MESSAGE 0
enum SpektrumDataType : uint8_t {
int8,
int16,
@@ -116,7 +145,7 @@ const SpektrumSensor spektrumSensors[] = {
{0x0b, 0, uint32, STR_SENSOR_GATE_TIME, UNIT_SECONDS, 3},
// Text Generator
{I2C_TEXTGEN, 0, uint32, STR_SENSOR_FLIGHT_MODE, UNIT_TEXT, 0},
//{I2C_TEXTGEN, 0, uint32, STR_SENSOR_FLIGHT_MODE, UNIT_TEXT, 0},
// AirSpeed, also has max (+2, int16)
{0x11, 0, int16, STR_SENSOR_ASPD, UNIT_KMH, 0},
@@ -143,17 +172,21 @@ const SpektrumSensor spektrumSensors[] = {
{0x15, 10, uint16bcd, STR_SENSOR_TEMP1, UNIT_CELSIUS, 0},
// {0x15, 0, uint8, STR_SENSOR_STATUS, UNIT_BITFIELD, 0},
// 0x16-0x17 GPS
// GPS is bcd encoded and also uses flags. Hard to get right without an actual GPS Sensor
// Time/date is also BCD encoded but so this FrSky's, so treat it as uint32
{I2C_GPS2, 0, uint16bcd, STR_SENSOR_GSPD, UNIT_KTS, 1},
{I2C_GPS2, 2, uint32, STR_SENSOR_GPSDATETIME, UNIT_DATETIME, 0},
//{0x17, 2, uint32, STR_SENSOR_GPSDATETIME, UNIT_DATETIME}, utc in bcd HH:MM:SS.S
{0x17, 6, uint8bcd, STR_SENSOR_SATELLITES, UNIT_RAW, 0},
//{0x17, 7, uint8bcd, STR_SENSOR_GPSALT, UNIT_METERS}, altitude high bits
// 0x16 GPS LOG
// Example 0x16: 97 00 | 54 71 12 28 | 40 80 09 82 | 85 14 | 13 | B9
// Alt: 009.7, LAT: 28.127154, LON: -82.098040 Course: 148.5, HDOP 1.3 Flags= B9
{I2C_GPS_LOC, 0, uint16bcd, STR_SENSOR_GPSALT, UNIT_METERS, 1}, // Atl-Low BCD 3.1
{I2C_GPS_LOC, 2, uint32bcd, STR_SENSOR_GPS, UNIT_GPS, 0}, // LAT and LON
{I2C_GPS_LOC, 10, uint16bcd, STR_SENSOR_HDG, UNIT_DEGREE, 1}, // Course BCD 3.1
//{I2C_GPS_LOC, 12, uint8bcd, STR_SENSOR_HDOP ??, UNIT_RAW, 1}, // HDOP BCD 1.1
// 0x17 GPS STAT
// Example 0x17: 25 00 | 00 28 17 17 | 06 | 01
// Spd:002.5k, TimeUTC:17:17:28.00, Sats: 06, AltH=01
{I2C_GPS_STAT, 0, uint16bcd, STR_SENSOR_GSPD, UNIT_KTS, 1}, // BCD 3.1
{I2C_GPS_STAT, 2, uint32bcd, STR_SENSOR_GPSDATETIME, UNIT_DATETIME, 1}, // BCD 6.1 HH:MM:SS.S
{I2C_GPS_STAT, 6, uint8bcd, STR_SENSOR_SATELLITES, UNIT_RAW, 0}, // BCD 2.0
// 0x19 Jetcat flow rate
// {0x19, 0, uint16bcd, STR_SENSOR_FUEL_CONSUMPTION, UNIT_MILLILITERS_PER_MINUTE, 1}, missing ml/min
{0x19, 2, uint32bcd, STR_SENSOR_FUEL, UNIT_MILLILITERS, 1},
@@ -279,19 +312,32 @@ const SpektrumSensor spektrumSensors[] = {
{I2C_PSEUDO_TX, 0, uint8, STR_SENSOR_TX_RSSI, UNIT_RAW, 0},
{I2C_PSEUDO_TX, 4, uint32, STR_SENSOR_BIND, UNIT_RAW, 0},
{I2C_PSEUDO_TX, 8, uint32, STR_SENSOR_FLIGHT_MODE, UNIT_TEXT, 0},
{0, 0, int16, NULL, UNIT_RAW, 0} //sentinel
};
// The bcd int parameter has wrong endian
static int32_t bcdToInt16(uint16_t bcd)
{
return (bcd & 0x0f00) + 10 * (bcd & 0xf000) + 100 * (bcd & 0x000f) + 1000 * (bcd & 0x00f0);
}
// Alt Low and High needs to be combined (in 2 diff packets)
static uint8_t gpsAltHigh = 0;
// Helper function declared later
static void processAS3XPacket(const uint8_t *packet);
static void processAlpha6Packet(const uint8_t *packet);
static void adjustTimeFromUTC(uint8_t hour, uint8_t min, uint8_t sec,
struct gtm *tp);
#if TEST_CAPTURED_MESSAGE
static uint8_t replaceForTestingPackage(const uint8_t *packet);
#endif
// The bcd int parameter has wrong endian
static int32_t bcdToInt8(uint8_t bcd)
{
return (bcd & 0xf) + 10 * (bcd & 0xf0);
return (bcd & 0xf) + 10 * (bcd >> 4 & 0xf);
}
static int32_t bcdToInt16(uint16_t bcd)
{
return (bcd & 0xf) + 10 * (bcd >> 4 & 0xf) + 100 * (bcd >> 8 & 0xf) +
1000 * (bcd >> 12 & 0xf);
}
static int32_t bcdToInt32(uint32_t bcd)
@@ -349,10 +395,18 @@ bool isSpektrumValidValue(int32_t value, const SpektrumDataType type)
void processSpektrumPacket(const uint8_t *packet)
{
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 0, 0, 0, packet[1], UNIT_RAW, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, I2C_PSEUDO_TX_RSSI, 0, 0,
packet[1], UNIT_RAW, 0);
// highest bit indicates that TM1100 is in use, ignore it
uint8_t i2cAddress = (packet[2] & 0x7f);
#if TEST_CAPTURED_MESSAGE
// Only for Testing when we don't have the sensor, but have the data
if (i2cAddress == I2C_FLITECTRL) {
i2cAddress = replaceForTestingPackage(packet);
}
#endif
if (i2cAddress == I2C_FWD_PGM) {
#if defined(LUA) && defined(MULTIMODULE)
// Forward Programming
@@ -388,51 +442,53 @@ void processSpektrumPacket(const uint8_t *packet)
}
#endif
//SmartBat Hack
if (i2cAddress == I2C_SMART_BAT_BASE_ADDRESS) {
i2cAddress = i2cAddress + (packet[4] >> 4); // use type to create virtual I2CAddresses
}
// SmartBat Hack
if (i2cAddress == I2C_SMART_BAT_BASE_ADDRESS) {
// use type to create virtual I2CAddresses
i2cAddress = i2cAddress + (packet[4] >> 4);
}
uint8_t instance = packet[3];
if (i2cAddress == I2C_TEXTGEN) {
/* FArzu: This don't seem to work at all.. The sensors was always 0.
TextGen now can be acceced via the new "Spectrum Telemetry Raw" LUA method
uint8_t lineNumber = packet[4];
uint16_t pseudoId = (i2cAddress << 8 | lineNumber);
for (int i=5; i<SPEKTRUM_TELEMETRY_LENGTH; i++)
{
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance, packet[i], UNIT_TEXT, i-5);
}
// Set a sential \0 just for safety since we have the space there
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance, '\0', UNIT_TEXT, 13);
*/
if (i2cAddress == I2C_TEXTGEN) {
// TextGen now accessed via the new "Spectrum Telemetry Raw" LUA method
return; // Not a sensor
}
// AS3X + SAFE information: Flight Mode
if (i2cAddress == I2C_FLITECTRL) {
processAS3XPacket(packet);
// Continue for backward compatibility with scripts using 05XX sensors
}
// Alpha6 Flight Controller (Blade Helis): Flight Mode
if (i2cAddress == I2C_ALPHA6) {
processAlpha6Packet(packet);
// Continue for backward compatibility with scripts using 24XX sensors
}
bool handled = false;
for (const SpektrumSensor * sensor = spektrumSensors; sensor->i2caddress; sensor++) {
uint16_t pseudoId = (sensor->i2caddress << 8 | sensor->startByte);
if (i2cAddress == sensor->i2caddress) {
handled = true;
// Extract value, skip header
int32_t value = spektrumGetValue(packet + 4, sensor->startByte, sensor->dataType);
int32_t value =
spektrumGetValue(packet + 4, sensor->startByte, sensor->dataType);
if (!isSpektrumValidValue(value, sensor->dataType))
continue;
// mV to VOLT PREC2 for Smart Batteries
if ((i2cAddress >= I2C_SMART_BAT_REALTIME && i2cAddress <= I2C_SMART_BAT_LIMITS) && sensor->unit == UNIT_VOLTS) {
if ((i2cAddress >= I2C_SMART_BAT_REALTIME &&
i2cAddress <= I2C_SMART_BAT_LIMITS) &&
sensor->unit == UNIT_VOLTS) {
if (value == -1) {
continue; // discard unavailable sensors
}
else {
} else {
value = value / 10;
}
}
@@ -468,13 +524,11 @@ void processSpektrumPacket(const uint8_t *packet)
value = value | cellIndex;
}
if (sensor->i2caddress == I2C_HIGH_CURRENT && sensor->unit == UNIT_AMPS)
// Spektrum's documents talks says: Resolution: 300A/2048 = 0.196791 A/tick
// Note that 300/2048 = 0,1464. DeviationTX also uses the 0.196791 figure
if (sensor->i2caddress == I2C_HIGH_CURRENT && sensor->unit == UNIT_AMPS) {
// Spektrum's documents talks says: Resolution: 300A/2048 = 0.196791
// A/tick Note that 300/2048 = 0,1464. DeviationTX also uses the
// 0.196791 figure
value = value * 196791 / 100000;
else if (sensor->i2caddress == I2C_GPS2 && sensor->unit == UNIT_DATETIME) {
// Frsky time is HH:MM:SS:00 bcd encodes while spektrum uses 0HH:MM:SS.S
value = (value & 0xfffffff0) << 4;
}
// Check if this looks like a LemonRX Transceiver, they use QoS Frame loss A as RSSI indicator(0-100)
@@ -493,7 +547,94 @@ void processSpektrumPacket(const uint8_t *packet)
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
uint16_t pseudoId = (sensor->i2caddress << 8 | sensor->startByte);
//*********** GPS STAT *********************************
// Example 0x17: 0 1 2 3 4 5 6 7
// 25 00 | 00 28 15 17 | 06 | 01
// Spd:002.5k, TimeUTC:17:15:28.00, Sats: 06, AltH=01
if (sensor->i2caddress == I2C_GPS_STAT && sensor->unit == UNIT_DATETIME) {
const uint8_t *packetData = packet + 4; // Skip the header
uint8_t sec = bcdToInt8(packetData[3]);
uint8_t min = bcdToInt8(packetData[4]);
uint8_t hour = bcdToInt8(packetData[5]);
struct gtm td;
adjustTimeFromUTC(hour, min, sec, &td);
// Depending on the last byte it SETS: DATE: HEX (YYMMDD01) TIME:
// HEX(HHMMSS00)
value = (td.tm_hour << 24) + (td.tm_min << 16) + (td.tm_sec << 8);
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance,
value, UNIT_DATETIME, 0);
value = ((td.tm_year + 1900 - 2000) << 24) + ((td.tm_mon + 1) << 16) +
(td.tm_mday << 8) + 1;
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance,
value, UNIT_DATETIME, 0);
// Get Altitude High since we need to combine it with Alt-Low
// Save the high part for later (0-99)
gpsAltHigh = bcdToInt8(packetData[7]);
continue; // setTelemetryValue handled
}
//*********** GPS LOC *********************************
// Example 0x16: 0 1 2 3 4 5 6 7 8 9 10 11 12 13
// 97 00 | 54 71 12 28 | 40 80 09 82 | 85 14 | 13 | B9
// Alt: 009.7, LAT: 28o 12'7154, LON: -82 09 8040 Course: 148.5, HDOP 1.3 Flags= B9
// ALTITUDE LOW (METERS)
if (sensor->i2caddress == I2C_GPS_LOC && sensor->startByte == 0) {
uint8_t gpsFlags = packet[4 + 13];
// Format Decimal: HHLLLL, for display will be HHLLL.L
value = (gpsAltHigh * 1000) + value;
if (gpsFlags & GPS_INFO_FLAGS_NEGATIVE_ALT) {
value = -value;
}
}
// Process LAT and LOG together
if (sensor->i2caddress == I2C_GPS_LOC && sensor->unit == UNIT_GPS) {
const uint8_t *packetData = packet + 4; // Skip the header
uint8_t gpsFlags = packetData[13];
// LATITUDE
uint16_t fmin =
bcdToInt8(packetData[2]) + (bcdToInt8(packetData[3]) * 100);
uint8_t min = bcdToInt8(packetData[4]);
uint8_t deg = bcdToInt8(packetData[5]);
// formula from code in gps.cpp
value = deg * 1000000UL + (min * 100000UL + fmin * 10UL) / 6;
if ((gpsFlags & GPS_INFO_FLAGS_IS_NORTH) == 0) { // SOUTH, negative
value = -value;
}
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance,
value, UNIT_GPS_LATITUDE, 0);
// LONGITUDE
fmin = bcdToInt8(packetData[6]) + (bcdToInt8(packetData[7]) * 100);
min = bcdToInt8(packetData[8]);
deg = bcdToInt8(packetData[9]);
// formula from code in gps.cpp
value = deg * 1000000UL + (min * 100000UL + fmin * 10UL) / 6;
if ((gpsFlags & GPS_INFO_FLAGS_IS_EAST) == 0) { // WEST, negative
value = -value;
}
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance,
value, UNIT_GPS_LONGITUDE, 0);
continue; // setTelemetryValue handled
}
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance, value, sensor->unit, sensor->precision);
}
}
@@ -588,7 +729,8 @@ void processDSMBindPacket(uint8_t module, const uint8_t *packet)
}
g_model.moduleData[module].channelsCount = channels - 8;
g_model.moduleData[module].multi.optionValue &= 0xFD; // clear the 11ms servo refresh rate flag
// clear the 11ms servo refresh rate flag
g_model.moduleData[module].multi.optionValue &= 0xFD;
storageDirty(EE_MODEL);
}
@@ -596,7 +738,8 @@ void processDSMBindPacket(uint8_t module, const uint8_t *packet)
debugval = packet[7] << 24 | packet[6] << 16 | packet[5] << 8 | packet[4];
/* log the bind packet as telemetry for quick debugging */
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 4, 0, 0, debugval, UNIT_RAW, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, I2C_PSEUDO_TX_BIND, 0, 0,
debugval, UNIT_RAW, 0);
/* Finally stop binding as the rx just told us that it is bound */
if (getModuleMode(module) == MODULE_MODE_BIND) {
@@ -699,3 +842,112 @@ void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
storageDirty(EE_MODEL);
}
extern int __offtime(const gtime_t *t, long int offset, struct gtm *tp);
static void adjustTimeFromUTC(uint8_t hour, uint8_t min, uint8_t sec,
struct gtm *tp)
{
// Get current UTC date/time
__offtime(&g_rtcTime, -g_eeGeneral.timezone * 3600, tp);
tp->tm_hour = hour;
tp->tm_min = min;
tp->tm_sec = sec;
gtime_t newTime = gmktime(tp);
__offtime(&newTime, +g_eeGeneral.timezone * 3600, tp);
}
// AS3X/SAFE flight Controller
// Contains Flight Mode, and Gyro Settings
static void processAS3XPacket(const uint8_t *packet)
{
const uint8_t *packetData = packet + 4; // Skip the header
// uint8_t state = packetData[1];
uint8_t flags = packetData[0];
// uint8_t dataPage = packetData[3] & 0x0F;
// 0=Gains, 1=Headings, 2=Angle Limits
uint8_t flightMode = packetData[2] & 0x0F;
char text[50];
sprintf(text, "%d ", flightMode + 1);
if (flags & FLITECTRL_FLAGS_IS_AS3X_STAB) {
strcat(text, "AS3X");
}
// only one should show
if (flags & FLITECTRL_FLAGS_IS_ANGLE_DEMAND) {
strcat(text, " Level");
} else if (flags & FLITECTRL_FLAGS_IS_SAFE_ENVELOPE) {
strcat(text, " Envelope");
} else if (flags & FLITECTRL_FLAGS_IS_AS3X_HEADING) {
strcat(text, " Heading");
}
setTelemetryText(PROTOCOL_TELEMETRY_SPEKTRUM, I2C_PSEUDO_TX_FM, 0, 0, text);
}
// Alpha6 flight Controller (Blade Helis based on AR636)
// Contains Flight Mode, and Gyro Settings
static void processAlpha6Packet(const uint8_t *packet)
{
const uint8_t *packetData = packet + 4; // Skip the header
uint8_t status = packetData[2] & 0x0F;
uint8_t flightMode = packetData[2] >> 4 & 0x0F;
char text[50];
sprintf(text, "%d ", flightMode);
if (flightMode == 0) {
strcat(text, "NOR");
} else if (flightMode == 1) {
strcat(text, "INT");
} else if (flightMode == 2) {
strcat(text, "ADV");
} else if (flightMode == 5) {
strcat(text, "PANIC");
}
if (status == 2) {
strcat(text, " HOLD");
}
setTelemetryText(PROTOCOL_TELEMETRY_SPEKTRUM, I2C_PSEUDO_TX_FM, 0, 0, text);
}
#if TEST_CAPTURED_MESSAGE
// For Testing purposes, replace the package for data captured
bool testflag = false;
static uint8_t replaceForTestingPackage(const uint8_t *packet)
{
// *********** GPS LOC *********************************
// Example 0x16: 0 1 2 3 4 5 6 7 8 9 10 11 12 13
// 16 00 | 97 00 | 54 71 12 28 | 40 80 09 82 | 85 14 | 13 | B9
// Alt: 009.7, LAT: 28o 12'7154, LON: -82 09 8040 Course: 148.5, HDOP 1.3 Flags= B9
const char test16data[] = {0x16, 0x00, 0x97, 0x00, 0x54, 0x71, 0x12, 0x28,
0x40, 0x80, 0x09, 0x82, 0x85, 0x14, 0x13, 0xB9};
// *********** GPS STAT *********************************
// Example 0x17: 0 1 2 3 4 5 6 7
// 17 00 | 25 00 | 00 28 18 21 | 06 | 00
// Spd:002.5k, TimeUTC:21:18:28.00, Sats: 06, AltH=00
const char test17data[] = {0x17, 0x00, 0x25, 0x00, 0x00,
0x28, 0x18, 0x21, 0x06, 0x00};
if (testflag) {
memcpy((char *)packet + 2, test16data, 16);
} else {
memcpy((char *)packet + 2, test17data, 10);
}
testflag = !testflag;
return packet[2] & 0x7ff;
}
#endif