Skip to content

Commit

Permalink
fix(telemetry): process DSM LIPO Cells Monitor sensor correctly (#4720)
Browse files Browse the repository at this point in the history
  • Loading branch information
@frankiearzu
frankiearzu authored Mar 13, 2024
1 parent 3085603 commit 6ad7910
Showing 1 changed file with 38 additions and 14 deletions.
52 changes: 38 additions & 14 deletions radio/src/telemetry/spektrum.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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
};

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand All @@ -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) {
Expand All @@ -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;
Expand Down

0 comments on commit 6ad7910

Please sign in to comment.