imu sync single core + code tidy + cpu total

rtlopez · Apr 20, 2024 · ca4b1e9 · ca4b1e9
1 parent bea9b68
commit ca4b1e9
Show file tree

Hide file tree

Showing 13 changed files with 41 additions and 72 deletions.
diff --git a/docs/calculations.ods b/docs/calculations.ods
diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
@@ -9,6 +9,7 @@
 #include "Logger.h"
 #include "Device/GyroDevice.h"
 #include "platform.h"
+#include "Hal/Pgm.h"
 
 #if defined(ESPFC_WIFI_ALT)
 #include <ESP8266WiFi.h>

diff --git a/lib/Hal/src/Hal.h → lib/Espfc/src/Hal/Pgm.h b/lib/Hal/src/Hal.h → lib/Espfc/src/Hal/Pgm.h
@@ -1,5 +1,4 @@
-#ifndef _HAL_H_
-#define _HAL_H_
+#pragma once
 
 #ifdef UNIT_TEST
 
@@ -20,7 +19,6 @@
 
 #endif // UNIT_TEST
 
-#undef max
-#undef min
+//#undef max
+//#undef min
 
-#endif // _HAL_H_
diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
@@ -3,9 +3,7 @@
 
 #include <cstddef>
 #include <cstdint>
-
 #include <EscDriver.h>
-#include <Hal.h>
 
 #include "Debug_Espfc.h"
 #include "ModelConfig.h"

diff --git a/lib/Espfc/src/Msp/Msp.h b/lib/Espfc/src/Msp/Msp.h
@@ -1,8 +1,8 @@
 #ifndef _ESPFC_MSP_MSP_H_
 #define _ESPFC_MSP_MSP_H_
 
-#include <Arduino.h>
-#include <Hal.h>
+#include <cstdint>
+#include "Hal/Pgm.h"
 
 extern "C" {
 #include "msp/msp_protocol.h"

diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp
@@ -59,15 +59,6 @@ int GyroSensor::begin()
   return 1;
 }
 
-int GyroSensor::update()
-{
-  int status = read();
-
-  if (status) filter();
-
-  return status;
-}
-
 int GyroSensor::read()
 {
   if (!_model.gyroActive()) return 0;

diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -22,7 +22,6 @@ class GyroSensor: public BaseSensor
     GyroSensor(Model& model);
 
     int begin();
-    int update() FAST_CODE_ATTR;
     int read() FAST_CODE_ATTR;
     int filter() FAST_CODE_ATTR;
     void postLoop();

diff --git a/lib/Espfc/src/SensorManager.cpp b/lib/Espfc/src/SensorManager.cpp
@@ -18,40 +18,27 @@ int SensorManager::begin()
 
 int SensorManager::read()
 {
-  int ret = 0;
   _gyro.read();
+
   if(_model.state.loopTimer.syncTo(_model.state.gyroTimer))
   {
-    ret = 1;
     _model.state.appQueue.send(Event(EVENT_GYRO_READ));
   }
 
-  int status = 0;
   if(_model.state.accelTimer.syncTo(_model.state.gyroTimer))
   {
-    status = _accel.update();
-    if(status)
-    {
-      _model.state.appQueue.send(Event(EVENT_ACCEL_READ));
-    }
+    _accel.update();
+    _model.state.appQueue.send(Event(EVENT_ACCEL_READ));
+    return 1;
   }
 
-  if (!status)
-  {
-    status = _mag.update();
-  }
+  if(_mag.update()) return 1;
 
-  if(!status)
-  {
-    status = _baro.update();
-  }
+  if(_baro.update()) return 1;
 
-  if(!status)
-  {
-    status = _voltage.update();
-  }
+  if(_voltage.update()) return 1;
 
-  return ret;
+  return 0;
 }
 
 int SensorManager::preLoop()
@@ -79,45 +66,40 @@ int SensorManager::fusion()
 // main task
 int SensorManager::update()
 {
-  int status = _gyro.update();
-
-  if(_model.state.gyroBiasSamples == 0)
-  {
-    _model.state.gyroBiasSamples = -1;
-    _fusion.restoreGain();
-  }
-
-  return status;
+  _gyro.read();
+  return preLoop();
 }
 
 // sub task
 int SensorManager::updateDelayed()
 {
   _gyro.postLoop();
-  int status = _accel.update();
+
+  // update at most one sensor besides gyro
+  int status = 0;
+  if(_model.state.accelTimer.syncTo(_model.state.gyroTimer))
+  {
+    _accel.update();
+    status = 1;
+  }
+
+  // delay imu update to next cycle
   if(_fusionUpdate)
   {
     _fusionUpdate = false;
     _fusion.update();
   }
-  _fusionUpdate = status; // update in next loop cycle
+  _fusionUpdate = status;
 
-  if(!status)
-  {
-    status = _mag.update();
-  }
+  if(status) return 1;
 
-  if(!status)
-  {
-    status = _baro.update();
-  }
+  if(_mag.update()) return 1;
 
-  if(!status)
-  {
-    _voltage.update();
-  }
+  if(_baro.update()) return 1;
+
+  if(_voltage.update()) return 0;
 
-  return status;
+  return 0;
 }
 
 }
diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h
@@ -154,8 +154,9 @@ class Stats
 
     float getCpuLoad() const
     {
-      float load = std::max(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1));
-      return load;
+      float maxLoad = std::max(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1));
+      float minLoad = std::min(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1));
+      return 0.7f * maxLoad + 0.3f * minLoad;
     }
 
     float getCpuTime() const

diff --git a/lib/Hal/library.json b/lib/Hal/library.json
diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp
@@ -1,7 +1,6 @@
 #include <unity.h>
 #include <ArduinoFake.h>
 #include <EscDriver.h>
-#include <Hal.h>
 #include <helper_3dmath.h>
 #include <Kalman.h>
 #include "msp/msp_protocol.h"

diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp
@@ -2,7 +2,6 @@
 #include <ArduinoFake.h>
 #include "Device/InputCRSF.h"
 #include <EscDriver.h>
-#include <Hal.h>
 #include <helper_3dmath.h>
 #include <Kalman.h>
 #include "msp/msp_protocol.h"

diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp
@@ -1,6 +1,5 @@
 #include <unity.h>
 #include <EscDriver.h>
-#include <Hal.h>
 #include <Kalman.h>
 #include <helper_3dmath.h>
 #include "msp/msp_protocol.h"
@@ -128,10 +127,15 @@ void test_math_bits_msb()
 
 void test_math_clock_align()
 {
+    TEST_ASSERT_EQUAL_INT( 250, Math::alignToClock(1000,  332));
+    TEST_ASSERT_EQUAL_INT( 333, Math::alignToClock(1000,  333));
+    TEST_ASSERT_EQUAL_INT( 333, Math::alignToClock(1000,  334));
     TEST_ASSERT_EQUAL_INT( 333, Math::alignToClock(1000,  400));
     TEST_ASSERT_EQUAL_INT( 500, Math::alignToClock(1000,  500));
     TEST_ASSERT_EQUAL_INT( 500, Math::alignToClock(1000,  800));
     TEST_ASSERT_EQUAL_INT(1000, Math::alignToClock(1000, 2000));
+    TEST_ASSERT_EQUAL_INT( 500, Math::alignToClock(8000,  500));
+    TEST_ASSERT_EQUAL_INT( 476, Math::alignToClock(6667,  500));
 }
 
 void test_math_peak_detect_full()