Merge pull request #143 from rtlopez/board-align

Board alignment
rtlopez · Jan 7, 2025 · aaa17a9 · aaa17a9
2 parents 15c09b2 + ddef9ce
commit aaa17a9
Show file tree

Hide file tree

Showing 15 changed files with 234 additions and 86 deletions.
diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml
@@ -33,7 +33,7 @@ jobs:
     runs-on: ubuntu-24.04
     strategy:
       matrix:
-        target: ['esp32', 'esp32s2', 'esp32s3', 'esp32c3', 'esp8266']
+        target: ['esp32', 'esp32s2', 'esp32s3', 'esp32c3', 'esp8266', 'rp2040']
     steps:
       - uses: actions/checkout@v4
       - name: Extract Version

diff --git a/lib/AHRS/src/helper_3dmath.h b/lib/AHRS/src/helper_3dmath.h
@@ -435,7 +435,55 @@ class VectorBase {
     }
 };
 
+template<typename T>
+class RotationMatrix
+{
+public:
+  RotationMatrix() = default;
+
+  void init(const VectorBase<T>& v)
+  {
+    const T cosx = cosf(v.x);
+    const T sinx = sinf(v.x);
+    const T cosy = cosf(v.y);
+    const T siny = sinf(v.y);
+    const T cosz = cosf(v.z);
+    const T sinz = sinf(v.z);
+
+    const T coszcosx = cosz * cosx;
+    const T sinzcosx = sinz * cosx;
+    const T coszsinx = sinx * cosz;
+    const T sinzsinx = sinx * sinz;
+
+    _m[0][0] = cosz * cosy;
+    _m[0][1] = -cosy * sinz;
+    _m[0][2] = siny;
+    _m[1][0] = sinzcosx + (coszsinx * siny);
+    _m[1][1] = coszcosx - (sinzsinx * siny);
+    _m[1][2] = -sinx * cosy;
+    _m[2][0] = (sinzsinx) - (coszcosx * siny);
+    _m[2][1] = (coszsinx) + (sinzcosx * siny);
+    _m[2][2] = cosy * cosx;
+  }
+
+  VectorBase<T> apply(const VectorBase<T>& v)
+  {
+    const T x = _m[0][0] * v.x + _m[1][0] * v.y + _m[2][0] * v.z;
+    const T y = _m[0][1] * v.x + _m[1][1] * v.y + _m[2][1] * v.z;
+    const T z = _m[0][2] * v.x + _m[1][2] * v.y + _m[2][2] * v.z;
+    return VectorBase<T>{x, y, z};
+  }
+
+private:
+  T _m[3][3] = {
+    { T{1}, T{0}, T{0} },
+    { T{0}, T{1}, T{0} },
+    { T{0}, T{0}, T{1} },
+  };
+};
+
 typedef VectorBase<float> VectorFloat;
 typedef VectorBase<int16_t> VectorInt16;
+typedef RotationMatrix<float> RotationMatrixFloat;
 
 #endif /* _HELPER_3DMATH_H_ */
diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
@@ -465,6 +465,10 @@ class Cli
         Param(PSTR("baro_lpf_type"), &c.baroFilter.type, filterTypeChoices),
         Param(PSTR("baro_lpf_freq"), &c.baroFilter.freq),
 
+        Param(PSTR("board_align_roll"), &c.boardAlignment[0]),
+        Param(PSTR("board_align_pitch"), &c.boardAlignment[1]),
+        Param(PSTR("board_align_yaw"), &c.boardAlignment[2]),
+
         Param(PSTR("vbat_source"), &c.vbatSource, voltageSourceChoices),
         Param(PSTR("vbat_scale"), &c.vbatScale),
         Param(PSTR("vbat_mul"), &c.vbatResMult),
@@ -1002,9 +1006,9 @@ class Cli
           s.print(_model.config.gyroBias[0]); s.print(' ');
           s.print(_model.config.gyroBias[1]); s.print(' ');
           s.print(_model.config.gyroBias[2]); s.print(F(" ["));
-          s.print(_model.state.gyroBias[0]); s.print(' ');
-          s.print(_model.state.gyroBias[1]); s.print(' ');
-          s.print(_model.state.gyroBias[2]); s.println(F("]"));
+          s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' ');
+          s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' ');
+          s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]"));
 
           s.print(F("accel offset: "));
           s.print(_model.config.accelBias[0]); s.print(' ');
@@ -1014,15 +1018,15 @@ class Cli
           s.print(_model.state.accelBias[1]); s.print(' ');
           s.print(_model.state.accelBias[2]); s.println(F("]"));
 
-          s.print(F(" mag offset: "));
+          s.print(F("  mag offset: "));
           s.print(_model.config.magCalibrationOffset[0]); s.print(' ');
           s.print(_model.config.magCalibrationOffset[1]); s.print(' ');
           s.print(_model.config.magCalibrationOffset[2]); s.print(F(" ["));
           s.print(_model.state.magCalibrationOffset[0]); s.print(' ');
           s.print(_model.state.magCalibrationOffset[1]); s.print(' ');
           s.print(_model.state.magCalibrationOffset[2]); s.println(F("]"));
 
-          s.print(F("  mag scale: "));
+          s.print(F("   mag scale: "));
           s.print(_model.config.magCalibrationScale[0]); s.print(' ');
           s.print(_model.config.magCalibrationScale[1]); s.print(' ');
           s.print(_model.config.magCalibrationScale[2]); s.print(F(" ["));

diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
@@ -441,6 +441,8 @@ class Model
         state.magTimer.setRate(state.magRate);
       }
 
+      state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2])));
+
       const uint32_t gyroPreFilterRate = state.gyroTimer.rate;
       const uint32_t gyroFilterRate = state.loopTimer.rate;
       const uint32_t inputFilterRate = state.inputTimer.rate;

diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
@@ -635,6 +635,8 @@ class ModelConfig
 
     uint8_t rescueConfigDelay = 30;
 
+    int16_t boardAlignment[3] = {0, 0, 0};
+
     ModelConfig()
     {
 #ifdef ESPFC_INPUT

diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
@@ -175,6 +175,8 @@ struct ModelState
   VectorFloat angle;
   Quaternion angleQ;
 
+  RotationMatrixFloat boardAlignment;
+
   Filter gyroFilter[3];
   Filter gyroFilter2[3];
   Filter gyroFilter3[3];

diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
@@ -716,9 +716,15 @@ class MspProcessor
           break;
 
         case MSP_BOARD_ALIGNMENT_CONFIG:
-          r.writeU16(0); // roll
-          r.writeU16(0); // pitch
-          r.writeU16(0); // yaw
+          r.writeU16(_model.config.boardAlignment[0]); // roll
+          r.writeU16(_model.config.boardAlignment[1]); // pitch
+          r.writeU16(_model.config.boardAlignment[2]); // yaw
+          break;
+
+        case MSP_SET_BOARD_ALIGNMENT_CONFIG:
+          _model.config.boardAlignment[0] = m.readU16();
+          _model.config.boardAlignment[1] = m.readU16();
+          _model.config.boardAlignment[2] = m.readU16();
           break;
 
         case MSP_RX_MAP:
@@ -1289,7 +1295,7 @@ class MspProcessor
         case MSP_RAW_IMU:
           for (int i = 0; i < 3; i++)
           {
-            r.writeU16(lrintf(_model.state.accel[i] * ACCEL_G_INV * 512.f));
+            r.writeU16(lrintf(_model.state.accel[i] * ACCEL_G_INV * 2048.f));
           }
           for (int i = 0; i < 3; i++)
           {

diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h
@@ -62,9 +62,11 @@ class AccelSensor: public BaseSensor
 
       Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER);
 
-      align(_model.state.accelRaw, _model.config.gyroAlign);
-
       _model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale;
+
+      align(_model.state.accel, _model.config.gyroAlign);
+      _model.state.accel = _model.state.boardAlignment.apply(_model.state.accel);
+
       for(size_t i = 0; i < 3; i++)
       {
         if(_model.config.debugMode == DEBUG_ACCELEROMETER)

diff --git a/lib/Espfc/src/Sensor/BaseSensor.cpp b/lib/Espfc/src/Sensor/BaseSensor.cpp
@@ -0,0 +1,70 @@
+#include "BaseSensor.h"
+#include "ModelConfig.h"
+#include "Utils/MemoryHelper.h"
+
+namespace Espfc {
+
+namespace Sensor {
+
+void FAST_CODE_ATTR BaseSensor::align(VectorFloat& dest, uint8_t rotation)
+{
+  const float x = dest.x;
+  const float y = dest.y;
+  const float z = dest.z;
+
+  switch(rotation)
+  {
+    default:
+    case ALIGN_CW0_DEG:
+      dest.x = x;
+      dest.y = y;
+      dest.z = z;
+      break;
+    case ALIGN_CW90_DEG:
+      dest.x = y;
+      dest.y = -x;
+      dest.z = z;
+      break;
+    case ALIGN_CW180_DEG:
+      dest.x = -x;
+      dest.y = -y;
+      dest.z = z;
+      break;
+    case ALIGN_CW270_DEG:
+      dest.x = -y;
+      dest.y = x;
+      dest.z = z;
+      break;
+    case ALIGN_CW0_DEG_FLIP:
+      dest.x = -x;
+      dest.y = y;
+      dest.z = -z;
+      break;
+    case ALIGN_CW90_DEG_FLIP:
+      dest.x = y;
+      dest.y = x;
+      dest.z = -z;
+      break;
+    case ALIGN_CW180_DEG_FLIP:
+      dest.x = x;
+      dest.y = -y;
+      dest.z = -z;
+      break;
+    case ALIGN_CW270_DEG_FLIP:
+      dest.x = -y;
+      dest.y = -x;
+      dest.z = -z;
+      break;
+  }
+}
+
+void FAST_CODE_ATTR BaseSensor::toVector(VectorInt16& v, uint8_t * buf)
+{
+  v.x = (int16_t)(((uint16_t)buf[0] << 8) | (uint16_t)buf[1]);
+  v.y = (int16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]);
+  v.z = (int16_t)(((uint16_t)buf[4] << 8) | (uint16_t)buf[5]);
+}
+
+}
+
+}
diff --git a/lib/Espfc/src/Sensor/BaseSensor.h b/lib/Espfc/src/Sensor/BaseSensor.h
@@ -1,79 +1,19 @@
-#ifndef _ESPFC_SENSOR_BASE_SENSOR_H_
-#define _ESPFC_SENSOR_BASE_SENSOR_H_
+#pragma once
 
-#include <cmath>
-#include "Model.h"
-#include "Hardware.h"
+#include <helper_3dmath.h>
+#include <cstdint>
 
 namespace Espfc {
 
 namespace Sensor {
 
 class BaseSensor
 {
-  public:
-    void FAST_CODE_ATTR align(VectorInt16& dest, uint8_t rotation)
-    {
-      const int16_t x = dest.x;
-      const int16_t y = dest.y;
-      const int16_t z = dest.z;
-
-      switch(rotation)
-      {
-        default:
-        case ALIGN_CW0_DEG:
-          dest.x = x;
-          dest.y = y;
-          dest.z = z;
-          break;
-        case ALIGN_CW90_DEG:
-          dest.x = y;
-          dest.y = -x;
-          dest.z = z;
-          break;
-        case ALIGN_CW180_DEG:
-          dest.x = -x;
-          dest.y = -y;
-          dest.z = z;
-          break;
-        case ALIGN_CW270_DEG:
-          dest.x = -y;
-          dest.y = x;
-          dest.z = z;
-          break;
-        case ALIGN_CW0_DEG_FLIP:
-          dest.x = -x;
-          dest.y = y;
-          dest.z = -z;
-          break;
-        case ALIGN_CW90_DEG_FLIP:
-          dest.x = y;
-          dest.y = x;
-          dest.z = -z;
-          break;
-        case ALIGN_CW180_DEG_FLIP:
-          dest.x = x;
-          dest.y = -y;
-          dest.z = -z;
-          break;
-        case ALIGN_CW270_DEG_FLIP:
-          dest.x = -y;
-          dest.y = -x;
-          dest.z = -z;
-          break;
-      }
-    }
-
-    void toVector(VectorInt16& v, uint8_t * buf)
-    {
-      v.x = (int16_t)(((uint16_t)buf[0] << 8) | (uint16_t)buf[1]);
-      v.y = (int16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]);
-      v.z = (int16_t)(((uint16_t)buf[4] << 8) | (uint16_t)buf[5]);
-    }
+public:
+  void align(VectorFloat& dest, uint8_t rotation);
+  void toVector(VectorInt16& v, uint8_t * buf);
 };
 
 }
 
 }
-
-#endif
diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp
@@ -1,6 +1,5 @@
 
 #include "GyroSensor.h"
-#include "Model.h"
 #include "Utils/FilterHelper.h"
 
 #define ESPFC_FUZZY_ACCEL_ZERO 0.05
@@ -22,7 +21,7 @@ int GyroSensor::begin()
 
   _gyro->setDLPFMode(_model.config.gyroDlpf);
   _gyro->setRate(_gyro->getRate());
-  _model.state.gyroScale = radians(2000.f) / 32768.f;
+  _model.state.gyroScale = Math::toRad(2000.f) / 32768.f;
 
   _model.state.gyroCalibrationState = CALIBRATION_START; // calibrate gyro on start
   _model.state.gyroCalibrationRate = _model.state.loopTimer.rate;
@@ -67,10 +66,11 @@ int FAST_CODE_ATTR GyroSensor::read()
 
   _gyro->readGyro(_model.state.gyroRaw);
 
-  align(_model.state.gyroRaw, _model.config.gyroAlign);
-
   VectorFloat input = static_cast<VectorFloat>(_model.state.gyroRaw) * _model.state.gyroScale;
 
+  align(input, _model.config.gyroAlign);
+  input = _model.state.boardAlignment.apply(input);
+
   if (_model.config.gyroFilter3.freq)
   {
     _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input);

diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -1,11 +1,13 @@
 #pragma once
 
 #include "BaseSensor.h"
+#include "Model.h"
 #include "Device/GyroDevice.h"
 #include "Math/Sma.h"
-#include "Math/FreqAnalyzer.h"
 #ifdef ESPFC_DSP
 #include "Math/FFTAnalyzer.h"
+#else
+#include "Math/FreqAnalyzer.h"
 #endif
 
 #define ESPFC_FUZZY_ACCEL_ZERO 0.05

diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h
@@ -58,9 +58,11 @@ class MagSensor: public BaseSensor
 
       Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER);
 
-      align(_model.state.magRaw, _model.config.magAlign);
       _model.state.mag = _mag->convert(_model.state.magRaw);
 
+      align(_model.state.mag, _model.config.magAlign);
+      _model.state.mag = _model.state.boardAlignment.apply(_model.state.mag);
+
       for(size_t i = 0; i < 3; i++)
       {
         _model.state.mag.set(i, _model.state.magFilter[i].update(_model.state.mag[i]));