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

fix(telemetry): DSM/LemonRX sensor cleanup for Altimeter and Flight Pack Monitors (#4600)

Browse Source
This commit is contained in:
Frankie Arzu
2024-02-11 18:55:13 +10:00
committed by GitHub
parent 7c8c41313a
commit 454e70cedd
1 changed files with 85 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/spektrum.cpp
+85 -23
View File
@@ -355,6 +355,8 @@ const SpektrumSensor spektrumSensors[] = {
// Alt Low and High needs to be combined (in 2 diff packets)
static uint8_t gpsAltHigh = 0;
static bool varioTelemetry = false;
static bool flightPackTelemetry = false;
// Helper function declared later
static void processAS3XPacket(const uint8_t *packet);
@@ -543,6 +545,12 @@ void processSpektrumPacket(const uint8_t *packet)
uint8_t instance = packet[3];
if (telemetryState == TELEMETRY_INIT) { // Telemetry Reset?
gpsAltHigh = 0;
varioTelemetry = false;
flightPackTelemetry = false;
}
if (i2cAddress == I2C_NODATA) {
// Not a Sensor.. Telemetry is alive, but no data (avoid creation of fake 0000,0002.. sensors)
return;
@@ -670,20 +678,28 @@ void processSpektrumPacket(const uint8_t *packet)
value = value * 196791 / 100000;
} // I2C_HIGH_CURRENT
// Check if this looks like a LemonRX Transceiver, they use QoS Frame loss A as RSSI indicator(0-100)
else if (i2cAddress == I2C_QOS && sensor->startByte == 0) {
if (spektrumGetValue(packet + 4, 2, uint16) == 0x8000 &&
spektrumGetValue(packet + 4, 4, uint16) == 0x8000 &&
spektrumGetValue(packet + 4, 6, uint16) == 0x8000 &&
spektrumGetValue(packet + 4, 8, uint16) == 0x8000) {
telemetryData.rssi.set(value);
else if (i2cAddress == I2C_QOS) {
if (sensor->startByte == 0) { // FdeA
// Check if this looks like a LemonRX Transceiver, they use QoS Frame loss A as RSSI indicator(0-100)
// farzu: new G2s has different signature, but i think using the Cyrf chip strength is
// more consistent across brands
if (spektrumGetValue(packet + 4, 2, uint16) == 0x8000 &&
spektrumGetValue(packet + 4, 4, uint16) == 0x8000 &&
spektrumGetValue(packet + 4, 6, uint16) == 0x8000 &&
spektrumGetValue(packet + 4, 8, uint16) == 0x8000) {
telemetryData.rssi.set(value);
}
else {
// Otherwise use the received signal strength of the telemetry packet as indicator
// Range is 0-31, multiply by 3 to get an almost full reading for 0x1f, the maximum the cyrf chip reports
telemetryData.rssi.set(packet[1] * 3);
}
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // Telemery Alive
} // FdeA
else if (sensor->startByte == 8 || sensor->startByte == 10) { // Flss and Hold
// Lemon-RX: F and H = 0x7FFF (alternative N0-DATA)
if (value == 0x7FFF) continue;
}
else {
// Otherwise use the received signal strength of the telemetry packet as indicator
// Range is 0-31, multiply by 3 to get an almost full reading for 0x1f, the maximum the cyrf chip reports
telemetryData.rssi.set(packet[1] * 3);
}
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
} // I2C_QOS
else if (sensor->i2caddress == I2C_GPS_STAT && sensor->unit == UNIT_DATETIME) {
@@ -724,6 +740,39 @@ void processSpektrumPacket(const uint8_t *packet)
}
} // I2C_GPS_BIN
else if (i2cAddress == I2C_FP_BATT) {
flightPackTelemetry = true;
// Lemon-RX G2: No Bat2: Current (-1.0 A)
if (sensor->startByte == 6 && ((int16_t) value) == -10) {
continue;
}
} // I2C_FP_BATT
else if (i2cAddress == I2C_PBOX) {
if (flightPackTelemetry && (sensor->startByte == 4 || sensor->startByte == 6)) {
// hide mAh Consumption already reported in Fligh Pack message
continue;
}
else if ((sensor->startByte == 0 || sensor->startByte == 4) &&
spektrumGetValue(packet + 4, 0, uint16) == 0) {
// No Bat1 Voltage, hide Voltage and Consumption
continue;
}
else if ((sensor->startByte == 2 || sensor->startByte == 6) &&
spektrumGetValue(packet + 4, 2, uint16) == 0) {
// No Bat2 Voltage, hide Voltage and Consumption
continue;
}
} // I2C_PBOX
else if (i2cAddress == I2C_VARIO) {
varioTelemetry = true;
}
else if (i2cAddress == I2C_ALTITUDE && varioTelemetry) {
// Altitude already reported in vario
continue;
}
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, pseudoId, 0, instance, value, sensor->unit, sensor->precision);
} // FOR
@@ -887,9 +936,10 @@ void processSpektrumTelemetryData(uint8_t module, uint8_t data,
const SpektrumSensor *getSpektrumSensor(uint16_t pseudoId)
{
uint8_t startByte = (uint8_t) (pseudoId & 0xff);
uint8_t i2cadd = (uint8_t) (pseudoId >> 8);
for (const SpektrumSensor * sensor = spektrumSensors; sensor->i2caddress; sensor++) {
uint8_t startByte = (uint8_t)(pseudoId & 0xff);
uint8_t i2cadd = (uint8_t)(pseudoId >> 8);
for (const SpektrumSensor *sensor = spektrumSensors; sensor->i2caddress;
sensor++) {
if (i2cadd == sensor->i2caddress && startByte == sensor->startByte) {
return sensor;
}
@@ -913,20 +963,32 @@ void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
if (unit == UNIT_RPMS) {
telemetrySensor.custom.ratio = 1;
telemetrySensor.custom.offset = 1;
}
else if (unit == UNIT_FAHRENHEIT) {
} else if (unit == UNIT_FAHRENHEIT) {
if (!IS_IMPERIAL_ENABLE()) {
telemetrySensor.unit = UNIT_CELSIUS;
}
}
else if (unit == UNIT_METERS) {
} else if (unit == UNIT_CELSIUS) {
if (IS_IMPERIAL_ENABLE()) {
telemetrySensor.unit = UNIT_FAHRENHEIT;
}
} else if (unit == UNIT_METERS) {
if (IS_IMPERIAL_ENABLE()) {
telemetrySensor.unit = UNIT_FEET;
}
} else if (unit == UNIT_KMH) {
if (IS_IMPERIAL_ENABLE()) {
telemetrySensor.unit = UNIT_KTS;
}
} else if (unit == UNIT_METERS_PER_SECOND) {
if (IS_IMPERIAL_ENABLE()) {
telemetrySensor.unit = UNIT_FEET_PER_SECOND;
}
} else if (unit == UNIT_KTS) {
if (!IS_IMPERIAL_ENABLE()) {
telemetrySensor.unit = UNIT_KMH;
}
}
}
else {
} else {
telemetrySensor.init(id);
}