diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index aab5ce5bd5..5ed1179ffc 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -489,7 +489,7 @@ task_t tasks[TASK_COUNT] = { #endif #ifdef USE_GPS - [TASK_GPS] = DEFINE_TASK("GPS", NULL, NULL, gpsUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer) + [TASK_GPS] = DEFINE_TASK("GPS", NULL, NULL, gpsUpdate, TASK_PERIOD_HZ(TASK_GPS_RATE), TASK_PRIORITY_MEDIUM), // Required to prevent buffer overruns if running at 115200 baud (115 bytes / period < 256 bytes buffer) #endif #ifdef USE_MAG diff --git a/src/main/io/gps.c b/src/main/io/gps.c index de5273def5..2415347bf6 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -683,13 +683,19 @@ void gpsUpdate(timeUs_t currentTimeUs) timeUs_t executeTimeUs; gpsState_e gpsCurState = gpsData.state; - // read out available GPS bytes if (gpsPort) { - while (serialRxBytesWaiting(gpsPort) && (cmpTimeUs(micros(), currentTimeUs) < GPS_MAX_WAIT_DATA_RX)) { + while (serialRxBytesWaiting(gpsPort)) { + if (cmpTimeUs(micros(), currentTimeUs) > GPS_MAX_WAIT_DATA_RX) { + // Wait 1ms and come back + rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE_FAST)); + return; + } gpsNewData(serialRead(gpsPort)); } - } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP + // Restore default task rate + rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE)); + } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP gpsSetState(GPS_STATE_RECEIVING_DATA); gpsData.lastMessage = millis(); sensorsSet(SENSOR_GPS); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 7db419b0ac..7f355a6851 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -180,6 +180,9 @@ extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Str #define GPS_DBHZ_MIN 0 #define GPS_DBHZ_MAX 55 +#define TASK_GPS_RATE 100 +#define TASK_GPS_RATE_FAST 1000 + void gpsInit(void); void gpsUpdate(timeUs_t currentTimeUs); bool gpsNewFrame(uint8_t c);