Mirror/edgetx
1
0
Fork 0
mirror of https://github.com/EdgeTX/edgetx.git synced 2025-07-22 07:45:12 +03:00
Code 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

3
radio/src/dataconstants.h
View file

@ -367,10 +367,9 @@ enum TelemetryUnit {
UNIT_CELLS = UNIT_FIRST_VIRTUAL,
UNIT_DATETIME,
UNIT_GPS,
// Internal units (not stored in sensor unit)
UNIT_GPS_LONGITUDE,
UNIT_GPS_LATITUDE,
UNIT_GPS_LONGITUDE_EW,
UNIT_GPS_LATITUDE_NS,
UNIT_DATETIME_YEAR,
UNIT_DATETIME_DAY_MONTH,
UNIT_DATETIME_HOUR_MIN,

39
radio/src/gui/taranis/lcd.cpp
View file

@ -865,31 +865,32 @@ void putsValueWithUnit(coord_t x, coord_t y, int32_t val, uint8_t unit, LcdFlags
}
}
void displayGpsCoord(coord_t x, coord_t y, char direction, int16_t bp, int16_t ap, LcdFlags att, bool seconds=true)
void displayGpsCoord(coord_t x, coord_t y, int32_t value, const char * direction, LcdFlags att, bool seconds=true)
{
if (!direction) direction = '-';
lcdDrawNumber(x, y, bp / 100, att); // ddd before '.'
uint32_t absvalue = abs(value);
lcdDrawNumber(x, y, absvalue / 1000000, att); // ddd
lcdDrawChar(lcdLastPos, y, '@', att);
uint8_t mn = bp % 100; // TODO div_t
if (g_eeGeneral.gpsFormat == 0) {
lcdDrawNumber(lcdNextPos, y, mn, att|LEFT|LEADING0, 2); // mm before '.'
absvalue = absvalue % 1000000;
absvalue *= 60;
if (g_eeGeneral.gpsFormat == 0 || !seconds) {
lcdDrawNumber(lcdNextPos, y, absvalue / 1000000, att|LEFT|LEADING0, 2); // mm before '.'
lcdDrawSolidVerticalLine(lcdLastPos, y, 2);
lcdLastPos += 1;
if (seconds) {
uint16_t ss = ap * 6 / 10;
lcdDrawNumber(lcdLastPos+3, y, ss / 100, att|LEFT|LEADING0, 2); // ''
lcdDrawPoint(lcdLastPos, y+FH-2, 0); // small decimal point
lcdDrawNumber(lcdLastPos+2, y, ss % 100, att|LEFT|LEADING0, 2); // ''
absvalue %= 1000000;
absvalue *= 60;
absvalue /= 10000;
lcdDrawNumber(lcdLastPos+2, y, absvalue, att|LEFT|PREC2);
lcdDrawSolidVerticalLine(lcdLastPos, y, 2);
lcdDrawSolidVerticalLine(lcdLastPos+2, y, 2);
lcdLastPos += 3;
}
lcdDrawChar(lcdLastPos+2, y, direction);
}
else {
lcdDrawNumber(lcdLastPos+FW, y, mn, att|LEFT|LEADING0, 2); // mm before '.'
lcdDrawPoint(lcdLastPos, y+FH-2, 0); // small decimal point
lcdDrawNumber(lcdLastPos+2, y, ap, att|LEFT|UNSIGN|LEADING0, 4); // after '.'
lcdDrawChar(lcdLastPos+1, y, direction);
absvalue /= 10000;
lcdDrawNumber(lcdLastPos+FW, y, absvalue, att|LEFT|PREC2); // mm.mmm
}
lcdDrawSizedText(lcdLastPos+1, y, direction + (value>=0 ? 0 : 1), 1);
}
void displayDate(coord_t x, coord_t y, TelemetryItem & telemetryItem, LcdFlags att)
@ -928,12 +929,12 @@ void displayGpsCoords(coord_t x, coord_t y, TelemetryItem & telemetryItem, LcdFl
if (att & DBLSIZE) {
x -= (g_eeGeneral.gpsFormat == 0 ? 54 : 51);
att &= ~0x0F00; // TODO constant
displayGpsCoord(x, y, telemetryItem.gps.longitudeEW, telemetryItem.gps.longitude_bp, telemetryItem.gps.longitude_ap, att);
displayGpsCoord(x, y+FH, telemetryItem.gps.latitudeNS, telemetryItem.gps.latitude_bp, telemetryItem.gps.latitude_ap, att);
displayGpsCoord(x, y, telemetryItem.gps.longitude, "EW", att);
displayGpsCoord(x, y+FH, telemetryItem.gps.latitude, "NS", att);
}
else {
displayGpsCoord(x, y, telemetryItem.gps.longitudeEW, telemetryItem.gps.longitude_bp, telemetryItem.gps.longitude_ap, att, false);
displayGpsCoord(lcdNextPos+FWNUM, y, telemetryItem.gps.latitudeNS, telemetryItem.gps.latitude_bp, telemetryItem.gps.latitude_ap, att, false);
displayGpsCoord(x, y, telemetryItem.gps.longitude, "EW", att, false);
displayGpsCoord(lcdNextPos+FWNUM, y, telemetryItem.gps.latitude, "NS", att, false);
}
}

19
radio/src/gui/taranis/menu_model_telemetry.cpp
View file

@ -96,10 +96,7 @@ enum menuModelTelemetryItems {
#define TELEM_COL1 (1*FW)
#define TELEM_COL2 (16*FW)
#define TELEM_COL3 (28*FW)
#define TELEM_BARS_COLMIN TELEM_COL2
#define TELEM_BARS_COLMAX TELEM_COL3
#define TELEM_SCRTYPE_COL TELEM_COL2
#define TELEM_COL3 (28*FW+2)
#define IF_FAS_OFFSET(x) x,
#define IS_RANGE_DEFINED(k) (g_model.frsky.channels[k].ratio > 0)
@ -651,7 +648,7 @@ void menuModelTelemetry(uint8_t event)
uint8_t screenIndex = TELEMETRY_CURRENT_SCREEN(k);
drawStringWithIndex(0*FW, y, STR_SCREEN, screenIndex+1);
TelemetryScreenType oldScreenType = TELEMETRY_SCREEN_TYPE(screenIndex);
TelemetryScreenType newScreenType = (TelemetryScreenType)selectMenuItem(TELEM_SCRTYPE_COL, y, PSTR(""), STR_VTELEMSCREENTYPE, oldScreenType, 0, TELEMETRY_SCREEN_TYPE_MAX, (menuHorizontalPosition==0 ? attr : 0), event);
TelemetryScreenType newScreenType = (TelemetryScreenType)selectMenuItem(TELEM_COL2, y, PSTR(""), STR_VTELEMSCREENTYPE, oldScreenType, 0, TELEMETRY_SCREEN_TYPE_MAX, (menuHorizontalPosition==0 ? attr : 0), event);
if (newScreenType != oldScreenType) {
g_model.frsky.screensType = (g_model.frsky.screensType & (~(0x03 << (2*screenIndex)))) | (newScreenType << (2*screenIndex));
memset(&g_model.frsky.screens[screenIndex], 0, sizeof(g_model.frsky.screens[screenIndex]));
@ -663,9 +660,9 @@ void menuModelTelemetry(uint8_t event)
// TODO better function name for ---
// TODO function for these lines
if (ZEXIST(scriptData.file))
lcdDrawSizedText(TELEM_SCRTYPE_COL+7*FW, y, scriptData.file, sizeof(scriptData.file), (menuHorizontalPosition==1 ? attr : 0));
lcdDrawSizedText(TELEM_COL2+7*FW, y, scriptData.file, sizeof(scriptData.file), (menuHorizontalPosition==1 ? attr : 0));
else
lcdDrawTextAtIndex(TELEM_SCRTYPE_COL+7*FW, y, STR_VCSWFUNC, 0, (menuHorizontalPosition==1 ? attr : 0));
lcdDrawTextAtIndex(TELEM_COL2+7*FW, y, STR_VCSWFUNC, 0, (menuHorizontalPosition==1 ? attr : 0));
if (menuHorizontalPosition==1 && attr && event==EVT_KEY_BREAK(KEY_ENTER) && READ_ONLY_UNLOCKED()) {
s_editMode = 0;
@ -728,12 +725,12 @@ void menuModelTelemetry(uint8_t event)
int barMin = -barMax;
if (barSource) {
if (barSource <= MIXSRC_LAST_CH) {
putsChannelValue(TELEM_BARS_COLMIN, y, barSource, calc100toRESX(bar.barMin), (menuHorizontalPosition==1 ? attr : 0) | LEFT);
putsChannelValue(TELEM_BARS_COLMAX, y, barSource, calc100toRESX(bar.barMax), (menuHorizontalPosition==2 ? attr : 0) | LEFT);
putsChannelValue(TELEM_COL2, y, barSource, calc100toRESX(bar.barMin), (menuHorizontalPosition==1 ? attr : 0) | LEFT);
putsChannelValue(TELEM_COL3, y, barSource, calc100toRESX(bar.barMax), (menuHorizontalPosition==2 ? attr : 0) | LEFT);
}
else {
putsChannelValue(TELEM_BARS_COLMIN, y, barSource, bar.barMin, (menuHorizontalPosition==1 ? attr : 0) | LEFT);
putsChannelValue(TELEM_BARS_COLMAX, y, barSource, bar.barMax, (menuHorizontalPosition==2 ? attr : 0) | LEFT);
putsChannelValue(TELEM_COL2, y, barSource, bar.barMin, (menuHorizontalPosition==1 ? attr : 0) | LEFT);
putsChannelValue(TELEM_COL3, y, barSource, bar.barMax, (menuHorizontalPosition==2 ? attr : 0) | LEFT);
}
}
else if (attr) {

4
radio/src/logs.cpp
View file

@ -291,8 +291,8 @@ void writeLogs()
TelemetryItem & telemetryItem = telemetryItems[i];
if (sensor.logs) {
if (sensor.unit == UNIT_GPS) {
if (telemetryItem.gps.longitudeEW && telemetryItem.gps.latitudeNS)
f_printf(&g_oLogFile, "%03d.%04d%c %03d.%04d%c,", telemetryItem.gps.longitude_bp, telemetryItem.gps.longitude_ap, telemetryItem.gps.longitudeEW, telemetryItem.gps.latitude_bp, telemetryItem.gps.latitude_ap, telemetryItem.gps.latitudeNS);
if (telemetryItem.gps.longitude && telemetryItem.gps.latitude)
f_printf(&g_oLogFile, "%f %f,", float(telemetryItem.gps.longitude)/1000000, float(telemetryItem.gps.latitude)/1000000);
else
f_printf(&g_oLogFile, ",");
}

12
radio/src/lua/api_general.cpp
View file

@ -147,15 +147,11 @@ static int luaGetDateTime(lua_State *L)
static void luaPushLatLon(TelemetrySensor & telemetrySensor, TelemetryItem & telemetryItem)
/* result is lua table containing members ["lat"] and ["lon"] as lua_Number (doubles) in decimal degrees */
{
uint32_t gpsLat = 0;
uint32_t gpsLon = 0;
telemetryItem.gps.extractLatitudeLongitude(&gpsLat, &gpsLon); /* close, but not the format we want */
lua_createtable(L, 0, 4);
lua_pushtablenumber(L, "lat", gpsLat / ((telemetryItem.gps.latitudeNS == 'S') ? -1000000.0 : 1000000.0));
lua_pushtablenumber(L, "pilot-lat", telemetryItem.pilotLatitude / ((telemetryItem.gps.latitudeNS == 'S') ? -1000000.0 : 1000000.0));
lua_pushtablenumber(L, "lon", gpsLon / ((telemetryItem.gps.longitudeEW == 'W') ? -1000000.0 : 1000000.0));
lua_pushtablenumber(L, "pilot-lon", telemetryItem.pilotLongitude / ((telemetryItem.gps.longitudeEW == 'W') ? -1000000.0 : 1000000.0));
lua_pushtablenumber(L, "lat", telemetryItem.gps.latitude / 1000000.0);
lua_pushtablenumber(L, "pilot-lat", telemetryItem.pilotLatitude / 1000000.0);
lua_pushtablenumber(L, "lon", telemetryItem.gps.longitude / 1000000.0);
lua_pushtablenumber(L, "pilot-lon", telemetryItem.pilotLongitude / 1000000.0);
}
static void luaPushTelemetryDateTime(TelemetrySensor & telemetrySensor, TelemetryItem & telemetryItem)

147
radio/src/telemetry/crossfire.cpp
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,18 +193,15 @@ 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) {
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;
}
}
else {
telemetrySensor.init(id);
}
storageDirty(EE_MODEL);
}

6
radio/src/telemetry/crossfire.h
View file

@ -21,9 +21,13 @@
#ifndef _CROSSFIRE_H_
#define _CROSSFIRE_H_
// Device address
#define BROADCAST_ADDRESS 0x00
#define LINK_STATS_ID 0x14
// Frame id
#define GPS_ID 0x02
#define BATTERY_ID 0x08
#define LINK_ID 0x14
#define CHANNELS_ID 0x16
void processCrossfireTelemetryData(uint8_t data);

47
radio/src/telemetry/frsky_d_arm.cpp
View file

@ -131,10 +131,18 @@ const FrSkyDSensor * getFrSkyDSensor(uint8_t id)
return result;
}
uint8_t lastId = 0;
uint16_t lastBPValue = 0;
uint16_t lastAPValue = 0;
int32_t getFrSkyDProtocolGPSValue(int32_t sign)
{
div_t qr = div(lastBPValue, 100);
return sign * (((uint32_t) (qr.quot) * 1000000) + (((uint32_t) (qr.rem) * 10000 + lastAPValue) * 5) / 3);
}
void processHubPacket(uint8_t id, int16_t value)
{
static uint8_t lastId = 0;
static uint16_t lastValue = 0;
TelemetryUnit unit = UNIT_RAW;
uint8_t precision = 0;
int32_t data = value;
@ -145,36 +153,43 @@ void processHubPacket(uint8_t id, int16_t value)
if (id == GPS_LAT_BP_ID || id == GPS_LONG_BP_ID || id == BARO_ALT_BP_ID || id == VOLTS_BP_ID) {
lastId = id;
lastValue = value;
lastBPValue = value;
return;
}
if (id == GPS_LAT_AP_ID) {
if (lastId == GPS_LAT_BP_ID) {
data += lastValue << 16;
unit = UNIT_GPS_LATITUDE;
lastId = id;
lastAPValue = data;
}
else {
return;
}
}
else if (id == GPS_LONG_AP_ID) {
if (lastId == GPS_LONG_BP_ID) {
data += lastValue << 16;
lastId = id;
lastAPValue = data;
}
return;
}
else if (id == GPS_LAT_NS_ID) {
if (lastId == GPS_LONG_BP_ID) {
id = GPS_LAT_AP_ID;
unit = UNIT_GPS_LONGITUDE;
unit = UNIT_GPS_LATITUDE;
data = getFrSkyDProtocolGPSValue(value == 'N' ? 1 : -1);
}
else {
return;
}
}
else if (id == GPS_LAT_NS_ID) {
id = GPS_LAT_AP_ID;
unit = UNIT_GPS_LATITUDE_NS;
}
else if (id == GPS_LONG_EW_ID) {
if (lastId == GPS_LONG_BP_ID) {
id = GPS_LAT_AP_ID;
unit = UNIT_GPS_LONGITUDE_EW;
unit = UNIT_GPS_LONGITUDE;
data = getFrSkyDProtocolGPSValue(value == 'E' ? 1 : -1);
}
else {
return;
}
}
else if (id == BARO_ALT_AP_ID) {
if (lastId == BARO_ALT_BP_ID) {
@ -182,7 +197,7 @@ void processHubPacket(uint8_t id, int16_t value)
telemetryData.varioHighPrecision = true;
data /= 10; // map hi precision vario into low precision. Altitude is stored in 0.1m anyways
}
data = (int16_t)lastValue * 10 + (((int16_t)lastValue < 0) ? -data : data);
data = (int16_t)lastBPValue * 10 + (((int16_t)lastBPValue < 0) ? -data : data);
unit = UNIT_METERS;
precision = 1;
}
@ -192,7 +207,7 @@ void processHubPacket(uint8_t id, int16_t value)
}
else if (id == VOLTS_AP_ID) {
if (lastId == VOLTS_BP_ID) {
data = ((lastValue * 100 + value * 10) * 210) / 110;
data = ((lastBPValue * 100 + value * 10) * 210) / 110;
unit = UNIT_VOLTS;
precision = 2;
}

21
radio/src/telemetry/frsky_sport.cpp
View file

@ -65,7 +65,8 @@ const FrSkySportSensor sportSensors[] = {
{ CELLS_FIRST_ID, CELLS_LAST_ID, 0, ZSTR_CELLS, UNIT_CELLS, 2 },
{ GPS_ALT_FIRST_ID, GPS_ALT_LAST_ID, 0, ZSTR_GPSALT, UNIT_METERS, 2 },
{ GPS_TIME_DATE_FIRST_ID, GPS_TIME_DATE_LAST_ID, 0, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
{ GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 0, ZSTR_GPS, UNIT_GPS, 0 },
{ GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 0, ZSTR_GPS, UNIT_GPS_LATITUDE, 0 },
{ GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 1, ZSTR_GPS, UNIT_GPS_LONGITUDE, 0 },
{ FUEL_QTY_FIRST_ID, FUEL_QTY_LAST_ID, 0, ZSTR_FUEL, UNIT_MILLILITERS, 2 },
{ GPS_COURS_FIRST_ID, GPS_COURS_LAST_ID, 0, ZSTR_HDG, UNIT_DEGREE, 2 },
{ RBOX_BATT1_FIRST_ID, RBOX_BATT1_LAST_ID, 0, ZSTR_BATT1_VOLTAGE, UNIT_VOLTS, 3 },
@ -250,7 +251,17 @@ void processSportPacket(uint8_t * packet)
if (id == ADC1_ID || id == ADC2_ID || id == BATT_ID || id == SWR_ID) {
data = SPORT_DATA_U8(packet);
}
if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) {
if (id >= GPS_LONG_LATI_FIRST_ID && id <= GPS_LONG_LATI_LAST_ID) {
int32_t value = (data & 0x3fffffff);
if (data & (1 << 30))
value = -value;
value = (value * 5) / 3; // min/10000 => deg/1000000
if (data & (1 << 31))
processSportPacket(id, 0, instance, value); // latitude
else
processSportPacket(id, 1, instance, value); // longitude
}
else if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) {
processSportPacket(id, 0, instance, data & 0xffff);
processSportPacket(id, 1, instance, data >> 16);
}
@ -271,10 +282,7 @@ void processSportPacket(uint8_t * packet)
else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
#if defined(LUA)
if (luaInputTelemetryFifo) {
#if defined __GNUC__
// TODO remove this ifdef when we have updated to MSVC recent version
luaInputTelemetryFifo->push((LuaTelemetryValue){(uint8_t)id, data});
#endif
}
#endif
}
@ -321,6 +329,9 @@ void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instanc
telemetrySensor.unit = UNIT_FEET;
}
}
else if (unit == UNIT_GPS_LATITUDE || unit == UNIT_GPS_LONGITUDE) {
telemetrySensor.unit = UNIT_GPS;
}
}
else {
telemetrySensor.init(id);

4
radio/src/telemetry/telemetry.cpp
View file

@ -63,8 +63,10 @@ lcdint_t applyChannelRatio(source_t channel, lcdint_t val)
void processTelemetryData(uint8_t data)
{
#if defined(CROSSFIRE)
if (telemetryProtocol == PROTOCOL_PULSES_CROSSFIRE)
if (telemetryProtocol == PROTOCOL_PULSES_CROSSFIRE) {
processCrossfireTelemetryData(data);
return;
}
#endif
processFrskyTelemetryData(data);
}

78
radio/src/telemetry/telemetry_sensors.cpp
View file

@ -23,16 +23,13 @@
TelemetryItem telemetryItems[MAX_SENSORS];
uint8_t allowNewSensors;
void TelemetryItem::gpsReceived()
// TODO in maths
uint32_t getDistFromEarthAxis(int32_t latitude)
{
if (!distFromEarthAxis) {
gps.extractLatitudeLongitude(&pilotLatitude, &pilotLongitude);
uint32_t lat = pilotLatitude / 10000;
uint32_t lat = abs(latitude) / 10000;
uint32_t angle2 = (lat*lat) / 10000;
uint32_t angle4 = angle2 * angle2;
distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
}
lastReceived = now();
return 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
}
void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec)
@ -123,59 +120,21 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
}
newVal = 0;
}
else if (unit == UNIT_GPS) {
uint32_t gps_long_lati_data = uint32_t(newVal);
uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
switch ((gps_long_lati_data & 0xc0000000) >> 30) {
case 0:
gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.latitude_ap = gps_long_lati_a1w;
gps.latitudeNS = 'N';
break;
case 1:
gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.latitude_ap = gps_long_lati_a1w;
gps.latitudeNS = 'S';
break;
case 2:
gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.longitude_ap = gps_long_lati_a1w;
gps.longitudeEW = 'E';
break;
case 3:
gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.longitude_ap = gps_long_lati_a1w;
gps.longitudeEW = 'W';
break;
}
if (gps.longitudeEW && gps.latitudeNS) {
gpsReceived();
else if (unit == UNIT_GPS_LATITUDE) {
if (!pilotLatitude) {
pilotLatitude = newVal;
distFromEarthAxis = getDistFromEarthAxis(newVal);
}
gps.latitude = newVal;
lastReceived = now();
return;
}
else if (unit >= UNIT_GPS_LONGITUDE && unit <= UNIT_GPS_LATITUDE_NS) {
uint32_t data = uint32_t(newVal);
switch (unit) {
case UNIT_GPS_LONGITUDE:
gps.longitude_bp = data >> 16;
gps.longitude_ap = data & 0xFFFF;
break;
case UNIT_GPS_LATITUDE:
gps.latitude_bp = data >> 16;
gps.latitude_ap = data & 0xFFFF;
break;
case UNIT_GPS_LONGITUDE_EW:
gps.longitudeEW = data;
break;
case UNIT_GPS_LATITUDE_NS:
gps.latitudeNS = data;
break;
}
if (gps.longitudeEW && gps.latitudeNS && gps.longitude_ap && gps.latitude_ap) {
gpsReceived();
else if (unit == UNIT_GPS_LONGITUDE) {
if (!pilotLongitude) {
pilotLongitude = newVal;
}
gps.longitude = newVal;
lastReceived = now();
return;
}
else if (unit == UNIT_DATETIME_YEAR) {
@ -374,14 +333,11 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
return;
}
}
uint32_t latitude, longitude;
gpsItem.gps.extractLatitudeLongitude(&latitude, &longitude);
uint32_t angle = (latitude > gpsItem.pilotLatitude) ? latitude - gpsItem.pilotLatitude : gpsItem.pilotLatitude - latitude;
uint32_t angle = abs(gpsItem.gps.latitude - gpsItem.pilotLatitude);
uint32_t dist = EARTH_RADIUS * angle / 1000000;
uint32_t result = dist*dist;
angle = (longitude > gpsItem.pilotLongitude) ? longitude - gpsItem.pilotLongitude : gpsItem.pilotLongitude - longitude;
angle = abs(gpsItem.gps.longitude - gpsItem.pilotLongitude);
dist = gpsItem.distFromEarthAxis * angle / 1000000;
result += dist*dist;

19
radio/src/telemetry/telemetry_sensors.h
View file

@ -31,12 +31,12 @@ class TelemetryItem
union {
int32_t valueMin; // min store
uint32_t pilotLongitude;
int32_t pilotLongitude;
};
union {
int32_t valueMax; // max store
uint32_t pilotLatitude;
int32_t pilotLatitude;
};
uint8_t lastReceived; // for detection of sensor loss
@ -64,22 +64,11 @@ class TelemetryItem
uint8_t sec;
} datetime;
struct {
uint16_t longitude_bp;
uint16_t longitude_ap;
char longitudeEW;
uint16_t latitude_bp;
uint16_t latitude_ap;
char latitudeNS;
int32_t latitude;
int32_t longitude;
// pilot longitude is stored in min
// pilot latitude is stored in max
// distFromEarthAxis is stored in value
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{
div_t qr = div(latitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + latitude_ap) * 5) / 3;
qr = div(longitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + longitude_ap) * 5) / 3;
}
} gps;
};

1
radio/src/translations/cz.h.txt
View file

@ -1208,6 +1208,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/de.h.txt
View file

@ -1211,6 +1211,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

4
radio/src/translations/en.h.txt
View file

@ -735,7 +735,7 @@
#define TR_HAPTICSTRENGTH INDENT "Strength"
#define TR_CONTRAST "Contrast"
#define TR_ALARMS_LABEL "Alarms"
#define TR_BATTERY_RANGE TR("Battery range","Battery meter range")
#define TR_BATTERY_RANGE TR("Battery range", "Battery meter range")
#define TR_BATTERYWARNING INDENT "Battery Low"
#define TR_INACTIVITYALARM INDENT "Inactivity"
#define TR_MEMORYWARNING INDENT "Memory Low"
@ -1225,10 +1225,12 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"
#define ZSTR_HDG "Hdg"
#define ZSTR_SATELLITES "Sats"
#define ZSTR_CELLS "Cels"
#define ZSTR_GPSALT "GAlt"
#define ZSTR_GPSDATETIME "Date"

1
radio/src/translations/es.h.txt
View file

@ -1171,6 +1171,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/fi.h.txt
View file

@ -1171,6 +1171,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/fr.h.txt
View file

@ -1198,6 +1198,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/it.h.txt
View file

@ -1207,6 +1207,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/nl.h.txt
View file

@ -1222,6 +1222,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/pl.h.txt
View file

@ -1209,6 +1209,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/pt.h.txt
View file

@ -1167,6 +1167,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"

1
radio/src/translations/se.h.txt
View file

@ -1223,6 +1223,7 @@
#define ZSTR_ACCY "AccY"
#define ZSTR_ACCZ "AccZ"
#define ZSTR_CURR "Curr"
#define ZSTR_CAPACITY "Capa"
#define ZSTR_VFAS "VFAS"
#define ZSTR_ASPD "ASpd"
#define ZSTR_GSPD "GSpd"