[Crossfire] Telemetry new fields (GPS / Battery)

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
+27 -26
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 '.'
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);
}
}
+8 -11
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) {