static void sendGPS(void)
{
int32_t localGPS_coord[2] = {0,0};
// Don't set dummy GPS data, if we already had a GPS fix
// it can be usefull to keep last valid coordinates
static uint8_t gpsFixOccured = 0;
//Dummy data if no 3D fix, this way we can display heading in Taranis
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
localGPS_coord[LAT] = GPS_coord[LAT];
localGPS_coord[LON] = GPS_coord[LON];
gpsFixOccured = 1;
} else {
// Send dummy GPS Data in order to display compass value
localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
}
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
}
bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
gpsCoordinateDDDMMmmmm_t coordinate;
uint32_t latitudeBcd, longitudeBcd, altitudeLo;
uint16_t altitudeLoBcd, groundCourseBcd, hdop;
uint8_t hdopBcd, gpsFlags;
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
return false;
}
// lattitude
GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate);
latitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
// longitude
GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate);
longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
// altitude (low order)
altitudeLo = ABS(gpsSol.llh.altCm) / 10;
altitudeLoBcd = dec2bcd(altitudeLo % 100000);
// Ground course
groundCourseBcd = dec2bcd(gpsSol.groundCourse);
// HDOP
hdop = gpsSol.hdop / 10;
hdop = (hdop > 99) ? 99 : hdop;
hdopBcd = dec2bcd(hdop);
// flags
gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT;
gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0;
gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0;
gpsFlags |= (gpsSol.llh.altCm < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0;
gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0;
// SRXL frame
sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16(dst, altitudeLoBcd);
sbufWriteU32(dst, latitudeBcd);
sbufWriteU32(dst, longitudeBcd);
sbufWriteU16(dst, groundCourseBcd);
sbufWriteU8(dst, hdopBcd);
sbufWriteU8(dst, gpsFlags);
return true;
}
static void sendLatLong(int32_t coord[2])
{
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(coord[LON] < 0 ? 'W' : 'E');
}
