Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Refactor gps (#13853)

Browse source
This commit is contained in:
Petr Ledvina 2024-10-23 21:45:17 +02:00 • committed by GitHub
parent 2dd6f95aad
commit 866191dfad
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
4 changed files with 76 additions and 95 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/io/dashboard.c
View file

@ -364,7 +364,7 @@ static void showRateProfilePage(void)
i2c_OLED_send_string(dev, lineBuffer);
}
#define SATELLITE_COUNT ARRAYLEN(GPS_svinfo_cno)
#define SATELLITE_COUNT ARRAYLEN(GPS_svinfo)
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
#ifdef USE_GPS
@ -392,7 +392,7 @@ static void showGpsPage(void)
uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo[index].cno * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
}

90
src/main/io/gps.c
View file

@ -77,10 +77,7 @@ gpsSolutionData_t gpsSol;
uint8_t GPS_update = 0; // toogle to distinct a GPS position update (directly or via MSP)
uint8_t GPS_numCh; // Details on numCh/svinfo in gps.h
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N];
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N];
uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N];
uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
// GPS LOST_COMMUNICATION timeout in ms (max time between received nav solutions)
#define GPS_TIMEOUT_MS 2500
@ -112,8 +109,6 @@ static const gpsInitData_t gpsInitData[] = {
{ GPS_BAUDRATE_9600, BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n" }
};
#define GPS_INIT_DATA_ENTRY_COUNT ARRAYLEN(gpsInitData)
#define DEFAULT_BAUD_RATE_INDEX 0
#ifdef USE_GPS_UBLOX
@ -360,11 +355,7 @@ uint32_t dashboardGpsNavSvInfoRcvCount = 0; // Count of
static void shiftPacketLog(void)
{
uint32_t i;
for (i = ARRAYLEN(dashboardGpsPacketLog) - 1; i > 0 ; i--) {
dashboardGpsPacketLog[i] = dashboardGpsPacketLog[i-1];
}
memmove(dashboardGpsPacketLog + 1, dashboardGpsPacketLog, (ARRAYLEN(dashboardGpsPacketLog) - 1) * sizeof(dashboardGpsPacketLog[0]));
}
static void logErrorToPacketLog(void)
@ -427,7 +418,7 @@ void gpsInit(void)
initBaudRateIndex = BAUD_COUNT;
initBaudRateCycleCount = 0;
gpsData.userBaudRateIndex = DEFAULT_BAUD_RATE_INDEX;
for (unsigned i = 0; i < GPS_INIT_DATA_ENTRY_COUNT; i++) {
for (unsigned i = 0; i < ARRAYLEN(gpsInitData); i++) {
if (gpsInitData[i].baudrateIndex == gpsPortConfig->gps_baudrateIndex) {
gpsData.userBaudRateIndex = i;
break;
@ -943,7 +934,7 @@ static void ubloxSetSbas(void)
}
}
void setSatInfoMessageRate(uint8_t divisor)
static void setSatInfoMessageRate(uint8_t divisor)
{
// enable satInfoMessage at 1:5 of the nav rate if configurator is connected
if (gpsData.ubloxM9orAbove) {
@ -1071,7 +1062,7 @@ void gpsConfigureUblox(void)
if (cmp32(gpsData.now, gpsData.state_ts) > GPS_CONFIG_BAUD_CHANGE_INTERVAL) {
gpsData.state_ts = gpsData.now;
messageSent = false;
++ messageCounter;
messageCounter++;
}
return;
}
@ -1083,7 +1074,7 @@ void gpsConfigureUblox(void)
if (gpsData.tempBaudRateIndex == 0) {
gpsData.tempBaudRateIndex = GPS_BAUDRATE_MAX; // slowest baud rate 9600
} else {
gpsData.tempBaudRateIndex--;
gpsData.tempBaudRateIndex--;
}
// set the FC baud rate to the new temp baud rate
serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.tempBaudRateIndex].baudrateIndex]);
@ -1116,7 +1107,7 @@ void gpsConfigureUblox(void)
if (cmp32(gpsData.now, gpsData.state_ts) < 1000) {
return;
}
if (gpsData.ackState == UBLOX_ACK_IDLE) {
// short delay before between commands, including the first command
@ -1266,7 +1257,7 @@ void gpsConfigureUblox(void)
default:
break;
}
}
}
// check the ackState after changing CONFIG state, or every iteration while not UBLOX_ACK_IDLE
switch (gpsData.ackState) {
@ -1324,7 +1315,7 @@ void gpsConfigureHardware(void)
static void updateGpsIndicator(timeUs_t currentTimeUs)
{
static uint32_t GPSLEDTime;
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) {
if (cmp32(currentTimeUs, GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) {
GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE;
}
@ -1493,7 +1484,7 @@ static void gpsNewData(uint16_t c)
}
#ifdef USE_GPS_UBLOX
ubloxVersion_e ubloxParseVersion(const uint32_t version) {
static ubloxVersion_e ubloxParseVersion(const uint32_t version) {
for (size_t i = 0; i < ARRAYLEN(ubloxVersionMap); ++i) {
if (version == ubloxVersionMap[i].hw) {
return (ubloxVersion_e) i;
@ -1716,8 +1707,8 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin
switch (svSatParam) {
case 1:
// SV PRN number
GPS_svinfo_chn[svSatNum - 1] = svSatNum;
GPS_svinfo_svid[svSatNum - 1] = grab_fields(str, 0);
GPS_svinfo[svSatNum - 1].chn = svSatNum;
GPS_svinfo[svSatNum - 1].svid = grab_fields(str, 0);
break;
/*case 2:
// Elevation, in degrees, 90 maximum
@ -1727,8 +1718,8 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin
break; */
case 4:
// SNR, 00 through 99 dB (null when not tracking)
GPS_svinfo_cno[svSatNum - 1] = grab_fields(str, 0);
GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
GPS_svinfo[svSatNum - 1].cno = grab_fields(str, 0);
GPS_svinfo[svSatNum - 1].quality = 0; // only used by ublox
break;
}
@ -2121,8 +2112,6 @@ static uint16_t ubxFrameParsePayloadCounter;
static bool UBLOX_parse_gps(void)
{
uint32_t i;
// lastUbxRcvMsgClass = ubxRcvMsgClass;
// lastUbxRcvMsgID = ubxRcvMsgID;
@ -2226,7 +2215,7 @@ static bool UBLOX_parse_gps(void)
dt.hours = ubxRcvMsgPayload.ubxNavPvt.hour;
dt.minutes = ubxRcvMsgPayload.ubxNavPvt.min;
dt.seconds = ubxRcvMsgPayload.ubxNavPvt.sec;
dt.millis = (ubxRcvMsgPayload.ubxNavPvt.nano > 0) ? ubxRcvMsgPayload.ubxNavPvt.nano / 1000 : 0; //up to 5ms of error
dt.millis = (ubxRcvMsgPayload.ubxNavPvt.nano > 0) ? ubxRcvMsgPayload.ubxNavPvt.nano / 1000000 : 0; // up to 5ms of error
rtcSetDateTime(&dt);
}
#endif
@ -2235,26 +2224,22 @@ static bool UBLOX_parse_gps(void)
#ifdef USE_DASHBOARD
*dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO;
#endif
GPS_numCh = ubxRcvMsgPayload.ubxNavSvinfo.numCh;
GPS_numCh = MIN(ubxRcvMsgPayload.ubxNavSvinfo.numCh, GPS_SV_MAXSATS_LEGACY);
// If we're receiving UBX-NAV-SVINFO messages, we detected a module version M7 or older.
// We can receive far more sats than we can handle for Configurator, which is the primary consumer for sat info.
// We're using the max for legacy (16) for our sizing, this smaller sizing triggers Configurator to know it's
// an M7 or earlier module and to use the older sat list format.
// We simply ignore any sats above that max, the down side is we may not see sats used for the solution, but
// the intent in Configurator is to see if sats are being acquired and their strength, so this is not an issue.
if (GPS_numCh > GPS_SV_MAXSATS_LEGACY)
GPS_numCh = GPS_SV_MAXSATS_LEGACY;
for (i = 0; i < GPS_numCh; i++) {
GPS_svinfo_chn[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].chn;
GPS_svinfo_svid[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].svid;
GPS_svinfo_quality[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].quality;
GPS_svinfo_cno[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].cno;
}
for (; i < GPS_SV_MAXSATS_LEGACY; i++) {
GPS_svinfo_chn[i] = 0;
GPS_svinfo_svid[i] = 0;
GPS_svinfo_quality[i] = 0;
GPS_svinfo_cno[i] = 0;
for (unsigned i = 0; i < ARRAYLEN(GPS_svinfo); i++) {
if (i < GPS_numCh) {
GPS_svinfo[i].chn = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].chn;
GPS_svinfo[i].svid = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].svid;
GPS_svinfo[i].quality = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].quality;
GPS_svinfo[i].cno = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].cno;
} else {
GPS_svinfo[i] = (GPS_svinfo_t){0};
}
}
#ifdef USE_DASHBOARD
dashboardGpsNavSvInfoRcvCount++;
@ -2264,31 +2249,28 @@ static bool UBLOX_parse_gps(void)
#ifdef USE_DASHBOARD
*dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO; // The display log only shows SVINFO for both SVINFO and SAT.
#endif
GPS_numCh = ubxRcvMsgPayload.ubxNavSat.numSvs;
GPS_numCh = MIN(ubxRcvMsgPayload.ubxNavSat.numSvs, GPS_SV_MAXSATS_M8N);
// If we're receiving UBX-NAV-SAT messages, we detected a module M8 or newer.
// We can receive far more sats than we can handle for Configurator, which is the primary consumer for sat info.
// We're using the max for M8 (32) for our sizing, since Configurator only supports a max of 32 sats and we
// want to limit the payload buffer space used.
// We simply ignore any sats above that max, the down side is we may not see sats used for the solution, but
// the intent in Configurator is to see if sats are being acquired and their strength, so this is not an issue.
if (GPS_numCh > GPS_SV_MAXSATS_M8N)
GPS_numCh = GPS_SV_MAXSATS_M8N;
for (i = 0; i < GPS_numCh; i++) {
GPS_svinfo_chn[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].gnssId;
GPS_svinfo_svid[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].svId;
GPS_svinfo_cno[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].cno;
GPS_svinfo_quality[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].flags;
}
for (; i < GPS_SV_MAXSATS_M8N; i++) {
GPS_svinfo_chn[i] = 255;
GPS_svinfo_svid[i] = 0;
GPS_svinfo_quality[i] = 0;
GPS_svinfo_cno[i] = 0;
for (unsigned i = 0; i < ARRAYLEN(GPS_svinfo); i++) {
if (i < GPS_numCh) {
GPS_svinfo[i].chn = ubxRcvMsgPayload.ubxNavSat.svs[i].gnssId;
GPS_svinfo[i].svid = ubxRcvMsgPayload.ubxNavSat.svs[i].svId;
GPS_svinfo[i].cno = ubxRcvMsgPayload.ubxNavSat.svs[i].cno;
GPS_svinfo[i].quality = ubxRcvMsgPayload.ubxNavSat.svs[i].flags;
} else {
GPS_svinfo[i] = (GPS_svinfo_t){ .chn = 255 };
}
}
// Setting the number of channels higher than GPS_SV_MAXSATS_LEGACY is the only way to tell BF Configurator we're sending the
// enhanced sat list info without changing the MSP protocol. Also, we're sending the complete list each time even if it's empty, so
// BF Conf can erase old entries shown on screen when channels are removed from the list.
// TODO: GPS_numCh = MAX(GPS_numCh, GPS_SV_MAXSATS_LEGACY + 1);
GPS_numCh = GPS_SV_MAXSATS_M8N;
#ifdef USE_DASHBOARD
dashboardGpsNavSvInfoRcvCount++;

65
src/main/io/gps.h
View file

@ -313,35 +313,40 @@ extern gpsData_t gpsData;
extern gpsSolutionData_t gpsSol;
#define GPS_SV_MAXSATS_LEGACY 16U
#define GPS_SV_MAXSATS_M8N 32U
#define GPS_SV_MAXSATS_M8N 32U // must be larger than MAXSATS_LEGACY
extern uint8_t GPS_update; // toggles on GPS nav position update (directly or via MSP)
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N]; // When NumCh is 16 or less: Channel number
// When NumCh is more than 16: GNSS Id
// 0 = GPS, 1 = SBAS, 2 = Galileo, 3 = BeiDou
// 4 = IMES, 5 = QZSS, 6 = Glonass
extern uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N]; // Satellite ID
extern uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N]; // When NumCh is 16 or less: Bitfield Qualtity
// When NumCh is more than 16: flags
// bits 2..0: signal quality indicator
// 0 = no signal
// 1 = searching signal
// 2 = signal acquired
// 3 = signal detected but unusable
// 4 = code locked and time synchronized
// 5,6,7 = code and carrier locked and time synchronized
// bit 3:
// 1 = signal currently being used for navigaion
// bits 5..4: signal health flag
// 0 = unknown
// 1 = healthy
// 2 = unhealthy
// bit 6:
// 1 = differential correction data available for this SV
// bit 7:
// 1 = carrier smoothed pseudorange used
extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N]; // Carrier to Noise Ratio (Signal Strength)
extern uint8_t GPS_numCh; // Number of svinfo channels
typedef struct GPS_svinfo_s {
uint8_t chn; // When NumCh is 16 or less: Channel number
// When NumCh is more than 16: GNSS Id
// 0 = GPS, 1 = SBAS, 2 = Galileo, 3 = BeiDou
// 4 = IMES, 5 = QZSS, 6 = Glonass
uint8_t svid; // Satellite ID
uint8_t quality; // When NumCh is 16 or less: Bitfield Qualtity
// When NumCh is more than 16: flags
// bits 2..0: signal quality indicator
// 0 = no signal
// 1 = searching signal
// 2 = signal acquired
// 3 = signal detected but unusable
// 4 = code locked and time synchronized
// 5,6,7 = code and carrier locked and time synchronized
// bit 3:
// 1 = signal currently being used for navigaion
// bits 5..4: signal health flag
// 0 = unknown
// 1 = healthy
// 2 = unhealthy
// bit 6:
// 1 = differential correction data available for this SV
// bit 7:
// 1 = carrier smoothed pseudorange used
uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
} GPS_svinfo_t;
extern GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
#define TASK_GPS_RATE 100 // default update rate of GPS task
#define TASK_GPS_RATE_FAST 500 // update rate of GPS task while Rx buffer is not empty
@ -375,12 +380,6 @@ extern uint32_t dashboardGpsNavSvInfoRcvCount; // Count of time
#define GPS_DBHZ_MAX 55
#endif // USE_DASHBOARD
#ifdef USE_GPS_UBLOX
ubloxVersion_e ubloxParseVersion(const uint32_t version);
void setSatInfoMessageRate(uint8_t divisor);
#endif
void gpsInit(void);
void gpsUpdate(timeUs_t currentTimeUs);
bool gpsNewFrame(uint8_t c);

12
src/main/msp/msp.c
View file

@ -1530,12 +1530,12 @@ case MSP_NAME:
case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh);
for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]);
}
for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo[i].chn);
sbufWriteU8(dst, GPS_svinfo[i].svid);
sbufWriteU8(dst, GPS_svinfo[i].quality);
sbufWriteU8(dst, GPS_svinfo[i].cno);
}
break;
#ifdef USE_GPS_RESCUE