Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-25 17:25:13 +03:00
Code Issues Wiki Activity

Port of GPS modifications from SVN

Browse source
This commit is contained in:
bsongis 2014-02-25 16:57:01 +01:00
parent c98e718e16
commit 590a58a6e9
8 changed files with 166 additions and 189 deletions
Download patch file Download diff file Expand all files Collapse all files

4
radio/src/gui/view_main.cpp
View file

@ -268,9 +268,9 @@ void displayTopBar()
} }
/* Altitude */ /* 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); 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);
} }
} }

22
radio/src/logs.cpp
View file

@ -149,7 +149,7 @@ const pm_char * openLogs()
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
f_puts("Rud,Ele,Thr,Ail,S1,S2,LS,RS,SA,SB,SC,SD,SE,SF,SG,SH\n", &g_oLogFile); f_puts("Rud,Ele,Thr,Ail,S1,S2,LS,RS,SA,SB,SC,SD,SE,SF,SG,SH\n", &g_oLogFile);
#else #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 #endif
} }
else { else {
@ -214,7 +214,7 @@ void writeLogs()
#if defined(FRSKY_HUB) #if defined(FRSKY_HUB)
if (IS_USR_PROTO_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.year+2000,
frskyData.hub.month, frskyData.hub.month,
frskyData.hub.day, frskyData.hub.day,
@ -229,20 +229,18 @@ void writeLogs()
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-', frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
frskyData.hub.gpsCourse_bp, frskyData.hub.gpsCourse_bp,
frskyData.hub.gpsCourse_ap, frskyData.hub.gpsCourse_ap,
TELEMETRY_GPS_SPEED_BP, TELEMETRY_GPS_SPEED_ARGS
TELEMETRY_GPS_SPEED_AP, TELEMETRY_GPS_ALT_ARGS
TELEMETRY_GPS_ALT_BP, TELEMETRY_BARO_ALT_ARGS
TELEMETRY_GPS_ALT_AP, TELEMETRY_VSPEED_ARGS
TELEMETRY_ALT,
TELEMETRY_VSPEED,
frskyData.hub.temperature1, frskyData.hub.temperature1,
frskyData.hub.temperature2, frskyData.hub.temperature2,
frskyData.hub.rpm, frskyData.hub.rpm,
frskyData.hub.fuelLevel, frskyData.hub.fuelLevel,
TELEMETRY_CELLS, TELEMETRY_CELLS_ARGS
TELEMETRY_CURRENT, TELEMETRY_CURRENT_ARGS
frskyData.hub.currentConsumption, frskyData.hub.currentConsumption,
TELEMETRY_VFAS, TELEMETRY_VFAS_ARGS
frskyData.hub.accelX, frskyData.hub.accelX,
frskyData.hub.accelY, frskyData.hub.accelY,
frskyData.hub.accelZ); frskyData.hub.accelZ);
@ -251,7 +249,7 @@ void writeLogs()
#if defined(WS_HOW_HIGH) #if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_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 #endif

23
radio/src/maths.cpp
View file

@ -48,18 +48,6 @@ lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val)
} }
#endif #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) #if defined(FRSKY_HUB)
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) 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; *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() 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); extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
uint32_t lat = frskyData.hub.pilotLatitude / 10000; uint32_t lat = frskyData.hub.pilotLatitude / 10000;
uint32_t angle2 = (lat*lat) / 10000; uint32_t angle2 = (lat*lat) / 10000;
@ -105,7 +84,7 @@ void getGpsDistance()
dist = frskyData.hub.distFromEarthAxis * angle / 1000000; dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
result += dist*dist; 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; result += dist*dist;
frskyData.hub.gpsDistance = isqrt32(result); frskyData.hub.gpsDistance = isqrt32(result);

4
radio/src/opentx.cpp
View file

@ -1275,7 +1275,7 @@ getvalue_t getValue(uint8_t i)
#if defined(FRSKY_SPORT) #if defined(FRSKY_SPORT)
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return frskyData.hub.baroAltitude; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return frskyData.hub.baroAltitude;
#elif defined(FRSKY_HUB) || defined(WS_HOW_HIGH) #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 #endif
#if defined(FRSKY_HUB) #if defined(FRSKY_HUB)
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return frskyData.hub.rpm; 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_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_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_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_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_CELLS_SUM) return (int16_t)frskyData.hub.cellsSum;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)frskyData.hub.vfas; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)frskyData.hub.vfas;

8
radio/src/telemetry/frsky.cpp
View file

@ -71,10 +71,10 @@ FrskyData frskyData;
#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
void checkMinMaxAltitude() void checkMinMaxAltitude()
{ {
if (TELEMETRY_ALT_BP > frskyData.hub.maxAltitude) if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = TELEMETRY_ALT_BP; frskyData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
if (TELEMETRY_ALT_BP < frskyData.hub.minAltitude) if (TELEMETRY_RELATIVE_BARO_ALT_BP < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = TELEMETRY_ALT_BP; frskyData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
} }
#endif #endif

45
radio/src/telemetry/frsky.h
View file

@ -221,31 +221,38 @@ bool FRSKY_alarmRaised(uint8_t idx);
void resetTelemetry(); void resetTelemetry();
#define TELEMETRY_STREAMING() (frskyStreaming > 0)
#define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * 2) #define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * 2)
#define TELEMETRY_MIN_CELL_VOLTAGE (frskyData.hub.minCellVolts * 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) #if defined(FRSKY_HUB)
#define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor) #define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor)
#else #else
#define TELEMETRY_OPENXSENSOR() (0) #define TELEMETRY_OPENXSENSOR() (0)
#endif #endif
#endif #endif

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

@ -187,8 +187,7 @@ void processHubPacket(uint8_t id, uint16_t value)
if (frskyData.hub.rpm > frskyData.hub.maxRpm) if (frskyData.hub.rpm > frskyData.hub.maxRpm)
frskyData.hub.maxRpm = frskyData.hub.rpm; frskyData.hub.maxRpm = frskyData.hub.rpm;
break; break;
//case GPS_COURS_BP_ID:
//frskyData.hub.gpsCourse_bp = value;
case TEMP1_ID: case TEMP1_ID:
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1) if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1; frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
@ -200,7 +199,7 @@ void processHubPacket(uint8_t id, uint16_t value)
break; break;
case CURRENT_ID: 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; frskyData.hub.current += g_model.frsky.fasOffset;
else else
frskyData.hub.current = 0; frskyData.hub.current = 0;
@ -227,17 +226,17 @@ void processHubPacket(uint8_t id, uint16_t value)
break; break;
case GPS_ALT_AP_ID: 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) if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp; frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;
frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset;
if (!frskyData.hub.baroAltitudeOffset) { if (!frskyData.hub.baroAltitudeOffset) {
if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude*100) int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP;
frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp/100; if (altitude > frskyData.hub.maxAltitude)
if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude*100) frskyData.hub.maxAltitude = altitude;
frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp/100; if (altitude < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = altitude;
} }
if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
// First received GPS position => Pilot GPS position // First received GPS position => Pilot GPS position
getGpsPilotPosition(); getGpsPilotPosition();
@ -246,6 +245,7 @@ void processHubPacket(uint8_t id, uint16_t value)
getGpsDistance(); getGpsDistance();
} }
break; break;
}
case GPS_SPEED_BP_ID: case GPS_SPEED_BP_ID:
// Speed => Max speed // Speed => Max speed
@ -310,10 +310,10 @@ void processSportPacket(uint8_t *packet)
/* uint8_t dataId = packet[0]; */ /* uint8_t dataId = packet[0]; */
uint8_t prim = packet[1]; uint8_t prim = packet[1];
uint16_t appId = *((uint16_t *)(packet+2)); uint16_t appId = *((uint16_t *)(packet+2));
static uint8_t t_coor;
if (!checkSportPacket(packet)) if (!checkSportPacket(packet)) {
return; return;
}
switch (prim) switch (prim)
{ {
@ -387,7 +387,7 @@ void processSportPacket(uint8_t *packet)
} }
else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) { else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
frskyData.hub.current = SPORT_DATA_U32(packet); 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; frskyData.hub.current += g_model.frsky.fasOffset;
else else
frskyData.hub.current = 0; 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) { else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
frskyData.hub.gpsSpeed_bp = SPORT_DATA_U32(packet); 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) if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp; frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
} }
else if(appId>=GPS_TIME_DATE_FIRST_ID && appId<=GPS_TIME_DATE_LAST_ID) { else if (appId >= GPS_TIME_DATE_FIRST_ID && appId <= GPS_TIME_DATE_LAST_ID) {
uint32_t gps_time_date = SPORT_DATA_U32(packet); uint32_t gps_time_date = SPORT_DATA_U32(packet);
if(gps_time_date & 0x000000ff) { if (gps_time_date & 0x000000ff) {
frskyData.hub.year = (uint16_t)((gps_time_date & 0xff000000)>>24); frskyData.hub.year = (uint16_t) ((gps_time_date & 0xff000000) >> 24);
frskyData.hub.month =(uint8_t)((gps_time_date & 0x00ff0000)>>16); frskyData.hub.month = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
frskyData.hub.day = (uint8_t)((gps_time_date & 0x0000ff00)>>8); frskyData.hub.day = (uint8_t) ((gps_time_date & 0x0000ff00) >> 8);
} }
else { else {
frskyData.hub.hour =(uint8_t) ((gps_time_date & 0xff000000)>>24); frskyData.hub.hour = (uint8_t) ((gps_time_date & 0xff000000) >> 24);
frskyData.hub.min = (uint8_t)((gps_time_date & 0x00ff0000)>>16); frskyData.hub.min = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
frskyData.hub.sec =(uint16_t) ((gps_time_date & 0x0000ff00)>>8); frskyData.hub.sec = (uint16_t) ((gps_time_date & 0x0000ff00) >> 8);
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24; 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) { else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) {
uint32_t course = SPORT_DATA_U32(packet); uint32_t course = SPORT_DATA_U32(packet);
frskyData.hub.gpsCourse_bp = course/100; frskyData.hub.gpsCourse_bp = course / 100;
frskyData.hub.gpsCourse_ap = course%100; frskyData.hub.gpsCourse_ap = course % 100;
} }
else if (appId>=GPS_ALT_FIRST_ID && appId<=GPS_ALT_LAST_ID) { else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
int32_t gps_altitude; frskyData.hub.gpsAltitude = SPORT_DATA_S32(packet);
gps_altitude = SPORT_DATA_S32(packet);
frskyData.hub.gpsAltitude_bp = gps_altitude; if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;
if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp;
frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset;
if (!frskyData.hub.baroAltitudeOffset) { if (!frskyData.hub.baroAltitudeOffset) {
if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude*100) int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP;
frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp/100; if (altitude > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = altitude;
if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude*100) if (altitude < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp/100; frskyData.hub.minAltitude = altitude;
} }
if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { if (frskyData.hub.gpsFix > 0) {
// First received GPS position => Pilot GPS position if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
if(t_coor) // First received GPS position => Pilot GPS position
getGpsPilotPosition(); getGpsPilotPosition();
} }
else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) { else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
getGpsDistance(); 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); else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) {
if (gps_long_lati_data) uint32_t gps_long_lati_data = SPORT_DATA_U32(packet);
{ uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
frskyData.hub.gpsFix = 1; gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
t_coor = 1; gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
} switch ((gps_long_lati_data & 0xc0000000) >> 30) {
case 0:
uint32_t gps_long_lati_b1w,gps_long_lati_a1w; frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps_long_lati_b1w = (gps_long_lati_data&0x3fffffff)/10000; frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w;
gps_long_lati_a1w = (gps_long_lati_data&0x3fffffff)%10000; frskyData.hub.gpsLatitudeNS = 'N';
switch ((gps_long_lati_data & 0xc0000000)>>30) { break;
case 0: { case 1:
frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w/60*100)+(gps_long_lati_b1w%60); 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.gpsLatitude_ap = gps_long_lati_a1w;
frskyData.hub.gpsLatitudeNS = 'N'; frskyData.hub.gpsLatitudeNS = 'S';
}break; break;
case 1: { case 2:
frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w/60*100) + (gps_long_lati_b1w%60); frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w;
frskyData.hub.gpsLatitudeNS = 'S'; }break; frskyData.hub.gpsLongitudeEW = 'E';
case 2: { break;
frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w/60*100) +(gps_long_lati_b1w%60); case 3:
frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
frskyData.hub.gpsLongitudeEW = 'E'; }break; frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w;
case 3: { frskyData.hub.gpsLongitudeEW = 'W';
frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w/60*100) +(gps_long_lati_b1w%60); break;
frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; }
frskyData.hub.gpsLongitudeEW = 'W';}break; if (frskyData.hub.gpsLongitudeEW && frskyData.hub.gpsLatitudeNS) {
} frskyData.hub.gpsFix = 1;
if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1) }
frskyData.hub.gpsFix = 0; else if (frskyData.hub.gpsFix > 0) {
if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1) frskyData.hub.gpsFix = 0;
frskyData.hub.gpsFix = 0; }
} }
else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) { else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
uint32_t cells = SPORT_DATA_U32(packet); uint32_t cells = SPORT_DATA_U32(packet);

63
radio/src/telemetry/frsky_sport.h
View file

@ -129,6 +129,7 @@ PACK(struct FrskySerialData {
uint16_t maxPower; uint16_t maxPower;
/* end */ /* end */
int32_t gpsAltitude;
uint8_t gpsDistNeeded; uint8_t gpsDistNeeded;
int8_t gpsFix; // -1=never fixed, 0=fix lost, 1=fixed int8_t gpsFix; // -1=never fixed, 0=fix lost, 1=fixed
@ -177,41 +178,37 @@ bool FRSKY_alarmRaised(uint8_t idx);
void resetTelemetry(); void resetTelemetry();
#define TELEMETRY_CELL_VOLTAGE(k) frskyData.hub.cellVolts[k] #define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0)
#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,"
/* Would be great, but f_printf() doesn't take floats... #define TELEMETRY_CELL_VOLTAGE(k) frskyData.hub.cellVolts[k]
#define TELEMETRY_ALT float(frskyData.hub.baroAltitude)/100 #define TELEMETRY_MIN_CELL_VOLTAGE frskyData.hub.minCellVolts
#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_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 #endif