diff --git a/radio/src/gui/view_main.cpp b/radio/src/gui/view_main.cpp index 5eb4575bd..c172f20dc 100644 --- a/radio/src/gui/view_main.cpp +++ b/radio/src/gui/view_main.cpp @@ -268,9 +268,9 @@ void displayTopBar() } /* Altitude */ - if (g_model.frsky.altitudeDisplayed && frskyData.hub.baroAltitudeOffset) { + if (g_model.frsky.altitudeDisplayed && TELEMETRY_BARO_ALT_AVAILABLE()) { LCD_ICON(altitude_icon_x, BAR_Y, ICON_ALTITUDE); - putsTelemetryValue(altitude_icon_x+2*FW-1, BAR_Y+1, TELEMETRY_ALT_BP, UNIT_METERS, LEFT); + putsTelemetryValue(altitude_icon_x+2*FW-1, BAR_Y+1, TELEMETRY_RELATIVE_BARO_ALT_BP, UNIT_METERS, LEFT); } } diff --git a/radio/src/logs.cpp b/radio/src/logs.cpp index 41cf29b8d..f3d4aa618 100644 --- a/radio/src/logs.cpp +++ b/radio/src/logs.cpp @@ -149,7 +149,7 @@ const pm_char * openLogs() #if defined(PCBTARANIS) f_puts("Rud,Ele,Thr,Ail,S1,S2,LS,RS,SA,SB,SC,SD,SE,SF,SG,SH\n", &g_oLogFile); #else - f_puts("Rud,Ele,Thr,Ail,P1,P2,P3,THR,RUD,ELE,ID0,ID1,ID2,AIL,GEA,TRN\n", &g_oLogFile); + f_puts("Rud,Ele,Thr,Ail,P1,P2,P3,THR,RUD,ELE,3POS,AIL,GEA,TRN\n", &g_oLogFile); #endif } else { @@ -214,7 +214,7 @@ void writeLogs() #if defined(FRSKY_HUB) if (IS_USR_PROTO_FRSKY_HUB()) { - f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d,%d.%02d,%d.%02d," TELEMETRY_ALT_FORMAT TELEMETRY_VSPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,", + f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d," TELEMETRY_GPS_SPEED_FORMAT TELEMETRY_GPS_ALT_FORMAT TELEMETRY_BARO_ALT_FORMAT TELEMETRY_VSPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,", frskyData.hub.year+2000, frskyData.hub.month, frskyData.hub.day, @@ -229,20 +229,18 @@ void writeLogs() frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-', frskyData.hub.gpsCourse_bp, frskyData.hub.gpsCourse_ap, - TELEMETRY_GPS_SPEED_BP, - TELEMETRY_GPS_SPEED_AP, - TELEMETRY_GPS_ALT_BP, - TELEMETRY_GPS_ALT_AP, - TELEMETRY_ALT, - TELEMETRY_VSPEED, + TELEMETRY_GPS_SPEED_ARGS + TELEMETRY_GPS_ALT_ARGS + TELEMETRY_BARO_ALT_ARGS + TELEMETRY_VSPEED_ARGS frskyData.hub.temperature1, frskyData.hub.temperature2, frskyData.hub.rpm, frskyData.hub.fuelLevel, - TELEMETRY_CELLS, - TELEMETRY_CURRENT, + TELEMETRY_CELLS_ARGS + TELEMETRY_CURRENT_ARGS frskyData.hub.currentConsumption, - TELEMETRY_VFAS, + TELEMETRY_VFAS_ARGS frskyData.hub.accelX, frskyData.hub.accelY, frskyData.hub.accelZ); @@ -251,7 +249,7 @@ void writeLogs() #if defined(WS_HOW_HIGH) if (IS_USR_PROTO_WS_HOW_HIGH()) { - f_printf(&g_oLogFile, "%d,", TELEMETRY_ALT_BP); + f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP); } #endif diff --git a/radio/src/maths.cpp b/radio/src/maths.cpp index 271aac8f3..cdd135966 100644 --- a/radio/src/maths.cpp +++ b/radio/src/maths.cpp @@ -48,18 +48,6 @@ lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val) } #endif -#if defined(PCBTARANIS) -double gpsToDouble(bool neg, int16_t bp, int16_t ap) -{ - double result = ap; - result /= 10000; - result += (bp % 100); - result /= 60; - result += (bp / 100); - return neg?-result:result; -} -#endif - #if defined(FRSKY_HUB) void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) { @@ -70,17 +58,8 @@ void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3; } -#if defined(PCBTARANIS) -double pilotLatitude; -double pilotLongitude; -#endif - void getGpsPilotPosition() { -#if defined(PCBTARANIS) - pilotLatitude = gpsToDouble(frskyData.hub.gpsLatitudeNS=='S', frskyData.hub.gpsLatitude_bp, frskyData.hub.gpsLatitude_ap); - pilotLongitude = gpsToDouble(frskyData.hub.gpsLongitudeEW=='W', frskyData.hub.gpsLongitude_bp, frskyData.hub.gpsLongitude_ap); -#endif extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude); uint32_t lat = frskyData.hub.pilotLatitude / 10000; uint32_t angle2 = (lat*lat) / 10000; @@ -105,7 +84,7 @@ void getGpsDistance() dist = frskyData.hub.distFromEarthAxis * angle / 1000000; result += dist*dist; - dist = abs(frskyData.hub.baroAltitudeOffset ? TELEMETRY_ALT_BP : TELEMETRY_GPS_ALT_BP); + dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP); result += dist*dist; frskyData.hub.gpsDistance = isqrt32(result); diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 36d95d213..c5808702b 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1275,7 +1275,7 @@ getvalue_t getValue(uint8_t i) #if defined(FRSKY_SPORT) else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return frskyData.hub.baroAltitude; #elif defined(FRSKY_HUB) || defined(WS_HOW_HIGH) - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return TELEMETRY_ALT_BP; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return TELEMETRY_RELATIVE_BARO_ALT_BP; #endif #if defined(FRSKY_HUB) else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return frskyData.hub.rpm; @@ -1284,7 +1284,7 @@ getvalue_t getValue(uint8_t i) else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T2) return frskyData.hub.temperature2; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_SPEED) return TELEMETRY_GPS_SPEED_BP; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DIST) return frskyData.hub.gpsDistance; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_GPSALT) return TELEMETRY_GPS_ALT_BP; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_GPSALT) return TELEMETRY_RELATIVE_GPS_ALT_BP; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELL) return (int16_t)TELEMETRY_MIN_CELL_VOLTAGE; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELLS_SUM) return (int16_t)frskyData.hub.cellsSum; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)frskyData.hub.vfas; diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp index 8e2a4bda2..1ef552dbf 100644 --- a/radio/src/telemetry/frsky.cpp +++ b/radio/src/telemetry/frsky.cpp @@ -71,10 +71,10 @@ FrskyData frskyData; #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) void checkMinMaxAltitude() { - if (TELEMETRY_ALT_BP > frskyData.hub.maxAltitude) - frskyData.hub.maxAltitude = TELEMETRY_ALT_BP; - if (TELEMETRY_ALT_BP < frskyData.hub.minAltitude) - frskyData.hub.minAltitude = TELEMETRY_ALT_BP; + if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude) + frskyData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP; + if (TELEMETRY_RELATIVE_BARO_ALT_BP < frskyData.hub.minAltitude) + frskyData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP; } #endif diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index 5acce01ac..56aafe12f 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -221,31 +221,38 @@ bool FRSKY_alarmRaised(uint8_t idx); void resetTelemetry(); +#define TELEMETRY_STREAMING() (frskyStreaming > 0) + #define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * 2) #define TELEMETRY_MIN_CELL_VOLTAGE (frskyData.hub.minCellVolts * 2) -#define TELEMETRY_ALT_BP frskyData.hub.baroAltitude_bp -#define TELEMETRY_ALT_AP frskyData.hub.baroAltitude_ap -#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp -#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap -#define TELEMETRY_GPS_ALT_AP frskyData.hub.gpsAltitude_ap -#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp -#define TELEMETRY_ALT frskyData.hub.baroAltitude_bp, frskyData.hub.baroAltitude_ap -#define TELEMETRY_ALT_FORMAT "%d.%02d," -#define TELEMETRY_CELLS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100 -#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," -#define TELEMETRY_CURRENT frskyData.hub.current / 100, frskyData.hub.current % 100 -#define TELEMETRY_CURRENT_FORMAT "%d.%02d," -#define TELEMETRY_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10 -#define TELEMETRY_VFAS_FORMAT "%d.%d," -#define TELEMETRY_VSPEED frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100 -#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," -#define TELEMETRY_STREAMING() (frskyStreaming > 0) +#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset) + +#define TELEMETRY_RELATIVE_BARO_ALT_BP frskyData.hub.baroAltitude_bp +#define TELEMETRY_RELATIVE_BARO_ALT_AP frskyData.hub.baroAltitude_ap +#define TELEMETRY_RELATIVE_GPS_ALT_BP frskyData.hub.gpsAltitude_bp +#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp +#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap + +#define TELEMETRY_BARO_ALT_FORMAT "%d," +#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude_bp, +#define TELEMETRY_GPS_ALT_FORMAT "%d," +#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude_bp, +#define TELEMETRY_GPS_SPEED_FORMAT "%d.%02d," +#define TELEMETRY_GPS_SPEED_ARGS TELEMETRY_GPS_SPEED_BP, TELEMETRY_GPS_SPEED_AP, +#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," +#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100, +#define TELEMETRY_CURRENT_FORMAT "%d.%02d," +#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 100, frskyData.hub.current % 100, +#define TELEMETRY_VFAS_FORMAT "%d.%d," +#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10, +#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," +#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100, #if defined(FRSKY_HUB) - #define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor) + #define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor) #else - #define TELEMETRY_OPENXSENSOR() (0) + #define TELEMETRY_OPENXSENSOR() (0) #endif #endif diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index ac46c7252..42aafb3ce 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -187,8 +187,7 @@ void processHubPacket(uint8_t id, uint16_t value) if (frskyData.hub.rpm > frskyData.hub.maxRpm) frskyData.hub.maxRpm = frskyData.hub.rpm; break; -//case GPS_COURS_BP_ID: - //frskyData.hub.gpsCourse_bp = value; + case TEMP1_ID: if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1) frskyData.hub.maxTemperature1 = frskyData.hub.temperature1; @@ -200,7 +199,7 @@ void processHubPacket(uint8_t id, uint16_t value) break; case CURRENT_ID: - if(((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0) + if (((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0) frskyData.hub.current += g_model.frsky.fasOffset; else frskyData.hub.current = 0; @@ -227,17 +226,17 @@ void processHubPacket(uint8_t id, uint16_t value) break; case GPS_ALT_AP_ID: - frskyData.hub.gpsAltitude_bp = frskyData.hub.gpsAltitude_bp*100; + { + frskyData.hub.gpsAltitude = (frskyData.hub.gpsAltitude_bp * 100) + frskyData.hub.gpsAltitude_ap; if (!frskyData.hub.gpsAltitudeOffset) - frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp; - frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset; + frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude; if (!frskyData.hub.baroAltitudeOffset) { - if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude*100) - frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp/100; - if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude*100) - frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp/100; + int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP; + if (altitude > frskyData.hub.maxAltitude) + frskyData.hub.maxAltitude = altitude; + if (altitude < frskyData.hub.minAltitude) + frskyData.hub.minAltitude = altitude; } - if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { // First received GPS position => Pilot GPS position getGpsPilotPosition(); @@ -246,6 +245,7 @@ void processHubPacket(uint8_t id, uint16_t value) getGpsDistance(); } break; + } case GPS_SPEED_BP_ID: // Speed => Max speed @@ -310,10 +310,10 @@ void processSportPacket(uint8_t *packet) /* uint8_t dataId = packet[0]; */ uint8_t prim = packet[1]; uint16_t appId = *((uint16_t *)(packet+2)); - static uint8_t t_coor; - if (!checkSportPacket(packet)) + if (!checkSportPacket(packet)) { return; + } switch (prim) { @@ -387,7 +387,7 @@ void processSportPacket(uint8_t *packet) } else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) { frskyData.hub.current = SPORT_DATA_U32(packet); - if(((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0) + if (((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0) frskyData.hub.current += g_model.frsky.fasOffset; else frskyData.hub.current = 0; @@ -399,91 +399,87 @@ void processSportPacket(uint8_t *packet) } else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) { frskyData.hub.gpsSpeed_bp = SPORT_DATA_U32(packet); - frskyData.hub.gpsSpeed_bp = (frskyData.hub.gpsSpeed_bp * 46) / 25/1000; + frskyData.hub.gpsSpeed_bp = (frskyData.hub.gpsSpeed_bp * 46) / 25 / 1000; if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed) frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp; } - else if(appId>=GPS_TIME_DATE_FIRST_ID && appId<=GPS_TIME_DATE_LAST_ID) { - uint32_t gps_time_date = SPORT_DATA_U32(packet); - if(gps_time_date & 0x000000ff) { - frskyData.hub.year = (uint16_t)((gps_time_date & 0xff000000)>>24); - frskyData.hub.month =(uint8_t)((gps_time_date & 0x00ff0000)>>16); - frskyData.hub.day = (uint8_t)((gps_time_date & 0x0000ff00)>>8); - } - else { - frskyData.hub.hour =(uint8_t) ((gps_time_date & 0xff000000)>>24); - frskyData.hub.min = (uint8_t)((gps_time_date & 0x00ff0000)>>16); - frskyData.hub.sec =(uint16_t) ((gps_time_date & 0x0000ff00)>>8); - frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24; - } - } - else if(appId>=GPS_COURS_FIRST_ID && appId<=GPS_COURS_LAST_ID) { - uint32_t course = SPORT_DATA_U32(packet); - frskyData.hub.gpsCourse_bp = course/100; - frskyData.hub.gpsCourse_ap = course%100; - } - else if (appId>=GPS_ALT_FIRST_ID && appId<=GPS_ALT_LAST_ID) { - int32_t gps_altitude; - gps_altitude = SPORT_DATA_S32(packet); - frskyData.hub.gpsAltitude_bp = gps_altitude; - - if (!frskyData.hub.gpsAltitudeOffset) - frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp; - - frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset; + else if (appId >= GPS_TIME_DATE_FIRST_ID && appId <= GPS_TIME_DATE_LAST_ID) { + uint32_t gps_time_date = SPORT_DATA_U32(packet); + if (gps_time_date & 0x000000ff) { + frskyData.hub.year = (uint16_t) ((gps_time_date & 0xff000000) >> 24); + frskyData.hub.month = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16); + frskyData.hub.day = (uint8_t) ((gps_time_date & 0x0000ff00) >> 8); + } + else { + frskyData.hub.hour = (uint8_t) ((gps_time_date & 0xff000000) >> 24); + frskyData.hub.min = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16); + frskyData.hub.sec = (uint16_t) ((gps_time_date & 0x0000ff00) >> 8); + frskyData.hub.hour = ((uint8_t) (frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24; + } + } + else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) { + uint32_t course = SPORT_DATA_U32(packet); + frskyData.hub.gpsCourse_bp = course / 100; + frskyData.hub.gpsCourse_ap = course % 100; + } + else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) { + frskyData.hub.gpsAltitude = SPORT_DATA_S32(packet); + + if (!frskyData.hub.gpsAltitudeOffset) + frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude; - if (!frskyData.hub.baroAltitudeOffset) { - if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude*100) - frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp/100; - - if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude*100) - frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp/100; - } - - if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { - // First received GPS position => Pilot GPS position - if(t_coor) - getGpsPilotPosition(); - } - else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) { - getGpsDistance(); - } - } - else if(appId>=GPS_LONG_LATI_FIRST_ID && appId<=GPS_LONG_LATI_LAST_ID) { - uint32_t gps_long_lati_data = SPORT_DATA_U32(packet); - if (gps_long_lati_data) - { - frskyData.hub.gpsFix = 1; - t_coor = 1; - } - - 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: { - frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w/60*100)+(gps_long_lati_b1w%60); - frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; - frskyData.hub.gpsLatitudeNS = 'N'; - }break; - case 1: { - frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w/60*100) + (gps_long_lati_b1w%60); - frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; - frskyData.hub.gpsLatitudeNS = 'S'; }break; - case 2: { - frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w/60*100) +(gps_long_lati_b1w%60); - frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; - frskyData.hub.gpsLongitudeEW = 'E'; }break; - case 3: { - frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w/60*100) +(gps_long_lati_b1w%60); - frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; - frskyData.hub.gpsLongitudeEW = 'W';}break; - } - if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1) - frskyData.hub.gpsFix = 0; - if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1) - frskyData.hub.gpsFix = 0; - } + if (!frskyData.hub.baroAltitudeOffset) { + int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP; + if (altitude > frskyData.hub.maxAltitude) + frskyData.hub.maxAltitude = altitude; + if (altitude < frskyData.hub.minAltitude) + frskyData.hub.minAltitude = altitude; + } + + if (frskyData.hub.gpsFix > 0) { + if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { + // First received GPS position => Pilot GPS position + getGpsPilotPosition(); + } + else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) { + getGpsDistance(); + } + } + } + else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) { + uint32_t gps_long_lati_data = SPORT_DATA_U32(packet); + 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: + frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; + frskyData.hub.gpsLatitudeNS = 'N'; + break; + case 1: + frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; + frskyData.hub.gpsLatitudeNS = 'S'; + break; + case 2: + frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; + frskyData.hub.gpsLongitudeEW = 'E'; + break; + case 3: + frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; + frskyData.hub.gpsLongitudeEW = 'W'; + break; + } + if (frskyData.hub.gpsLongitudeEW && frskyData.hub.gpsLatitudeNS) { + frskyData.hub.gpsFix = 1; + } + else if (frskyData.hub.gpsFix > 0) { + frskyData.hub.gpsFix = 0; + } + } else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) { uint32_t cells = SPORT_DATA_U32(packet); diff --git a/radio/src/telemetry/frsky_sport.h b/radio/src/telemetry/frsky_sport.h index 4b39022a1..2bcc29bb7 100644 --- a/radio/src/telemetry/frsky_sport.h +++ b/radio/src/telemetry/frsky_sport.h @@ -129,6 +129,7 @@ PACK(struct FrskySerialData { uint16_t maxPower; /* end */ + int32_t gpsAltitude; uint8_t gpsDistNeeded; int8_t gpsFix; // -1=never fixed, 0=fix lost, 1=fixed @@ -177,41 +178,37 @@ bool FRSKY_alarmRaised(uint8_t idx); void resetTelemetry(); -#define TELEMETRY_CELL_VOLTAGE(k) frskyData.hub.cellVolts[k] -#define TELEMETRY_MIN_CELL_VOLTAGE frskyData.hub.minCellVolts -#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp -#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap -#define TELEMETRY_GPS_SPEED_LOG frskyData.hub.gpsSpeed_bp<0?'-':' ',abs(frskyData.hub.gpsSpeed_bp/1000),abs(frskyData.hub.gpsSpeed_bp%1000) -#define TELEMETRY_GPS_ALT_AP (frskyData.hub.gpsAltitude_bp%100) -#define TELEMETRY_GPS_ALT_BP (frskyData.hub.gpsAltitude_bp/100) -#define TELEMETRY_GPS_ALT_LOG frskyData.hub.gpsAltitude_bp < 0 ? '-':' ',abs(frskyData.hub.gpsAltitude_bp/100),abs(frskyData.hub.gpsAltitude_bp%100) -#define TELEMETRY_ALT_BP (frskyData.hub.baroAltitude / 100) -#define TELEMETRY_ALT_AP (frskyData.hub.baroAltitude % 100) -#define TELEMETRY_ALT frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(frskyData.hub.baroAltitude / 100), abs(frskyData.hub.baroAltitude % 100) -#define TELEMETRY_ALT_FORMAT "%c%d.%02d," -#define TELEMETRY_CELLS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100 -#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," -#define TELEMETRY_CURRENT frskyData.hub.current / 10, frskyData.hub.current % 10 -#define TELEMETRY_CURRENT_FORMAT "%d.%d," -#define TELEMETRY_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10 -#define TELEMETRY_VFAS_FORMAT "%d.%d," -#define TELEMETRY_VSPEED frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100) -#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," +#define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0) -/* Would be great, but f_printf() doesn't take floats... -#define TELEMETRY_ALT float(frskyData.hub.baroAltitude)/100 -#define TELEMETRY_ALT_FORMAT "%.2f," -#define TELEMETRY_CELLS float(frskyData.hub.cellsSum)/10, float(frskyData.hub.cellVolts[0])/100, float(frskyData.hub.cellVolts[1])/100, float(frskyData.hub.cellVolts[2])/100, float(frskyData.hub.cellVolts[3])/100, float(frskyData.hub.cellVolts[4])/100, float(frskyData.hub.cellVolts[5])/100 -#define TELEMETRY_CELLS_FORMAT "%.1f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f," -#define TELEMETRY_CURRENT float(frskyData.hub.current)/100 -#define TELEMETRY_CURRENT_FORMAT "%.2f," -#define TELEMETRY_VFAS float(frskyData.hub.vfas)/10 -#define TELEMETRY_VFAS_FORMAT "%.1f," -#define TELEMETRY_VSPEED float(frskyData.hub.varioSpeed)/100 -#define TELEMETRY_VSPEED_FORMAT "%.2f," -*/ +#define TELEMETRY_CELL_VOLTAGE(k) frskyData.hub.cellVolts[k] +#define TELEMETRY_MIN_CELL_VOLTAGE frskyData.hub.minCellVolts -#define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0) +#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset) + +#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp +#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap + +#define TELEMETRY_ABSOLUTE_GPS_ALT (frskyData.hub.gpsAltitude) +#define TELEMETRY_RELATIVE_GPS_ALT (frskyData.hub.gpsAltitude + frskyData.hub.gpsAltitudeOffset) +#define TELEMETRY_RELATIVE_GPS_ALT_BP (TELEMETRY_RELATIVE_GPS_ALT / 100) + +#define TELEMETRY_RELATIVE_BARO_ALT_BP (frskyData.hub.baroAltitude / 100) +#define TELEMETRY_RELATIVE_BARO_ALT_AP (frskyData.hub.baroAltitude % 100) + +#define TELEMETRY_BARO_ALT_FORMAT "%c%d.%02d," +#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(frskyData.hub.baroAltitude / 100), abs(frskyData.hub.baroAltitude % 100), +#define TELEMETRY_GPS_ALT_FORMAT "%c%d.%02d," +#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude < 0 ? '-' : ' ', abs(frskyData.hub.gpsAltitude / 100), abs(frskyData.hub.gpsAltitude % 100), +#define TELEMETRY_GPS_SPEED_FORMAT "%c%d.%02d," +#define TELEMETRY_GPS_SPEED_ARGS frskyData.hub.gpsSpeed_bp < 0 ? '-' : ' ', abs(frskyData.hub.gpsSpeed_bp / 1000), abs(frskyData.hub.gpsSpeed_bp % 1000), +#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," +#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, +#define TELEMETRY_CURRENT_FORMAT "%d.%d," +#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 10, frskyData.hub.current % 10, +#define TELEMETRY_VFAS_FORMAT "%d.%d," +#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10, +#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," +#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100), #endif