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); 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) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
#ifdef USE_GPS #ifdef USE_GPS
@ -392,7 +392,7 @@ static void showGpsPage(void)
uint32_t index; uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; 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); bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset); i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
} }

80
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_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_numCh; // Details on numCh/svinfo in gps.h
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N]; GPS_svinfo_t GPS_svinfo[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 LOST_COMMUNICATION timeout in ms (max time between received nav solutions) // GPS LOST_COMMUNICATION timeout in ms (max time between received nav solutions)
#define GPS_TIMEOUT_MS 2500 #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" } { 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 #define DEFAULT_BAUD_RATE_INDEX 0
#ifdef USE_GPS_UBLOX #ifdef USE_GPS_UBLOX
@ -360,11 +355,7 @@ uint32_t dashboardGpsNavSvInfoRcvCount = 0; // Count of
static void shiftPacketLog(void) static void shiftPacketLog(void)
{ {
uint32_t i; memmove(dashboardGpsPacketLog + 1, dashboardGpsPacketLog, (ARRAYLEN(dashboardGpsPacketLog) - 1) * sizeof(dashboardGpsPacketLog[0]));
for (i = ARRAYLEN(dashboardGpsPacketLog) - 1; i > 0 ; i--) {
dashboardGpsPacketLog[i] = dashboardGpsPacketLog[i-1];
}
} }
static void logErrorToPacketLog(void) static void logErrorToPacketLog(void)
@ -427,7 +418,7 @@ void gpsInit(void)
initBaudRateIndex = BAUD_COUNT; initBaudRateIndex = BAUD_COUNT;
initBaudRateCycleCount = 0; initBaudRateCycleCount = 0;
gpsData.userBaudRateIndex = DEFAULT_BAUD_RATE_INDEX; 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) { if (gpsInitData[i].baudrateIndex == gpsPortConfig->gps_baudrateIndex) {
gpsData.userBaudRateIndex = i; gpsData.userBaudRateIndex = i;
break; 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 // enable satInfoMessage at 1:5 of the nav rate if configurator is connected
if (gpsData.ubloxM9orAbove) { if (gpsData.ubloxM9orAbove) {
@ -1071,7 +1062,7 @@ void gpsConfigureUblox(void)
if (cmp32(gpsData.now, gpsData.state_ts) > GPS_CONFIG_BAUD_CHANGE_INTERVAL) { if (cmp32(gpsData.now, gpsData.state_ts) > GPS_CONFIG_BAUD_CHANGE_INTERVAL) {
gpsData.state_ts = gpsData.now; gpsData.state_ts = gpsData.now;
messageSent = false; messageSent = false;
++ messageCounter; messageCounter++;
} }
return; return;
} }
@ -1324,7 +1315,7 @@ void gpsConfigureHardware(void)
static void updateGpsIndicator(timeUs_t currentTimeUs) static void updateGpsIndicator(timeUs_t currentTimeUs)
{ {
static uint32_t GPSLEDTime; 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; GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE; LED1_TOGGLE;
} }
@ -1493,7 +1484,7 @@ static void gpsNewData(uint16_t c)
} }
#ifdef USE_GPS_UBLOX #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) { for (size_t i = 0; i < ARRAYLEN(ubloxVersionMap); ++i) {
if (version == ubloxVersionMap[i].hw) { if (version == ubloxVersionMap[i].hw) {
return (ubloxVersion_e) i; return (ubloxVersion_e) i;
@ -1716,8 +1707,8 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin
switch (svSatParam) { switch (svSatParam) {
case 1: case 1:
// SV PRN number // SV PRN number
GPS_svinfo_chn[svSatNum - 1] = svSatNum; GPS_svinfo[svSatNum - 1].chn = svSatNum;
GPS_svinfo_svid[svSatNum - 1] = grab_fields(str, 0); GPS_svinfo[svSatNum - 1].svid = grab_fields(str, 0);
break; break;
/*case 2: /*case 2:
// Elevation, in degrees, 90 maximum // Elevation, in degrees, 90 maximum
@ -1727,8 +1718,8 @@ static void parseFieldNmea(gpsDataNmea_t *data, char *str, uint8_t gpsFrame, uin
break; */ break; */
case 4: case 4:
// SNR, 00 through 99 dB (null when not tracking) // SNR, 00 through 99 dB (null when not tracking)
GPS_svinfo_cno[svSatNum - 1] = grab_fields(str, 0); GPS_svinfo[svSatNum - 1].cno = grab_fields(str, 0);
GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox GPS_svinfo[svSatNum - 1].quality = 0; // only used by ublox
break; break;
} }
@ -2121,8 +2112,6 @@ static uint16_t ubxFrameParsePayloadCounter;
static bool UBLOX_parse_gps(void) static bool UBLOX_parse_gps(void)
{ {
uint32_t i;
// lastUbxRcvMsgClass = ubxRcvMsgClass; // lastUbxRcvMsgClass = ubxRcvMsgClass;
// lastUbxRcvMsgID = ubxRcvMsgID; // lastUbxRcvMsgID = ubxRcvMsgID;
@ -2226,7 +2215,7 @@ static bool UBLOX_parse_gps(void)
dt.hours = ubxRcvMsgPayload.ubxNavPvt.hour; dt.hours = ubxRcvMsgPayload.ubxNavPvt.hour;
dt.minutes = ubxRcvMsgPayload.ubxNavPvt.min; dt.minutes = ubxRcvMsgPayload.ubxNavPvt.min;
dt.seconds = ubxRcvMsgPayload.ubxNavPvt.sec; 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); rtcSetDateTime(&dt);
} }
#endif #endif
@ -2235,26 +2224,22 @@ static bool UBLOX_parse_gps(void)
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
*dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO; *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO;
#endif #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. // 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 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 // 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. // 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 // 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. // 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) for (unsigned i = 0; i < ARRAYLEN(GPS_svinfo); i++) {
GPS_numCh = GPS_SV_MAXSATS_LEGACY; if (i < GPS_numCh) {
for (i = 0; i < GPS_numCh; i++) { GPS_svinfo[i].chn = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].chn;
GPS_svinfo_chn[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].chn; GPS_svinfo[i].svid = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].svid;
GPS_svinfo_svid[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].svid; GPS_svinfo[i].quality = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].quality;
GPS_svinfo_quality[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].quality; GPS_svinfo[i].cno = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].cno;
GPS_svinfo_cno[i] = ubxRcvMsgPayload.ubxNavSvinfo.channel[i].cno; } else {
GPS_svinfo[i] = (GPS_svinfo_t){0};
} }
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;
} }
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
dashboardGpsNavSvInfoRcvCount++; dashboardGpsNavSvInfoRcvCount++;
@ -2264,31 +2249,28 @@ static bool UBLOX_parse_gps(void)
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
*dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO; // The display log only shows SVINFO for both SVINFO and SAT. *dashboardGpsPacketLogCurrentChar = DASHBOARD_LOG_UBLOX_SVINFO; // The display log only shows SVINFO for both SVINFO and SAT.
#endif #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. // 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 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 // 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. // 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 // 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. // 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) for (unsigned i = 0; i < ARRAYLEN(GPS_svinfo); i++) {
GPS_numCh = GPS_SV_MAXSATS_M8N; if (i < GPS_numCh) {
for (i = 0; i < GPS_numCh; i++) { GPS_svinfo[i].chn = ubxRcvMsgPayload.ubxNavSat.svs[i].gnssId;
GPS_svinfo_chn[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].gnssId; GPS_svinfo[i].svid = ubxRcvMsgPayload.ubxNavSat.svs[i].svId;
GPS_svinfo_svid[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].svId; GPS_svinfo[i].cno = ubxRcvMsgPayload.ubxNavSat.svs[i].cno;
GPS_svinfo_cno[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].cno; GPS_svinfo[i].quality = ubxRcvMsgPayload.ubxNavSat.svs[i].flags;
GPS_svinfo_quality[i] = ubxRcvMsgPayload.ubxNavSat.svs[i].flags; } else {
GPS_svinfo[i] = (GPS_svinfo_t){ .chn = 255 };
} }
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;
} }
// Setting the number of channels higher than GPS_SV_MAXSATS_LEGACY is the only way to tell BF Configurator we're sending the // 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 // 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. // 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; GPS_numCh = GPS_SV_MAXSATS_M8N;
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
dashboardGpsNavSvInfoRcvCount++; dashboardGpsNavSvInfoRcvCount++;

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

@ -313,16 +313,19 @@ extern gpsData_t gpsData;
extern gpsSolutionData_t gpsSol; extern gpsSolutionData_t gpsSol;
#define GPS_SV_MAXSATS_LEGACY 16U #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_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 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 // When NumCh is more than 16: GNSS Id
// 0 = GPS, 1 = SBAS, 2 = Galileo, 3 = BeiDou // 0 = GPS, 1 = SBAS, 2 = Galileo, 3 = BeiDou
// 4 = IMES, 5 = QZSS, 6 = Glonass // 4 = IMES, 5 = QZSS, 6 = Glonass
extern uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N]; // Satellite ID uint8_t svid; // Satellite ID
extern uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N]; // When NumCh is 16 or less: Bitfield Qualtity uint8_t quality; // When NumCh is 16 or less: Bitfield Qualtity
// When NumCh is more than 16: flags // When NumCh is more than 16: flags
// bits 2..0: signal quality indicator // bits 2..0: signal quality indicator
// 0 = no signal // 0 = no signal
@ -341,7 +344,9 @@ extern uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N]; // When NumCh is 16 or l
// 1 = differential correction data available for this SV // 1 = differential correction data available for this SV
// bit 7: // bit 7:
// 1 = carrier smoothed pseudorange used // 1 = carrier smoothed pseudorange used
extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N]; // Carrier to Noise Ratio (Signal Strength) 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 100 // default update rate of GPS task
#define TASK_GPS_RATE_FAST 500 // update rate of GPS task while Rx buffer is not empty #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 #define GPS_DBHZ_MAX 55
#endif // USE_DASHBOARD #endif // USE_DASHBOARD
#ifdef USE_GPS_UBLOX
ubloxVersion_e ubloxParseVersion(const uint32_t version);
void setSatInfoMessageRate(uint8_t divisor);
#endif
void gpsInit(void); void gpsInit(void);
void gpsUpdate(timeUs_t currentTimeUs); void gpsUpdate(timeUs_t currentTimeUs);
bool gpsNewFrame(uint8_t c); bool gpsNewFrame(uint8_t c);

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

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