diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 7141f334c..4d4ea00f8 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -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, diff --git a/radio/src/gui/taranis/lcd.cpp b/radio/src/gui/taranis/lcd.cpp index 26f64ef84..ba74fa448 100644 --- a/radio/src/gui/taranis/lcd.cpp +++ b/radio/src/gui/taranis/lcd.cpp @@ -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 '.' - 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 '.' + uint32_t absvalue = abs(value); + lcdDrawNumber(x, y, absvalue / 1000000, att); // ddd + lcdDrawChar(lcdLastPos, y, '@', att); + 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) { + absvalue %= 1000000; + absvalue *= 60; + absvalue /= 10000; + lcdDrawNumber(lcdLastPos+2, y, absvalue, att|LEFT|PREC2); lcdDrawSolidVerticalLine(lcdLastPos, y, 2); - 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); // '' - lcdDrawSolidVerticalLine(lcdLastPos, y, 2); - lcdDrawSolidVerticalLine(lcdLastPos+2, y, 2); - } - 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); + lcdDrawSolidVerticalLine(lcdLastPos+2, y, 2); + lcdLastPos += 3; } + } + else { + 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); } } diff --git a/radio/src/gui/taranis/menu_model_telemetry.cpp b/radio/src/gui/taranis/menu_model_telemetry.cpp index e2950ad3e..2837ff6cd 100644 --- a/radio/src/gui/taranis/menu_model_telemetry.cpp +++ b/radio/src/gui/taranis/menu_model_telemetry.cpp @@ -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) { diff --git a/radio/src/logs.cpp b/radio/src/logs.cpp index 9efce60f1..590f2905e 100644 --- a/radio/src/logs.cpp +++ b/radio/src/logs.cpp @@ -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, ","); } diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index a3478fa11..7d1a8b8a7 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -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) diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 041cace1d..4f3ba0353 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -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 +bool getCrossfireTelemetryValue(uint8_t index, uint32_t & value) +{ + bool result = false; + value = 0; + uint8_t * byte = &telemetryRxBuffer[index]; + for (uint8_t i=0; i(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(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(2, sensor.precision); + telemetrySensor.init(sensor.name, unit, prec); + if (id == LINK_ID) { + telemetrySensor.logs = true; } storageDirty(EE_MODEL); diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index 37afb7e0b..c2a93e627 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -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); diff --git a/radio/src/telemetry/frsky_d_arm.cpp b/radio/src/telemetry/frsky_d_arm.cpp index 1e476b7b0..da5b5071d 100644 --- a/radio/src/telemetry/frsky_d_arm.cpp +++ b/radio/src/telemetry/frsky_d_arm.cpp @@ -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; - } - else { - return; + lastId = id; + lastAPValue = data; } + 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) { - id = GPS_LAT_AP_ID; - unit = UNIT_GPS_LONGITUDE_EW; + if (lastId == GPS_LONG_BP_ID) { + id = GPS_LAT_AP_ID; + 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; } diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index a9cd0d17c..695f21b00 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -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); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index ec3ee5fcf..1013304ed 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -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); } diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp index 292bba9f3..74bef8907 100644 --- a/radio/src/telemetry/telemetry_sensors.cpp +++ b/radio/src/telemetry/telemetry_sensors.cpp @@ -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 angle2 = (lat*lat) / 10000; - uint32_t angle4 = angle2 * angle2; - distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); - } - lastReceived = now(); + uint32_t lat = abs(latitude) / 10000; + uint32_t angle2 = (lat*lat) / 10000; + uint32_t angle4 = angle2 * angle2; + 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; diff --git a/radio/src/telemetry/telemetry_sensors.h b/radio/src/telemetry/telemetry_sensors.h index 26747076b..4baac0475 100644 --- a/radio/src/telemetry/telemetry_sensors.h +++ b/radio/src/telemetry/telemetry_sensors.h @@ -30,13 +30,13 @@ class TelemetryItem }; union { - int32_t valueMin; // min store - uint32_t pilotLongitude; + int32_t valueMin; // min store + int32_t pilotLongitude; }; union { - int32_t valueMax; // max store - uint32_t pilotLatitude; + int32_t valueMax; // max store + 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; }; diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt index 142b296b9..9a7c0f954 100644 --- a/radio/src/translations/cz.h.txt +++ b/radio/src/translations/cz.h.txt @@ -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" diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt index ad63f61bb..562da4eac 100644 --- a/radio/src/translations/de.h.txt +++ b/radio/src/translations/de.h.txt @@ -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" diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index ca976047e..983e5e64a 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -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" diff --git a/radio/src/translations/es.h.txt b/radio/src/translations/es.h.txt index 4fb4eb1df..2c0365e5f 100644 --- a/radio/src/translations/es.h.txt +++ b/radio/src/translations/es.h.txt @@ -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" diff --git a/radio/src/translations/fi.h.txt b/radio/src/translations/fi.h.txt index f19995340..945170c03 100644 --- a/radio/src/translations/fi.h.txt +++ b/radio/src/translations/fi.h.txt @@ -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" diff --git a/radio/src/translations/fr.h.txt b/radio/src/translations/fr.h.txt index 502653276..d226e0d37 100644 --- a/radio/src/translations/fr.h.txt +++ b/radio/src/translations/fr.h.txt @@ -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" diff --git a/radio/src/translations/it.h.txt b/radio/src/translations/it.h.txt index 09c6dea86..6eac3b210 100644 --- a/radio/src/translations/it.h.txt +++ b/radio/src/translations/it.h.txt @@ -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" diff --git a/radio/src/translations/nl.h.txt b/radio/src/translations/nl.h.txt index 5a3b23a04..167aea6f8 100644 --- a/radio/src/translations/nl.h.txt +++ b/radio/src/translations/nl.h.txt @@ -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" diff --git a/radio/src/translations/pl.h.txt b/radio/src/translations/pl.h.txt index e83e1c519..f0596d511 100644 --- a/radio/src/translations/pl.h.txt +++ b/radio/src/translations/pl.h.txt @@ -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" diff --git a/radio/src/translations/pt.h.txt b/radio/src/translations/pt.h.txt index bebf620a7..b817ae62b 100644 --- a/radio/src/translations/pt.h.txt +++ b/radio/src/translations/pt.h.txt @@ -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" diff --git a/radio/src/translations/se.h.txt b/radio/src/translations/se.h.txt index 604669621..8926a004d 100644 --- a/radio/src/translations/se.h.txt +++ b/radio/src/translations/se.h.txt @@ -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"