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

[Crossfire] Telemetry new fields (GPS / Battery)

Browse Source 
I did a refactoring for the latitude and longitude decoding which should be tested carefully for FrSky S.PORT and D telemetry
There was a double conversion in S.PORT protocol, there are less maths now, and perhaps precision will be better
This commit is contained in:
Bertrand Songis
2016-04-26 19:36:48 +02:00
parent 34444aedb2
commit 26e156f9ad
23 changed files with 243 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/crossfire.cpp
+106 -43
View File
@@ -25,45 +25,71 @@ struct CrossfireSensor {
const uint8_t subId;
const char * name;
const TelemetryUnit unit;
const uint8_t prec;
const uint8_t precision;
};
enum CrossfireSensorIndexes {
RX_RSSI1_INDEX,
RX_RSSI2_INDEX,
RX_QUALITY_INDEX,
RX_SNR_INDEX,
RX_ANTENNA_INDEX,
RF_MODE_INDEX,
TX_POWER_INDEX,
TX_RSSI_INDEX,
TX_QUALITY_INDEX,
TX_SNR_INDEX,
BATT_VOLTAGE_INDEX,
BATT_CURRENT_INDEX,
BATT_CAPACITY_INDEX,
GPS_LATITUDE_INDEX,
GPS_LONGITUDE_INDEX,
GPS_GROUND_SPEED_INDEX,
GPS_HEADING_INDEX,
GPS_ALTITUDE_INDEX,
GPS_SATELLITES_INDEX,
UNKNOW_INDEX,
};
const CrossfireSensor crossfireSensors[] = {
{LINK_STATS_ID, 0, ZSTR_RX_RSSI1, UNIT_DB, 0},
{LINK_STATS_ID, 1, ZSTR_RX_RSSI2, UNIT_DB, 0},
{LINK_STATS_ID, 2, ZSTR_RX_QUALITY, UNIT_PERCENT, 0},
{LINK_STATS_ID, 3, ZSTR_RX_SNR, UNIT_DB, 0},
{LINK_STATS_ID, 4, ZSTR_ANTENNA, UNIT_RAW, 0},
{LINK_STATS_ID, 5, ZSTR_RF_MODE, UNIT_RAW, 0},
{LINK_STATS_ID, 6, ZSTR_TX_POWER, UNIT_MILLIWATTS, 0},
{LINK_STATS_ID, 7, ZSTR_TX_RSSI, UNIT_DB, 0},
{LINK_STATS_ID, 8, ZSTR_TX_QUALITY, UNIT_PERCENT, 0},
{LINK_STATS_ID, 9, ZSTR_TX_SNR, UNIT_DB, 0},
{ 0, 0, NULL, UNIT_RAW, 0} // sentinel
{LINK_ID, 0, ZSTR_RX_RSSI1, UNIT_DB, 0},
{LINK_ID, 1, ZSTR_RX_RSSI2, UNIT_DB, 0},
{LINK_ID, 2, ZSTR_RX_QUALITY, UNIT_PERCENT, 0},
{LINK_ID, 3, ZSTR_RX_SNR, UNIT_DB, 0},
{LINK_ID, 4, ZSTR_ANTENNA, UNIT_RAW, 0},
{LINK_ID, 5, ZSTR_RF_MODE, UNIT_RAW, 0},
{LINK_ID, 6, ZSTR_TX_POWER, UNIT_MILLIWATTS, 0},
{LINK_ID, 7, ZSTR_TX_RSSI, UNIT_DB, 0},
{LINK_ID, 8, ZSTR_TX_QUALITY, UNIT_PERCENT, 0},
{LINK_ID, 9, ZSTR_TX_SNR, UNIT_DB, 0},
{BATTERY_ID, 0, ZSTR_BATT, UNIT_VOLTS, 1},
{BATTERY_ID, 1, ZSTR_CURR, UNIT_AMPS, 1},
{BATTERY_ID, 2, ZSTR_CAPACITY, UNIT_MAH, 0},
{GPS_ID, 0, ZSTR_GPS, UNIT_GPS_LATITUDE, 0},
{GPS_ID, 0, ZSTR_GPS, UNIT_GPS_LONGITUDE, 0},
{GPS_ID, 2, ZSTR_GSPD, UNIT_KMH, 1},
{GPS_ID, 3, ZSTR_HDG, UNIT_DEGREE, 3},
{GPS_ID, 4, ZSTR_ALT, UNIT_METERS, 0},
{GPS_ID, 5, ZSTR_SATELLITES, UNIT_RAW, 0},
{0, 0, "UNKNOWN", UNIT_RAW, 0},
};
const CrossfireSensor * getCrossfireSensor(uint8_t id, uint8_t subId=0)
const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
{
const CrossfireSensor * result = NULL;
for (const CrossfireSensor * sensor = crossfireSensors; sensor->id; sensor++) {
if (id == sensor->id && subId == sensor->subId) {
result = sensor;
break;
}
}
return result;
if (id == LINK_ID)
return crossfireSensors[RX_RSSI1_INDEX+subId];
else if (id == BATTERY_ID)
return crossfireSensors[BATT_VOLTAGE_INDEX+subId];
else if (id == GPS_ID)
return crossfireSensors[GPS_LATITUDE_INDEX+subId];
else
return crossfireSensors[UNKNOW_INDEX];
}
void processCrossfireTelemetryFrame(uint8_t id, uint8_t subId, uint32_t value)
void processCrossfireTelemetryValue(uint8_t index, uint32_t value)
{
const CrossfireSensor * sensor = getCrossfireSensor(id, subId);
TelemetryUnit unit = UNIT_RAW;
uint8_t precision = 0;
if (sensor) {
unit = sensor->unit;
precision = sensor->prec;
}
setTelemetryValue(TELEM_PROTO_CROSSFIRE, id, 0, subId, value, unit, precision);
const CrossfireSensor & sensor = crossfireSensors[index];
setTelemetryValue(TELEM_PROTO_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, sensor.precision);
}
bool checkCrossfireTelemetryFrameCRC()
@@ -73,6 +99,22 @@ bool checkCrossfireTelemetryFrameCRC()
return (crc == telemetryRxBuffer[len+1]);
}
template<int N>
bool getCrossfireTelemetryValue(uint8_t index, uint32_t & value)
{
bool result = false;
value = 0;
uint8_t * byte = &telemetryRxBuffer[index];
for (uint8_t i=0; i<N; i++) {
value <<= 8;
if (*byte != 0xff) {
result = true;
}
value += *byte++;
}
return result;
}
void processCrossfireTelemetryFrame()
{
if (!checkCrossfireTelemetryFrameCRC()) {
@@ -82,12 +124,36 @@ void processCrossfireTelemetryFrame()
}
uint8_t id = telemetryRxBuffer[2];
uint32_t value;
switch(id) {
case LINK_STATS_ID:
case GPS_ID:
if (getCrossfireTelemetryValue<4>(3, value))
processCrossfireTelemetryValue(GPS_LATITUDE_INDEX, value/10);
if (getCrossfireTelemetryValue<4>(7, value))
processCrossfireTelemetryValue(GPS_LONGITUDE_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(11, value))
processCrossfireTelemetryValue(GPS_GROUND_SPEED_INDEX, value);
if (getCrossfireTelemetryValue<2>(13, value))
processCrossfireTelemetryValue(GPS_HEADING_INDEX, value);
if (getCrossfireTelemetryValue<2>(15, value))
processCrossfireTelemetryValue(GPS_ALTITUDE_INDEX, value - 1000);
if (getCrossfireTelemetryValue<1>(17, value))
processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
break;
case LINK_ID:
for (int i=0; i<10; i++) {
processCrossfireTelemetryFrame(id, i, telemetryRxBuffer[3+i]);
if (getCrossfireTelemetryValue<1>(3+i, value))
processCrossfireTelemetryValue(i, value);
}
break;
case BATTERY_ID:
if (getCrossfireTelemetryValue<2>(3, value))
processCrossfireTelemetryValue(BATT_VOLTAGE_INDEX, value);
if (getCrossfireTelemetryValue<2>(5, value))
processCrossfireTelemetryValue(BATT_CURRENT_INDEX, value);
if (getCrossfireTelemetryValue<3>(7, value))
processCrossfireTelemetryValue(BATT_CAPACITY_INDEX, value);
break;
}
}
@@ -127,17 +193,14 @@ void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
telemetrySensor.id = id;
telemetrySensor.instance = subId;
const CrossfireSensor * sensor = getCrossfireSensor(id, subId);
if (sensor) {
TelemetryUnit unit = sensor->unit;
uint8_t prec = min<uint8_t>(2, sensor->prec);
telemetrySensor.init(sensor->name, unit, prec);
if (id == LINK_STATS_ID) {
telemetrySensor.logs = true;
}
}
else {
telemetrySensor.init(id);
const CrossfireSensor & sensor = getCrossfireSensor(id, subId);
TelemetryUnit unit = sensor.unit;
if (unit == UNIT_GPS_LATITUDE || unit == UNIT_GPS_LONGITUDE)
unit = UNIT_GPS;
uint8_t prec = min<uint8_t>(2, sensor.precision);
telemetrySensor.init(sensor.name, unit, prec);
if (id == LINK_ID) {
telemetrySensor.logs = true;
}
storageDirty(EE_MODEL);