diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index adcbd37339..80cbe69cba 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -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); } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 299e594b65..8360af4ce6 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -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++; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 66153393d4..c22bb47fec 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -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); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 1f260dc80d..bf520e0d6a 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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