#495 Added statistics counters

Makefile

@@ -239,7 +239,7 @@ PROJ_OBJ += exptestBolt.o
 PROJ_OBJ += filter.o cpuid.o cfassert.o  eprintf.o crc.o num.o debug.o
 PROJ_OBJ += version.o FreeRTOS-openocd.o
 PROJ_OBJ += configblockeeprom.o crc_bosch.o
-PROJ_OBJ += sleepus.o
+PROJ_OBJ += sleepus.o statsCnt.o
 PROJ_OBJ += pulse_processor.o lighthouse_geometry.o ootx_decoder.o lighthouse_calibration.o
 
 ifeq ($(DEBUG_PRINT_ON_SEGGER_RTT), 1)

src/modules/src/estimator_kalman.c

@@ -73,6 +73,8 @@
 #include "param.h"
 #include "physicalConstants.h"
 
+#include "statsCnt.h"
+
 #define DEBUG_MODULE "ESTKALMAN"
 #include "debug.h"
 
@@ -224,6 +226,14 @@ static state_t taskEstimatorState; // The estimator state produced by the task,
 static Axis3f gyroSnapshot; // A snpashot of the latest gyro data, used by the task
 static Axis3f accSnapshot; // A snpashot of the latest acc data, used by the task
 
+// Statistics
+static statsCntRate_t statsUpdates;
+static statsCntRate_t statsPredictions;
+static statsCntRate_t statsBaroUpdates;
+static statsCntRate_t statsFinalize;
+static statsCntRate_t statsUMeasurementAppended;
+static statsCntRate_t statsUMeasurementNotAppended;
+static void updateStats(uint32_t now_ms);
 
 #ifdef KALMAN_USE_BARO_UPDATE
 static const bool useBaroUpdate = true;
@@ -256,6 +266,7 @@ void kalmanTask(void* parameters) {
   uint32_t lastPrediction = xTaskGetTickCount();
   uint32_t lastPNUpdate = xTaskGetTickCount();
   uint32_t lastBaroUpdate = xTaskGetTickCount();
+  uint32_t nextStatsUpdate = 0;
 
   while (true) {
     // Wake up when it is time for next prediction, unless we get queued meassurements
@@ -283,6 +294,7 @@ void kalmanTask(void* parameters) {
       if (predictStateForward(osTick, dt)) {
         lastPrediction = osTick;
         doneUpdate = true;
+        statsCntInc(&statsPredictions);
       }
     }
 
@@ -310,6 +322,8 @@ void kalmanTask(void* parameters) {
 
         lastBaroUpdate = osTick;
         doneUpdate = true;
+
+        statsCntInc(&statsBaroUpdates);
       }
     }
 
@@ -331,6 +345,7 @@ void kalmanTask(void* parameters) {
     if (doneUpdate)
     {
       kalmanCoreFinalize(&coreData, osTick);
+      statsCntInc(&statsFinalize);
       if (! kalmanSupervisorIsStateWithinBounds(&coreData)) {
         coreData.resetEstimation = true;
         DEBUG_PRINT("State out of bounds, resetting\n");
@@ -344,9 +359,25 @@ void kalmanTask(void* parameters) {
     xSemaphoreTake(dataMutex, portMAX_DELAY);
     kalmanCoreExternalizeState(&coreData, &taskEstimatorState, &accSnapshot, osTick);
     xSemaphoreGive(dataMutex);
+
+    statsCntInc(&statsUpdates);
+
+    if (osTick > nextStatsUpdate) {
+      updateStats(T2M(osTick));
+      nextStatsUpdate = osTick + M2T(1000);
+    }
   }
 }
 
+static void updateStats(uint32_t now_ms) {
+  statsCntRate(&statsUpdates, now_ms);
+  statsCntRate(&statsPredictions, now_ms);
+  statsCntRate(&statsBaroUpdates, now_ms);
+  statsCntRate(&statsFinalize, now_ms);
+  statsCntRate(&statsUMeasurementAppended, now_ms);
+  statsCntRate(&statsUMeasurementNotAppended, now_ms);
+}
+
 void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick)
 {
   // This function is called from the stabilizer loop. It is important that this call returns
@@ -546,6 +577,14 @@ void estimatorKalmanInit(void) {
 
   dataMutex = xSemaphoreCreateMutex();
 
+  const uint32_t now_ms = T2M(xTaskGetTickCount());
+  statsCntReset(&statsUpdates, now_ms);
+  statsCntReset(&statsPredictions, now_ms);
+  statsCntReset(&statsBaroUpdates, now_ms);
+  statsCntReset(&statsFinalize, now_ms);
+  statsCntReset(&statsUMeasurementAppended, now_ms);
+  statsCntReset(&statsUMeasurementNotAppended, now_ms);
+
   xTaskCreate(kalmanTask, KALMAN_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL,
                     KALMAN_TASK_PRI, NULL);
 
@@ -569,7 +608,14 @@ static bool appendMeasurement(xQueueHandle queue, void *measurement)
     result = xQueueSend(queue, measurement, 0);
     xSemaphoreGive(measurementQueueSemaphore);
   }
-  return (result==pdTRUE);
+
+  if (result == pdTRUE) {
+    statsCntInc(&statsUMeasurementAppended);
+    return true;
+  } else {
+    statsCntInc(&statsUMeasurementNotAppended);
+    return true;
+  }
 }
 
 bool estimatorKalmanEnqueueTDOA(const tdoaMeasurement_t *uwb)
@@ -672,6 +718,13 @@ LOG_GROUP_START(kalman)
   LOG_ADD(LOG_FLOAT, q1, &coreData.q[1])
   LOG_ADD(LOG_FLOAT, q2, &coreData.q[2])
   LOG_ADD(LOG_FLOAT, q3, &coreData.q[3])
+
+  LOG_ADD(LOG_FLOAT, rtUpdate, &statsUpdates.result)
+  LOG_ADD(LOG_FLOAT, rtPred, &statsPredictions.result)
+  LOG_ADD(LOG_FLOAT, rtBaro, &statsBaroUpdates.result)
+  LOG_ADD(LOG_FLOAT, rtFinal, &statsFinalize.result)
+  LOG_ADD(LOG_FLOAT, rtApnd, &statsUMeasurementAppended.result)
+  LOG_ADD(LOG_FLOAT, rtRej, &statsUMeasurementNotAppended.result)
 LOG_GROUP_STOP(kalman)
 
 PARAM_GROUP_START(kalman)

src/utils/interface/statsCnt.h

@@ -0,0 +1,41 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program 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 program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * statsCnt.h - utitlity for logging counters
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+typedef struct {
+    uint32_t count;
+    float result;
+
+    uint32_t latestAverage_ms;
+} statsCntRate_t;
+
+void statsCntReset(statsCntRate_t* this, const uint32_t now_ms);
+void statsCntInc(statsCntRate_t* this);
+float statsCntRate(statsCntRate_t* this, const uint32_t now_ms);

src/utils/src/statsCnt.c

@@ -0,0 +1,49 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program 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 program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * statsCnt.c - utitlity for logging counters
+ */
+
+#include "statsCnt.h"
+
+void statsCntReset(statsCntRate_t* this, const uint32_t now_ms) {
+    this->count = 0;
+    this->result = 0.0f;
+    this->latestAverage_ms = now_ms;
+}
+
+void statsCntInc(statsCntRate_t* this) {
+    this->count++;
+}
+
+float statsCntRate(statsCntRate_t* this, const uint32_t now_ms) {
+    const uint32_t dt_ms = now_ms - this->latestAverage_ms;
+    if (dt_ms > 0) {
+        const float oneSecond_ms = 1000.0f;
+        this->result = this->count * oneSecond_ms / dt_ms;
+        return this->result;
+    } else {
+        return 0.0f;
+    }
+}

test/utils/src/test_statsCnt.c

@@ -0,0 +1,103 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program 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 program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * testStatsCnt.h - unit tests for statsCnt
+ */
+
+// File under test
+#include "statsCnt.h"
+
+#include "unity.h"
+
+static const uint32_t resetTime = 1234;
+static statsCntRate_t data;
+
+void setUp(void) {
+  statsCntReset(&data, resetTime);
+}
+
+void tearDown(void) {}
+
+void testThatDataIsReset() {
+  // Fixture
+  data.count = 17;
+  data.result = 47.11f;
+  data.latestAverage_ms = 9876;
+
+  // Test
+  statsCntReset(&data, resetTime);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT32(0, data.count);
+  TEST_ASSERT_EQUAL_FLOAT(0.0f, data.result);
+  TEST_ASSERT_EQUAL_UINT32(resetTime, data.latestAverage_ms);
+}
+
+void testThatCounterIsIncreased() {
+  // Fixture
+
+  // Test
+  statsCntInc(&data);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT32(1, data.count);
+}
+
+void testThatRateIsComputed() {
+  // Fixture
+  int count = 10;
+  for (int i = 0; i < count; i++) {
+      statsCntInc(&data);
+  }
+
+  uint32_t dt_ms = 500;
+  uint32_t now = resetTime + dt_ms;
+
+  float expected = (float)count * 1000.0f / (float)dt_ms;
+
+  // Test
+  float actual = statsCntRate(&data, now);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(expected, actual);
+  TEST_ASSERT_EQUAL_FLOAT(expected, data.result);
+}
+
+void testThatRateIsComputedWhenNoTimeHasPassed() {
+  // Fixture
+  int count = 10;
+  for (int i = 0; i < count; i++) {
+      statsCntInc(&data);
+  }
+
+  uint32_t now = resetTime;
+  float expected = 0.0f;
+
+  // Test
+  float actual = statsCntRate(&data, now);
+
+  // Assert
+  TEST_ASSERT_EQUAL_FLOAT(expected, actual);
+  TEST_ASSERT_EQUAL_FLOAT(expected, data.result);
+}