fix(telemetry): process DSM LIPO Cells Monitor sensor correctly (#4720)
This commit is contained in:
@@ -49,6 +49,7 @@
|
||||
#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 I2C_PSEUDO_TX_CELLS (I2C_PSEUDO_TX << 8 | 10)
|
||||
|
||||
#define SPEKTRUM_TELEMETRY_LENGTH 18
|
||||
#define DSM_BIND_PACKET_LENGTH 12
|
||||
@@ -142,6 +143,7 @@ struct SpektrumSensor {
|
||||
|
||||
#define SS(i2caddress,startByte,dataType,name,unit,precision) {i2caddress,startByte,dataType,precision,unit,name}
|
||||
|
||||
// IMPORTANT: Keep the sensor table incremtally sorted by i2caddress
|
||||
const SpektrumSensor spektrumSensors[] = {
|
||||
// 0x01 High voltage internal sensor
|
||||
SS(I2C_VOLTAGE, 0, int16, STR_SENSOR_A1, UNIT_VOLTS, 2), // 0.01V increments
|
||||
@@ -268,13 +270,13 @@ const SpektrumSensor spektrumSensors[] = {
|
||||
//SS(0x38, 0, uint16, STR_SENSOR_PRESSSURE, UNIT_PSI, 1),
|
||||
|
||||
// 0x3A Lipo 6s Monitor Cells
|
||||
SS(I2C_CELLS, 0, uint16, STR_SENSOR_CELLS, UNIT_VOLTS, 2), // Voltage across cell 1, .01V steps
|
||||
SS(I2C_CELLS, 2, uint16, STR_SENSOR_CELLS, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 4, uint16, STR_SENSOR_CELLS, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 6, uint16, STR_SENSOR_CELLS, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 8, uint16, STR_SENSOR_CELLS, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 10, uint16, STR_SENSOR_CELLS, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 12, uint16, STR_SENSOR_TEMP2, UNIT_CELSIUS, 2), // Temperature, 0.1C (0-655.34C)
|
||||
SS(I2C_CELLS, 0, uint16, STR_SENSOR_CL01, UNIT_VOLTS, 2), // Voltage across cell 1, .01V steps
|
||||
SS(I2C_CELLS, 2, uint16, STR_SENSOR_CL02, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 4, uint16, STR_SENSOR_CL03, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 6, uint16, STR_SENSOR_CL04, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 8, uint16, STR_SENSOR_CL05, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 10, uint16, STR_SENSOR_CL06, UNIT_VOLTS, 2),
|
||||
SS(I2C_CELLS, 12, uint16, STR_SENSOR_TEMP2, UNIT_CELSIUS, 1), // Temperature, 0.1C (0-655.34C)
|
||||
|
||||
// 0x40 Vario-S
|
||||
SS(I2C_VARIO, 0, int16, STR_SENSOR_ALT, UNIT_METERS, 1),
|
||||
@@ -350,6 +352,7 @@ const SpektrumSensor spektrumSensors[] = {
|
||||
SS(I2C_PSEUDO_TX, 0, uint8, STR_SENSOR_TX_RSSI, UNIT_RAW, 0),
|
||||
SS(I2C_PSEUDO_TX, 4, uint32, STR_SENSOR_BIND, UNIT_RAW, 0),
|
||||
SS(I2C_PSEUDO_TX, 8, uint32, STR_SENSOR_FLIGHT_MODE, UNIT_TEXT, 0),
|
||||
SS(I2C_PSEUDO_TX, 10, uint32, STR_SENSOR_CELLS, UNIT_CELLS, 2),
|
||||
SS(0, 0, int16, NULL, UNIT_RAW, 0) //sentinel
|
||||
};
|
||||
|
||||
@@ -625,11 +628,13 @@ void processSpektrumPacket(const uint8_t *packet)
|
||||
|
||||
bool handled = false;
|
||||
for (const SpektrumSensor * sensor = spektrumSensors; sensor->i2caddress; sensor++) {
|
||||
uint16_t pseudoId = (sensor->i2caddress << 8 | sensor->startByte);
|
||||
|
||||
if (i2cAddress != sensor->i2caddress) // Not the sensor for current packet
|
||||
// Optimization... the sensor table is sorted incrementally by i2cAddress
|
||||
if (sensor->i2caddress < i2cAddress) // haven't reach the sesnor def. keep going
|
||||
continue;
|
||||
|
||||
if (sensor->i2caddress > i2cAddress) // We past it, done
|
||||
break;
|
||||
|
||||
uint16_t pseudoId = (sensor->i2caddress << 8 | sensor->startByte);
|
||||
handled = true;
|
||||
|
||||
// Extract value, skip header
|
||||
@@ -666,9 +671,14 @@ void processSpektrumPacket(const uint8_t *packet)
|
||||
} // I2C_ESC
|
||||
|
||||
else if (i2cAddress == I2C_CELLS && sensor->unit == UNIT_VOLTS) {
|
||||
// Map to FrSky style cell values
|
||||
if (value == 0x7FFF) continue; // ignore NO-DATA
|
||||
|
||||
// Map to FrSky style cell values (All Cells in a single Sensor)
|
||||
int cellIndex = (sensor->startByte / 2) << 16;
|
||||
value = value | cellIndex;
|
||||
uint32_t valueCells = cellIndex | value;
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, I2C_PSEUDO_TX_CELLS, 0, instance, valueCells, UNIT_CELLS, 2);
|
||||
|
||||
// Continue to process regular Single Cell value
|
||||
} // I2C_CELLS
|
||||
|
||||
else if (sensor->i2caddress == I2C_HIGH_CURRENT && sensor->unit == UNIT_AMPS) {
|
||||
@@ -1081,6 +1091,7 @@ static int testStep = 0;
|
||||
static bool real0x16 = false;
|
||||
static bool real0x17 = false;
|
||||
static bool real0x34 = false;
|
||||
static bool real0x3A = false;
|
||||
|
||||
// *********** GPS LOC (BCD) ******************************
|
||||
// Example 0x16: 0 1 2 3 4 5 6 7 8 9 10 11 12 13
|
||||
@@ -1106,6 +1117,15 @@ static char test17data[] = {0x17, 0x00, 0x25, 0x00, 0x00,
|
||||
static char test34data[] = {0x34, 0x00, 0x2F, 0x00, 0x30, 0x09, 0x85, 0x01,
|
||||
0x2B, 0x00, 0x07, 0x0A, 0x81, 0x01 };
|
||||
|
||||
// *********** Lipo monitor (Big-Endian)***************
|
||||
// Example 0x3A: 0 1 2 3 4 5 6 7 8 9 10 11 12 13
|
||||
// 3A 00 | 01 9A | 01 9B | 01 9C | 01 9D | 7F FF | 7F FF | 0F AC
|
||||
// 4.10V 4.11V 4.12V 4.12v -- -- 40.1C
|
||||
static char test3Adata[] = {0x3A, 0x00, 0x01, 0x9A, 0x01, 0x9B, 0x01, 0x9C,
|
||||
0x01, 0x9D, 0x7F, 0xFF, 0x7F, 0xFF,
|
||||
0x01, 0x91 };
|
||||
|
||||
|
||||
static uint8_t replaceForTestingPackage(const uint8_t *packet)
|
||||
{
|
||||
uint8_t i2cAddress = packet[2] & 0x7f;
|
||||
@@ -1114,6 +1134,7 @@ static uint8_t replaceForTestingPackage(const uint8_t *packet)
|
||||
if (i2cAddress == I2C_GPS_LOC) real0x16 = true;
|
||||
else if (i2cAddress == I2C_GPS_STAT) real0x17 = true;
|
||||
else if (i2cAddress == I2C_FP_BATT) real0x34 = true;
|
||||
else if (i2cAddress == I2C_CELLS) real0x3A = true;
|
||||
|
||||
// Only Substiture AS3X/SAFE I2C_FLITECTRL packages, since they are constantly brodcast
|
||||
if (i2cAddress != I2C_FLITECTRL) {
|
||||
@@ -1137,9 +1158,12 @@ static uint8_t replaceForTestingPackage(const uint8_t *packet)
|
||||
case 3: // Return Dual Bat monitor
|
||||
if (!real0x34) memcpy((char *)packet + 2, test34data, 14);
|
||||
break;
|
||||
case 4: // Return LIPO monitor
|
||||
if (!real0x3A) memcpy((char *)packet + 2, test3Adata, 16);
|
||||
break;
|
||||
}
|
||||
|
||||
testStep = (testStep + 1) % 4;
|
||||
testStep = (testStep + 1) % 5;
|
||||
|
||||
|
||||
return packet[2] & 0x7f;
|
||||
|
||||
Reference in New Issue
Block a user