Add GPS Lap Timer (#11856)

* Add gps lap timer

* change timing to GPS time instead of local time

* rebase and minor changes

* implement KarateBrot's suggestions

* follow ledvinap's suggestions, some OSD symbol changes

* move platform.h include to the top

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

* fix osd elements not showing, remove useless block

* cleanup, move pg stuff to pg folder

* cleanup from review

* minor mods to gps lap timer update, add number of laps tracked

* rename time variable

* add const to timeMs

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

* Update licenses, add is_sys_element macro

* update licenses

* round to nearest centisecond

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

---------

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

make/source.mk

@@ -87,6 +87,7 @@ COMMON_SRC = \
             flight/position.c \
             flight/failsafe.c \
             flight/gps_rescue.c \
+            fc/gps_lap_timer.c \
             flight/dyn_notch_filter.c \
             flight/imu.c \
             flight/mixer.c \
@@ -137,6 +138,7 @@ COMMON_SRC = \
             cms/cms_menu_failsafe.c \
             cms/cms_menu_firmware.c \
             cms/cms_menu_gps_rescue.c \
+            cms/cms_menu_gps_lap_timer.c \
             cms/cms_menu_imu.c \
             cms/cms_menu_ledstrip.c \
             cms/cms_menu_main.c \
@@ -395,6 +397,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             cms/cms_menu_failsafe.c \
             cms/cms_menu_firmware.c \
             cms/cms_menu_gps_rescue.c \
+            cms/cms_menu_gps_lap_timer.c \
             cms/cms_menu_imu.c \
             cms/cms_menu_ledstrip.c \
             cms/cms_menu_main.c \

src/main/cli/cli.c

@@ -514,6 +514,11 @@ static void printValuePointer(const char *cmdName, const clivalue_t *var, const
                 // uin32_t array
                 cliPrintf("%u", ((uint32_t *)valuePointer)[i]);
                 break;
+
+            case VAR_INT32:
+                // in32_t array
+                cliPrintf("%d", ((int32_t *)valuePointer)[i]);
+                break;
             }
 
             if (i < var->config.array.length - 1) {
@@ -543,6 +548,10 @@ static void printValuePointer(const char *cmdName, const clivalue_t *var, const
         case VAR_UINT32:
             value = *(uint32_t *)valuePointer;
 
+            break;
+        case VAR_INT32:
+            value = *(int32_t *)valuePointer;
+
             break;
         }
 
@@ -556,6 +565,13 @@ static void printValuePointer(const char *cmdName, const clivalue_t *var, const
                 } else if (full) {
                     cliPrintf(" 0 %u", var->config.u32Max);
                 }
+            } else if ((var->type & VALUE_TYPE_MASK) == VAR_INT32) {
+                cliPrintf("%d", (int32_t)value);
+                if ((int32_t)value > var->config.d32Max || (int32_t)value < -var->config.d32Max) {
+                    valueIsCorrupted = true;
+                } else if (full) {
+                    cliPrintf(" 0 %u", var->config.u32Max);
+                }
             } else {
                 int min;
                 int max;
@@ -627,6 +643,9 @@ static bool valuePtrEqualsDefault(const clivalue_t *var, const void *ptr, const
         case VAR_UINT32:
             result = result && (((uint32_t *)ptr)[i] & mask) == (((uint32_t *)ptrDefault)[i] & mask);
             break;
+        case VAR_INT32:
+            result = result && (((int32_t *)ptr)[i] & mask) == (((int32_t *)ptrDefault)[i] & mask);
+            break;
         }
     }
 
@@ -792,6 +811,10 @@ static void cliPrintVarRange(const clivalue_t *var)
         case VAR_UINT32:
             cliPrintLinef("Allowed range: 0 - %u", var->config.u32Max);
 
+            break;
+        case VAR_INT32:
+            cliPrintLinef("Allowed range: %d - %d", -var->config.d32Max, var->config.d32Max);
+
             break;
         case VAR_UINT8:
         case VAR_UINT16:
@@ -873,6 +896,16 @@ static void cliSetVar(const clivalue_t *var, const uint32_t value)
             }
             *(uint32_t *)ptr = workValue;
             break;
+
+        case VAR_INT32:
+            mask = 1 << var->config.bitpos;
+            if (value) {
+                workValue = *(int32_t *)ptr | mask;
+            } else {
+                workValue = *(int32_t *)ptr & ~mask;
+            }
+            *(int32_t *)ptr = workValue;
+            break;
         }
     } else {
         switch (var->type & VALUE_TYPE_MASK) {
@@ -895,6 +928,10 @@ static void cliSetVar(const clivalue_t *var, const uint32_t value)
         case VAR_UINT32:
             *(uint32_t *)ptr = value;
             break;
+
+        case VAR_INT32:
+            *(int32_t *)ptr = value;
+            break;
         }
     }
 }
@@ -4400,6 +4437,14 @@ STATIC_UNIT_TESTED void cliSet(const char *cmdName, char *cmdline)
                         cliSetVar(val, value);
                         valueChanged = true;
                     }
+                } else if ((val->type & VALUE_TYPE_MASK) == VAR_INT32) {
+                    int32_t value = strtol(eqptr, NULL, 10);
+
+                    // INT32s are limited to being symmetric, so we test both bounds with the same magnitude
+                    if (value <= val->config.d32Max && value >= -val->config.d32Max) {
+                        cliSetVar(val, value);
+                        valueChanged = true;
+                    }
                 } else {
                     int value = atoi(eqptr);
 
@@ -4497,6 +4542,15 @@ STATIC_UNIT_TESTED void cliSet(const char *cmdName, char *cmdline)
                             *data = (uint32_t)strtoul((const char*) valPtr, NULL, 10);
                         }
 
+                        break;
+                    case VAR_INT32:
+                        {
+                            // fetch data pointer
+                            int32_t *data = (int32_t *)cliGetValuePointer(val) + i;
+                            // store value
+                            *data = (int32_t)strtol((const char*) valPtr, NULL, 10);
+                        }
+
                         break;
                     }
 

src/main/cli/settings.c

@@ -50,6 +50,7 @@
 #include "config/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
+#include "fc/gps_lap_timer.h"
 #include "fc/parameter_names.h"
 #include "fc/rc.h"
 #include "fc/rc_adjustments.h"
@@ -1003,6 +1004,7 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_GPS_AUTO_BAUD,              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) },
     { PARAM_NAME_GPS_UBLOX_ACQUIRE_MODEL,    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODELS }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_acquire_model) },
     { PARAM_NAME_GPS_UBLOX_FLIGHT_MODEL,     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODELS }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_flight_model) },
+    { PARAM_NAME_GPS_UPDATE_RATE_HZ,         VAR_UINT8  | MASTER_VALUE,               .config.minmaxUnsigned = {1, 19},          PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_update_rate_hz) },
     { PARAM_NAME_GPS_UBLOX_USE_GALILEO,      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) },
     { PARAM_NAME_GPS_UBLOX_FULL_POWER,       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_full_power) },
     { PARAM_NAME_GPS_SET_HOME_POINT_ONCE,    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) },
@@ -1048,6 +1050,14 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_GPS_RESCUE_USE_MAG,         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
 #endif // USE_MAG
 #endif // USE_GPS_RESCUE
+
+#ifdef USE_GPS_LAP_TIMER
+    { PARAM_NAME_GPS_LAP_TIMER_GATE_LAT,       VAR_INT32  | MASTER_VALUE, .config.d32Max = 900000000,           PG_GPS_LAP_TIMER, offsetof(gpsLapTimerConfig_t, gateLat) },
+    { PARAM_NAME_GPS_LAP_TIMER_GATE_LON,       VAR_INT32  | MASTER_VALUE, .config.d32Max = 1800000000,          PG_GPS_LAP_TIMER, offsetof(gpsLapTimerConfig_t, gateLon) },
+    { PARAM_NAME_GPS_LAP_TIMER_MIN_LAP_TIME,   VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_GPS_LAP_TIMER, offsetof(gpsLapTimerConfig_t, minimumLapTimeSeconds) },
+    { PARAM_NAME_GPS_LAP_TIMER_GATE_TOLERANCE, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 100 },  PG_GPS_LAP_TIMER, offsetof(gpsLapTimerConfig_t, gateToleranceM) },
+#endif // USE_GPS_LAP_TIMER
+
 #endif // USE_GPS
 
     { PARAM_NAME_DEADBAND,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },
@@ -1381,6 +1391,11 @@ const clivalue_t valueTable[] = {
     { "osd_gps_speed_pos",          VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_SPEED]) },
     { "osd_gps_lon_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LON]) },
     { "osd_gps_lat_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LAT]) },
+#ifdef USE_GPS_LAP_TIMER
+    { "osd_gps_lap_curr_pos",       VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LAP_TIME_CURRENT]) },
+    { "osd_gps_lap_prev_pos",       VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LAP_TIME_PREVIOUS]) },
+    { "osd_gps_lap_best3_pos",      VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LAP_TIME_BEST3]) },
+#endif // USE_GPS_LAP_TIMER
     { "osd_gps_sats_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_SATS]) },
     { "osd_home_dir_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_HOME_DIR]) },
     { "osd_home_dist_pos",          VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_HOME_DIST]) },

src/main/cli/settings.h

@@ -164,6 +164,7 @@ typedef enum {
     VAR_UINT16 = (2 << VALUE_TYPE_OFFSET),
     VAR_INT16 = (3 << VALUE_TYPE_OFFSET),
     VAR_UINT32 = (4 << VALUE_TYPE_OFFSET),
+    VAR_INT32 = (5 << VALUE_TYPE_OFFSET),
 
     // value section, bits 3-4
     MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
@@ -219,6 +220,7 @@ typedef union {
     cliStringLengthConfig_t string;           // used for MODE_STRING
     uint8_t bitpos;                           // used for MODE_BITSET
     uint32_t u32Max;                          // used for MODE_DIRECT with VAR_UINT32
+    int32_t d32Max;                           // used for MODE_DIRECT with VAR_INT32
 } cliValueConfig_t;
 
 typedef struct clivalue_s {

src/main/cms/cms_menu_gps_lap_timer.c

@@ -0,0 +1,129 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <ctype.h>
+
+#include "platform.h"
+
+#ifdef USE_CMS_GPS_LAP_TIMER_MENU
+
+#include "cli/settings.h"
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+#include "config/config.h"
+
+#include "fc/gps_lap_timer.h"
+
+static uint16_t gpsLapTimerConfig_minimumLapTimeSeconds;
+static uint8_t gpsLapTimerConfig_gateToleranceM;
+
+static const void *cms_menuGpsLapTimerOnEnter(displayPort_t *pDisp)
+{
+    UNUSED(pDisp);
+
+    gpsLapTimerConfig_minimumLapTimeSeconds = gpsLapTimerConfig()->minimumLapTimeSeconds;
+    gpsLapTimerConfig_gateToleranceM = gpsLapTimerConfig()->gateToleranceM;
+
+    return NULL;
+}
+
+static const void *cms_menuGpsLapTimerOnExit(displayPort_t *pDisp, const OSD_Entry *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    gpsLapTimerConfigMutable()->minimumLapTimeSeconds = gpsLapTimerConfig_minimumLapTimeSeconds;
+    gpsLapTimerConfigMutable()->gateToleranceM = gpsLapTimerConfig_gateToleranceM;
+
+    return NULL;
+}
+
+static const void *cmsStartSetGate(displayPort_t *pDisp)
+{
+    UNUSED(pDisp);
+
+    gpsLapTimerStartSetGate();
+
+    return NULL;
+}
+
+static const void *cmsEndSetGate(displayPort_t *pDisp, const OSD_Entry *self)
+{
+    UNUSED(pDisp);
+    UNUSED(self);
+
+    gpsLapTimerEndSetGate();
+
+    return NULL;
+}
+
+static OSD_UINT16_t setGateCMSWaitCount = { &gpsLapTimerData.numberOfSetReadings, 0, 2000, 0 };
+static OSD_UINT32_t setGateCMSdistCM = { &gpsLapTimerData.distToPointCM, 0, 200, 0 };
+
+static const OSD_Entry cmsSetGateMenuEntries[] = {
+    {"SETTING POSITION", OME_Label,     NULL, NULL},
+
+    {"READINGS",  OME_UINT16 | DYNAMIC, NULL, &setGateCMSWaitCount},
+    {"DISTANCE(CM)",  OME_UINT32 | DYNAMIC, NULL, &setGateCMSdistCM},
+
+    {"BACK",       OME_Back,            NULL, NULL},
+    { NULL,        OME_END,             NULL, NULL}
+};
+
+static CMS_Menu cmsSetGateMenu = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "GATESET",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = cmsStartSetGate,
+    .onExit = cmsEndSetGate,
+    .onDisplayUpdate = NULL,
+    .entries = cmsSetGateMenuEntries,
+};
+
+const OSD_Entry cms_menuGpsLapTimerEntries[] =
+{
+    {"--- GPS LAP TIMER ---", OME_Label,   NULL,          NULL},
+
+    {"SET POSITION",          OME_Funcall, cmsMenuChange, &cmsSetGateMenu},
+    {"GATE RADIUS(M)",        OME_UINT8,   NULL,          &(OSD_UINT8_t){&gpsLapTimerConfig_gateToleranceM, 1, 100, 1}},
+    {"MIN LAP SECONDS",       OME_UINT16,  NULL,          &(OSD_UINT16_t){&gpsLapTimerConfig_minimumLapTimeSeconds,   0, 3000, 1}},
+
+    {"BACK",                  OME_Back,    NULL,          NULL},
+    {NULL,                    OME_END,     NULL,          NULL}};
+
+CMS_Menu cms_menuGpsLapTimer = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "MENUGPSLAPTIMER",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = cms_menuGpsLapTimerOnEnter,
+    .onExit = cms_menuGpsLapTimerOnExit,
+    .onDisplayUpdate = NULL,
+    .entries = cms_menuGpsLapTimerEntries,
+};
+
+#endif // CMS_GPS_LAP_TIMER_MENU

src/main/cms/cms_menu_gps_lap_timer.h

@@ -0,0 +1,24 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+extern CMS_Menu cms_menuGpsLapTimer;

src/main/cms/cms_menu_misc.c

@@ -32,6 +32,7 @@
 
 #include "cms/cms.h"
 #include "cms/cms_types.h"
+#include "cms/cms_menu_gps_lap_timer.h"
 #include "cms/cms_menu_ledstrip.h"
 
 #include "common/utils.h"
@@ -161,6 +162,9 @@ static const OSD_Entry menuMiscEntries[]=
     { "DIGITAL IDLE",  OME_UINT8 | REBOOT_REQUIRED,   NULL,          &(OSD_UINT8_t) { &motorConfig_digitalIdleOffsetValue,    0,  200, 1 } },
     { "FPV CAM ANGLE", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &rxConfig_fpvCamAngleDegrees,           0,   90, 1 } },
     { "RC PREV",       OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview},
+#ifdef USE_GPS_LAP_TIMER
+    { "GPS LAP TIMER", OME_Submenu, cmsMenuChange, &cms_menuGpsLapTimer },
+#endif // USE_GPS_LAP_TIMER
 
     { "BACK", OME_Back, NULL, NULL},
     { NULL, OME_END, NULL, NULL}

src/main/cms/cms_menu_osd.c

@@ -164,6 +164,11 @@ const OSD_Entry menuOsdActiveElemsEntries[] =
     {"STICK OVERLAY LEFT", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_STICK_OVERLAY_LEFT]},
     {"STICK OVERLAY RIGHT",OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_STICK_OVERLAY_RIGHT]},
 #endif
+#ifdef USE_GPS_LAP_TIMER
+    {"LAP TIME CURRENT",   OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_GPS_LAP_TIME_CURRENT]},
+    {"LAP TIME PREVIOUS",  OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_GPS_LAP_TIME_PREVIOUS]},
+    {"LAP TIME BEST 3",    OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_GPS_LAP_TIME_BEST3]},
+#endif // USE_GPS_LAP_TIMER
     {"PILOT NAME",         OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_PILOT_NAME]},
     {"RC CHANNELS",        OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_RC_CHANNELS]},
     {"CAMERA FRAME",       OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_CAMERA_FRAME]},

src/main/drivers/osd_symbols.h

@@ -139,6 +139,10 @@
 #define SYM_ON_M                    0x9B
 #define SYM_FLY_M                   0x9C
 
+// Lap Timer
+#define SYM_CHECKERED_FLAG          0x24
+#define SYM_PREV_LAP_TIME           0x79
+
 // Speed
 #define SYM_SPEED                   0x70
 #define SYM_KPH                     0x9E

src/main/fc/gps_lap_timer.c

@@ -0,0 +1,154 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "common/maths.h"
+
+#include "drivers/time.h"
+
+#include "fc/rc_modes.h"
+
+#include "io/gps.h"
+
+#include "gps_lap_timer.h"
+
+#define MAX_GATE_SET_READINGS 1000
+
+// if 1000 readings of 32-bit values are used, the total (for use in calculating the averate) would need 42 bits max.
+static int64_t gateSetLatReadings;
+static int64_t gateSetLonReadings;
+static bool settingGate = false;
+static uint32_t minDistance = UINT32_MAX;
+static uint32_t minDistanceTime = 0;
+static bool wasInCircle = false;
+
+gpsLapTimerData_t gpsLapTimerData;
+
+void gpsLapTimerInit(void)
+{
+    gpsLapTimerData.gateLocation.lat = gpsLapTimerConfig()->gateLat;
+    gpsLapTimerData.gateLocation.lon  = gpsLapTimerConfig()->gateLon;
+    gpsLapTimerData.numberOfSetReadings = 0;
+    gpsLapTimerData.bestLapTime = 0;
+    gpsLapTimerData.best3Consec = 0;
+    for (uint8_t i = 0; i < MAX_N_RECORDED_PREVIOUS_LAPS; i++) {
+        gpsLapTimerData.previousLaps[i] = 0;
+    }
+    gpsLapTimerData.timerRunning = false;
+    gpsLapTimerData.timeOfLastLap = 0;
+    gpsLapTimerData.numberOfLapsRecorded = 0;
+}
+
+void gpsLapTimerStartSetGate(void)
+{
+    settingGate = true;
+    gateSetLatReadings = 0;
+    gateSetLonReadings = 0;
+    gpsLapTimerData.numberOfSetReadings = 0;
+}
+
+void gpsLapTimerProcessSettingGate(void)
+{
+    if (gpsLapTimerData.numberOfSetReadings < MAX_GATE_SET_READINGS){
+        gateSetLatReadings += gpsSol.llh.lat;
+        gateSetLonReadings += gpsSol.llh.lon;
+        gpsLapTimerData.numberOfSetReadings++;
+    }
+    gpsLapTimerData.gateLocation.lat = gateSetLatReadings / gpsLapTimerData.numberOfSetReadings;
+    gpsLapTimerData.gateLocation.lon = gateSetLonReadings / gpsLapTimerData.numberOfSetReadings;
+}
+
+void gpsLapTimerEndSetGate(void)
+{
+    settingGate = false;
+    gpsLapTimerConfigMutable()->gateLat = gpsLapTimerData.gateLocation.lat;
+    gpsLapTimerConfigMutable()->gateLon = gpsLapTimerData.gateLocation.lon;
+}
+
+void gpsLapTimerNewGpsData(void)
+{
+    GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &gpsLapTimerData.gateLocation.lat, &gpsLapTimerData.gateLocation.lon, &gpsLapTimerData.distToPointCM, &gpsLapTimerData.dirToPoint);
+    
+    if (settingGate) {
+        gpsLapTimerProcessSettingGate();
+        return;
+    }
+
+    // Reset all lap timer data if reset mode is active
+    if (IS_RC_MODE_ACTIVE(BOXLAPTIMERRESET)) {
+        gpsLapTimerInit();
+        return;
+    }
+
+    // Current lap time is at least the min lap timer or timer not running, so we need to get ready to record a gate crossing
+    if (!gpsLapTimerData.timerRunning || gpsSol.time - gpsLapTimerData.timeOfLastLap > (gpsLapTimerConfig()->minimumLapTimeSeconds * 1000)) {
+        // Within radius of gate, record the closest point we get before leaving
+        if (gpsLapTimerData.distToPointCM < (gpsLapTimerConfig()->gateToleranceM * 100)) {
+            // Either just entered the circle or were already in circle but are the closest we've been to the center this lap
+            if (!wasInCircle || gpsLapTimerData.distToPointCM < minDistance) {
+                // Track the closest we've been to the center
+                minDistance = gpsLapTimerData.distToPointCM;
+                // Track the time we were the closest to the center, which will be used to determine the actual lap time
+                minDistanceTime = gpsSol.time;
+            }
+            wasInCircle = true;
+        } else {
+            // Just left the circle, so record the lap time
+            if (wasInCircle) {
+                // Not the first time through the gate
+                if (gpsLapTimerData.timerRunning) {
+                    uint32_t lapTime = minDistanceTime - gpsLapTimerData.timeOfLastLap;
+
+                    // Update best lap time
+                    if (gpsLapTimerData.numberOfLapsRecorded >= 1 && (lapTime < gpsLapTimerData.bestLapTime || gpsLapTimerData.bestLapTime == 0)) {
+                        gpsLapTimerData.bestLapTime = lapTime;
+                    }
+
+                    // Shift array of previously recorded laps and add latest
+                    for (unsigned i = MAX_N_RECORDED_PREVIOUS_LAPS - 1; i > 0; i--) {
+                        gpsLapTimerData.previousLaps[i] = gpsLapTimerData.previousLaps[i-1];
+                    }
+                    gpsLapTimerData.previousLaps[0] = lapTime;
+
+                    // Check if we're able to calculate a best N consec time yet, and add them up just in case
+                    uint32_t sumLastNLaps = 0;
+                    if (gpsLapTimerData.numberOfLapsRecorded >= MAX_N_RECORDED_PREVIOUS_LAPS) {
+                        for (int i = 0; i < MAX_N_RECORDED_PREVIOUS_LAPS; i++) {
+                            sumLastNLaps += gpsLapTimerData.previousLaps[i];
+                        }
+                    }
+
+                    // Check if this is better than the previous best
+                    if (sumLastNLaps > 0 && (sumLastNLaps < gpsLapTimerData.best3Consec || gpsLapTimerData.best3Consec == 0)) {
+                        gpsLapTimerData.best3Consec = sumLastNLaps;
+                    }
+                }
+                gpsLapTimerData.timeOfLastLap = minDistanceTime;
+                gpsLapTimerData.timerRunning = true;
+            }
+            wasInCircle = false;
+        }
+    }
+}

src/main/fc/gps_lap_timer.h

@@ -0,0 +1,49 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "io/gps.h"
+
+#include "pg/gps_lap_timer.h"
+
+#define MAX_N_RECORDED_PREVIOUS_LAPS 3
+
+typedef struct gpsLapTimerData_s {
+    gpsLocation_t gateLocation;
+    uint32_t previousLaps[MAX_N_RECORDED_PREVIOUS_LAPS];
+    uint32_t currentLapTime;
+    uint32_t bestLapTime;
+    uint32_t best3Consec;
+    uint32_t distToPointCM;
+    uint32_t timeOfLastLap;
+    int32_t  dirToPoint;
+    uint16_t numberOfSetReadings;
+    uint16_t numberOfLapsRecorded;
+    bool timerRunning;
+} gpsLapTimerData_t;
+
+extern gpsLapTimerData_t gpsLapTimerData;
+
+void gpsLapTimerInit(void);
+void gpsLapTimerNewGpsData(void);
+void gpsLapTimerStartSetGate(void);
+void gpsLapTimerEndSetGate(void);

src/main/fc/init.c

@@ -86,6 +86,7 @@
 
 #include "fc/board_info.h"
 #include "fc/dispatch.h"
+#include "fc/gps_lap_timer.h"
 #include "fc/init.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
@@ -758,6 +759,9 @@ void init(void)
 #ifdef USE_GPS_RESCUE
         gpsRescueInit();
 #endif
+#ifdef USE_GPS_LAP_TIMER
+        gpsLapTimerInit();
+#endif // USE_GPS_LAP_TIMER
     }
 #endif
 

src/main/fc/parameter_names.h

@@ -149,6 +149,7 @@
 #define PARAM_NAME_GPS_SET_HOME_POINT_ONCE "gps_set_home_point_once"
 #define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed"
 #define PARAM_NAME_GPS_NMEA_CUSTOM_COMMANDS "gps_nmea_custom_commands"
+#define PARAM_NAME_GPS_UPDATE_RATE_HZ "gps_update_rate_hz"
 
 #ifdef USE_GPS_RESCUE
 #define PARAM_NAME_GPS_RESCUE_MIN_START_DIST "gps_rescue_min_start_dist"
@@ -187,4 +188,11 @@
 #define PARAM_NAME_GPS_RESCUE_USE_MAG "gps_rescue_use_mag"
 #endif
 #endif
+
+#ifdef USE_GPS_LAP_TIMER
+#define PARAM_NAME_GPS_LAP_TIMER_GATE_LAT "gps_lap_timer_gate_lat"
+#define PARAM_NAME_GPS_LAP_TIMER_GATE_LON "gps_lap_timer_gate_lon"
+#define PARAM_NAME_GPS_LAP_TIMER_MIN_LAP_TIME "gps_lap_timer_min_lap_time_s"
+#define PARAM_NAME_GPS_LAP_TIMER_GATE_TOLERANCE "gps_lap_timer_gate_tolerance_m"
+#endif // USE_GPS_LAP_TIMER
 #endif

src/main/fc/rc_modes.h

@@ -78,6 +78,7 @@ typedef enum {
     BOXSTICKCOMMANDDISABLE,
     BOXBEEPERMUTE,
     BOXREADY,
+    BOXLAPTIMERRESET,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/io/gps.c

@@ -45,6 +45,8 @@
 #include "io/serial.h"
 
 #include "config/config.h"
+
+#include "fc/gps_lap_timer.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"
@@ -577,7 +579,12 @@ static void ubloxSetMessageRate(uint8_t messageClass, uint8_t messageID, uint8_t
 static void ubloxSetNavRate(uint16_t measRate, uint16_t navRate, uint16_t timeRef)
 {
     ubxMessage_t tx_buffer;
-    tx_buffer.payload.cfg_rate.measRate = measRate;
+
+    uint16_t measRateMilliseconds = 1000 / measRate;
+    // Testing has  revealed this is the max rate common modules can achieve
+    if (measRateMilliseconds < 53) measRateMilliseconds = 53;
+
+    tx_buffer.payload.cfg_rate.measRate = measRateMilliseconds;
     tx_buffer.payload.cfg_rate.navRate = navRate;
     tx_buffer.payload.cfg_rate.timeRef = timeRef;
     ubloxSendConfigMessage(&tx_buffer, MSG_CFG_RATE, sizeof(ubxCfgRate_t));
@@ -743,7 +750,7 @@ void gpsInitUblox(void)
                         ubloxSetMessageRate(CLASS_NAV, MSG_DOP, 1); // set DOP MSG rate
                         break;
                     case UBLOX_SET_NAV_RATE:
-                        ubloxSetNavRate(0x64, 1, 1); // set rate to 10Hz (measurement period: 100ms, navigation rate: 1 cycle) (for 5Hz use 0xC8)
+                        ubloxSetNavRate(gpsConfig()->gps_update_rate_hz, 1, 1);
                         break;
                     case UBLOX_SET_SBAS:
                         ubloxSetSbas();
@@ -1579,6 +1586,7 @@ static bool UBLOX_parse_gps(void)
         gpsSol.llh.lon = _buffer.posllh.longitude;
         gpsSol.llh.lat = _buffer.posllh.latitude;
         gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10;  //alt in cm
+        gpsSol.time = _buffer.posllh.time;
         gpsSetFixState(next_fix);
         _new_position = true;
         break;
@@ -1999,6 +2007,9 @@ void onGpsNewData(void)
 #ifdef USE_GPS_RESCUE
     gpsRescueNewGpsData();
 #endif
+#ifdef USE_GPS_LAP_TIMER
+    gpsLapTimerNewGpsData();
+#endif // USE_GPS_LAP_TIMER
 }
 
 void gpsSetFixState(bool state)

src/main/io/gps.h

@@ -129,6 +129,7 @@ typedef struct gpsSolutionData_s {
     uint16_t groundSpeed;           // speed in 0.1m/s
     uint16_t groundCourse;          // degrees * 10
     uint8_t numSat;
+    uint32_t time;                  // GPS msToW
 } gpsSolutionData_t;
 
 typedef struct gpsData_s {

src/main/msp/msp_box.c

@@ -100,7 +100,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { .boxId = BOXMSPOVERRIDE, .boxName = "MSP OVERRIDE", .permanentId = 50},
     { .boxId = BOXSTICKCOMMANDDISABLE, .boxName = "STICK COMMANDS DISABLE", .permanentId = 51},
     { .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 52},
-    { .boxId = BOXREADY, .boxName = "READY", .permanentId = 53}
+    { .boxId = BOXREADY, .boxName = "READY", .permanentId = 53},
+    { .boxId = BOXLAPTIMERRESET, .boxName = "LAP TIMER RESET", .permanentId = 54},
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -338,6 +339,10 @@ void initActiveBoxIds(void)
     BME(BOXSTICKCOMMANDDISABLE);
     BME(BOXREADY);
 
+#if defined(USE_GPS_LAP_TIMER)
+    BME(BOXLAPTIMERRESET);
+#endif
+
 #undef BME
     // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
     for (boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)

src/main/osd/osd.c

@@ -61,6 +61,7 @@
 #include "drivers/time.h"
 
 #include "fc/core.h"
+#include "fc/gps_lap_timer.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -193,6 +194,8 @@ const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
     OSD_STAT_TOTAL_TIME,
     OSD_STAT_TOTAL_DIST,
     OSD_STAT_WATT_HOURS_DRAWN,
+    OSD_STAT_BEST_3_CONSEC_LAPS,
+    OSD_STAT_BEST_LAP,
 };
 
 // Group elements in a number of groups to reduce task scheduling overhead
@@ -545,6 +548,19 @@ void osdInit(displayPort_t *osdDisplayPortToUse, osdDisplayPortDevice_e displayP
     }
 }
 
+#ifdef USE_GPS_LAP_TIMER
+void printLapTime(char *buffer, const uint32_t timeMs) {
+    if (timeMs != 0) {
+        const uint32_t timeRoundMs = timeMs + 5; // round value in division by 10
+        const int timeSeconds = timeRoundMs / 1000;
+        const int timeDecimals = (timeRoundMs % 1000) / 10;
+        tfp_sprintf(buffer, "%3u.%02u", timeSeconds, timeDecimals);
+    } else {
+        tfp_sprintf(buffer, "  -.--");
+    }
+}
+#endif // USE_GPS_LAP_TIMER
+
 static void osdResetStats(void)
 {
     stats.max_current     = 0;
@@ -951,6 +967,20 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
         return true;
 #endif
 
+#ifdef USE_GPS_LAP_TIMER
+    case OSD_STAT_BEST_3_CONSEC_LAPS: {
+        printLapTime(buff, gpsLapTimerData.best3Consec);
+        osdDisplayStatisticLabel(midCol, displayRow, "BEST 3 CON", buff);
+        return true;
+    }
+
+    case OSD_STAT_BEST_LAP: {
+        printLapTime(buff, gpsLapTimerData.bestLapTime);
+        osdDisplayStatisticLabel(midCol, displayRow, "BEST LAP", buff);
+        return true;
+    }
+#endif // USE_GPS_LAP_TIMER
+
 #ifdef USE_PERSISTENT_STATS
     case OSD_STAT_TOTAL_FLIGHTS:
         itoa(statsConfig()->stats_total_flights, buff, 10);

src/main/osd/osd.h

@@ -186,6 +186,9 @@ typedef enum {
     OSD_SYS_WARNINGS,
     OSD_SYS_VTX_TEMP,
     OSD_SYS_FAN_SPEED,
+    OSD_GPS_LAP_TIME_CURRENT,
+    OSD_GPS_LAP_TIME_PREVIOUS,
+    OSD_GPS_LAP_TIME_BEST3,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -227,6 +230,8 @@ typedef enum {
     OSD_STAT_MIN_RSSI_DBM,
     OSD_STAT_WATT_HOURS_DRAWN,
     OSD_STAT_MIN_RSNR,
+    OSD_STAT_BEST_3_CONSEC_LAPS,
+    OSD_STAT_BEST_LAP,
     OSD_STAT_COUNT // MUST BE LAST
 } osd_stats_e;
 

src/main/osd/osd_elements.c

@@ -138,6 +138,7 @@
 
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
+#include "fc/gps_lap_timer.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
@@ -234,6 +235,9 @@ static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32];
 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 #define BLINK(item) (IS_BLINK(item) && blinkState)
 
+// Return whether element is a SYS element and needs special handling
+#define IS_SYS_OSD_ELEMENT(item) (item >= OSD_SYS_GOGGLE_VOLTAGE) && (item <= OSD_SYS_FAN_SPEED)
+
 enum {UP, DOWN};
 
 static int osdDisplayWrite(osdElementParms_t *element, uint8_t x, uint8_t y, uint8_t attr, const char *s)
@@ -1040,8 +1044,14 @@ static void osdElementGpsHomeDirection(osdElementParms_t *element)
 {
     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
         if (GPS_distanceToHome > 0) {
-            const int h = DECIDEGREES_TO_DEGREES(GPS_directionToHome - attitude.values.yaw);
+            int direction = GPS_directionToHome;
-            element->buff[0] = osdGetDirectionSymbolFromHeading(h);
+#ifdef USE_GPS_LAP_TIMER
+            // Override the "home" point to the start/finish location if the lap timer is running
+            if (gpsLapTimerData.timerRunning) {
+                direction = lrintf(gpsLapTimerData.dirToPoint * 0.1f); // Convert from centidegree to degree and round to nearest
+            }
+#endif
+            element->buff[0] = osdGetDirectionSymbolFromHeading(DECIDEGREES_TO_DEGREES(direction - attitude.values.yaw));
         } else {
             element->buff[0] = SYM_OVER_HOME;
         }
@@ -1057,7 +1067,14 @@ static void osdElementGpsHomeDirection(osdElementParms_t *element)
 static void osdElementGpsHomeDistance(osdElementParms_t *element)
 {
     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
-        osdFormatDistanceString(element->buff, GPS_distanceToHome, SYM_HOMEFLAG);
+        int distance = GPS_distanceToHome;
+#ifdef USE_GPS_LAP_TIMER
+        // Change the "home" point to the start/finish location if the lap timer is running
+        if (gpsLapTimerData.timerRunning) {
+            distance = lrintf(gpsLapTimerData.distToPointCM * 0.01f); // Round to nearest natural number
+        }
+#endif
+        osdFormatDistanceString(element->buff, distance, SYM_HOMEFLAG);
     } else {
         element->buff[0] = SYM_HOMEFLAG;
         // We use this symbol when we don't have a FIX
@@ -1129,6 +1146,35 @@ static void osdElementEfficiency(osdElementParms_t *element)
 }
 #endif // USE_GPS
 
+#ifdef USE_GPS_LAP_TIMER
+static void osdFormatLapTime(osdElementParms_t *element, uint32_t timeMs, uint8_t symbol)
+{
+    timeMs += 5;  // round to nearest centisecond (+/- 5ms)
+    uint32_t seconds = timeMs / 1000;
+    uint32_t decimals = (timeMs % 1000) / 10;
+    tfp_sprintf(element->buff, "%c%3u.%02u", symbol, seconds, decimals);
+}
+
+static void osdElementGpsLapTimeCurrent(osdElementParms_t *element)
+{
+    if (gpsLapTimerData.timerRunning) {
+        osdFormatLapTime(element, gpsSol.time - gpsLapTimerData.timeOfLastLap, SYM_TOTAL_DISTANCE);
+    } else {
+        osdFormatLapTime(element, 0, SYM_TOTAL_DISTANCE);
+    }
+}
+
+static void osdElementGpsLapTimePrevious(osdElementParms_t *element)
+{
+    osdFormatLapTime(element, gpsLapTimerData.previousLaps[0], SYM_PREV_LAP_TIME);
+}
+
+static void osdElementGpsLapTimeBest3(osdElementParms_t *element)
+{
+    osdFormatLapTime(element, gpsLapTimerData.best3Consec, SYM_CHECKERED_FLAG);
+}
+#endif // GPS_LAP_TIMER
+
 static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
 {
     // Draw AH sides
@@ -1852,6 +1898,11 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
 #ifdef USE_GPS
     [OSD_EFFICIENCY]              = osdElementEfficiency,
 #endif
+#ifdef USE_GPS_LAP_TIMER
+    [OSD_GPS_LAP_TIME_CURRENT]    = osdElementGpsLapTimeCurrent,
+    [OSD_GPS_LAP_TIME_PREVIOUS]   = osdElementGpsLapTimePrevious,
+    [OSD_GPS_LAP_TIME_BEST3]      = osdElementGpsLapTimeBest3,
+#endif // GPS_LAP_TIMER
 #ifdef USE_PERSISTENT_STATS
     [OSD_TOTAL_FLIGHTS]           = osdElementTotalFlights,
 #endif
@@ -1932,6 +1983,14 @@ void osdAddActiveElements(void)
     }
 #endif
 
+#ifdef USE_GPS_LAP_TIMER
+    if (sensors(SENSOR_GPS)) {
+        osdAddActiveElement(OSD_GPS_LAP_TIME_CURRENT);
+        osdAddActiveElement(OSD_GPS_LAP_TIME_PREVIOUS);
+        osdAddActiveElement(OSD_GPS_LAP_TIME_BEST3);
+    }
+#endif // GPS_LAP_TIMER
+
 #ifdef USE_PERSISTENT_STATS
     osdAddActiveElement(OSD_TOTAL_FLIGHTS);
 #endif
@@ -1962,7 +2021,7 @@ static void osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
     element.attr = DISPLAYPORT_SEVERITY_NORMAL;
 
     // Call the element drawing function
-    if ((item >= OSD_SYS_GOGGLE_VOLTAGE) && (item < OSD_ITEM_COUNT)) {
+    if (IS_SYS_OSD_ELEMENT(item)) {
         displaySys(osdDisplayPort, elemPosX, elemPosY, (displayPortSystemElement_e)(item - OSD_SYS_GOGGLE_VOLTAGE + DISPLAYPORT_SYS_GOGGLE_VOLTAGE));
     } else {
         osdElementDrawFunction[item](&element);

src/main/pg/gps.c

@@ -38,6 +38,7 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
     .autoBaud = GPS_AUTOBAUD_OFF,
     .gps_ublox_acquire_model = UBLOX_MODEL_STATIONARY,
     .gps_ublox_flight_model = UBLOX_MODEL_AIRBORNE_4G,
+    .gps_update_rate_hz = 10,
     .gps_ublox_use_galileo = false,
     .gps_ublox_full_power = true,
     .gps_set_home_point_once = false,

src/main/pg/gps.h

@@ -34,6 +34,7 @@ typedef struct gpsConfig_s {
     uint8_t autoBaud;
     uint8_t gps_ublox_acquire_model;
     uint8_t gps_ublox_flight_model;
+    uint8_t gps_update_rate_hz;
     bool gps_ublox_use_galileo;
     bool gps_ublox_full_power;
     bool gps_set_home_point_once;

src/main/pg/gps_lap_timer.c

@@ -0,0 +1,40 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_GPS_LAP_TIMER
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "gps_lap_timer.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gpsLapTimerConfig_t, gpsLapTimerConfig, PG_GPS_LAP_TIMER, 1);
+
+PG_RESET_TEMPLATE(gpsLapTimerConfig_t, gpsLapTimerConfig,
+    .gateLat = 0,
+    .gateLon = 0,
+    .minimumLapTimeSeconds = 10,
+    .gateToleranceM = 9,
+);
+
+#endif // USE_GPS_LAP_TIMER

src/main/pg/gps_lap_timer.h

@@ -0,0 +1,35 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "pg/pg.h"
+
+typedef struct gpsLapTimerConfig_s {
+    int32_t gateLat;
+    int32_t gateLon;
+    uint16_t minimumLapTimeSeconds;
+    uint8_t gateToleranceM;
+} gpsLapTimerConfig_t;
+
+PG_DECLARE(gpsLapTimerConfig_t, gpsLapTimerConfig);

src/main/pg/pg_ids.h

@@ -81,6 +81,7 @@
 #define PG_GPS_RESCUE 55 // struct OK
 #define PG_POSITION 56
 #define PG_VTX_IO_CONFIG 57
+#define PG_GPS_LAP_TIMER 58
 
 // Driver configuration
 #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight

src/main/target/common_post.h

@@ -579,4 +579,9 @@ extern uint8_t __config_end;
 
 #ifndef USE_GPS
 #undef USE_GPS_PLUS_CODES
+#undef USE_GPS_LAP_TIMER
+#endif
+
+#ifdef USE_GPS_LAP_TIMER
+#define USE_CMS_GPS_LAP_TIMER_MENU
 #endif