Implement Stopwatch (#12623)

* Implement stopwatch to measure time periods

* Add float version of stopwatchGetMicros()

make/source.mk

@@ -288,6 +288,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             common/filter.c \
             common/maths.c \
             common/sdft.c \
+            common/stopwatch.c \
             common/typeconversion.c \
             drivers/accgyro/accgyro_mpu.c \
             drivers/accgyro/accgyro_mpu3050.c \

src/main/common/stopwatch.c

@@ -0,0 +1,79 @@
+/*
+ * 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"
+
+#include "common/time.h"
+
+#include "drivers/system.h"
+
+#include "stopwatch.h"
+
+void stopwatchInit(stopwatch_t *watch)
+{
+    watch->start = 0;
+    watch->stop = 0;
+    stopwatchReset(watch);
+}
+
+void stopwatchReset(stopwatch_t *watch)
+{
+    watch->isRunning = false;
+    watch->elapsed = 0;
+}
+
+uint32_t stopwatchStart(stopwatch_t *watch)
+{
+    if (watch->isRunning == false) {
+        watch->isRunning = true;
+        watch->start = getCycleCounter();
+    }
+    return watch->start;
+}
+
+uint32_t stopwatchStop(stopwatch_t *watch)
+{
+    if (watch->isRunning == true) {
+        watch->stop = getCycleCounter();
+        watch->elapsed += cmpTimeCycles(watch->stop, watch->start);
+        watch->isRunning = false;
+    }
+    return watch->stop;
+}
+
+uint32_t stopwatchGetCycles(stopwatch_t *watch)
+{
+    if (watch->isRunning == true) {
+        return watch->elapsed + cmpTimeCycles(getCycleCounter(), watch->start);
+    } else {
+        return watch->elapsed;
+    }
+}
+
+uint32_t stopwatchGetMicros(stopwatch_t *watch)
+{
+    return clockCyclesToMicros(stopwatchGetCycles(watch));
+}
+
+float stopwatchGetMicrosf(stopwatch_t *watch)
+{
+    return clockCyclesToMicrosf(stopwatchGetCycles(watch));
+}

src/main/common/stopwatch.h

@@ -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/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct stopwatch_s {
+    bool isRunning;
+    uint32_t start;
+    uint32_t stop;
+    uint32_t elapsed;
+} stopwatch_t;
+
+void stopwatchInit(stopwatch_t *watch);
+void stopwatchReset(stopwatch_t *watch);
+uint32_t stopwatchStart(stopwatch_t *watch);
+uint32_t stopwatchStop(stopwatch_t *watch);
+uint32_t stopwatchGetCycles(stopwatch_t *watch);
+uint32_t stopwatchGetMicros(stopwatch_t *watch);
+float stopwatchGetMicrosf(stopwatch_t *watch);

src/main/drivers/system.c

@@ -46,6 +46,7 @@
 
 // cycles per microsecond
 static uint32_t usTicks = 0;
+static float usTicksInv = 0.0f;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
 static volatile uint32_t sysTickUptime = 0;
 static volatile uint32_t sysTickValStamp = 0;
@@ -67,6 +68,7 @@ void cycleCounterInit(void)
     cpuClockFrequency = clocks.SYSCLK_Frequency;
 #endif
     usTicks = cpuClockFrequency / 1000000;
+    usTicksInv = 1e6f / cpuClockFrequency;
 
     CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
 
@@ -162,6 +164,11 @@ int32_t clockCyclesToMicros(int32_t clockCycles)
     return clockCycles / usTicks;
 }
 
+float clockCyclesToMicrosf(int32_t clockCycles)
+{
+    return clockCycles * usTicksInv;
+}
+
 // Note that this conversion is signed as this is used for periods rather than absolute timestamps
 int32_t clockCyclesTo10thMicros(int32_t clockCycles)
 {

src/main/drivers/system.h

@@ -66,6 +66,7 @@ void systemResetToBootloader(bootloaderRequestType_e requestType);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
 int32_t clockCyclesToMicros(int32_t clockCycles);
+float clockCyclesToMicrosf(int32_t clockCycles);
 int32_t clockCyclesTo10thMicros(int32_t clockCycles);
 uint32_t clockMicrosToCycles(uint32_t micros);
 uint32_t getCycleCounter(void);