mirror of
https://github.com/opentx/opentx.git
synced 2025-07-25 17:25:13 +03:00
Port of GPS modifications from SVN
This commit is contained in:
bsongis
parent
c98e718e16
commit
590a58a6e9
8 changed files with 166 additions and 189 deletions
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -221,26 +221,33 @@ 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_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_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_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)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
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);
|
||||
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;
|
||||
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) {
|
||||
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;
|
||||
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;
|
||||
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_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.gpsFix > 0) {
|
||||
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);
|
||||
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);
|
||||
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.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.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;
|
||||
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)
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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_STREAMING() (frskyData.rssi[0].value > 0)
|
||||
|
||||
#define TELEMETRY_CELL_VOLTAGE(k) frskyData.hub.cellVolts[k]
|
||||
#define TELEMETRY_MIN_CELL_VOLTAGE frskyData.hub.minCellVolts
|
||||
|
||||
#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_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_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_CURRENT frskyData.hub.current / 10, frskyData.hub.current % 10
|
||||
#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_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10
|
||||
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 10, frskyData.hub.current % 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_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
|
||||
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
|
||||
|
||||
/* 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_STREAMING() (frskyData.rssi[0].value > 0)
|
||||
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100),
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue