diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index e754c8b..a8d3334 100644 --- 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 index 7838dd8..34f50c2 100644 --- a/lib/AHRS/src/helper_3dmath.h +++ b/lib/AHRS/src/helper_3dmath.h @@ -435,7 +435,55 @@ class VectorBase { } }; +template +class RotationMatrix +{ +public: + RotationMatrix() = default; + + void init(const VectorBase& 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 apply(const VectorBase& 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{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 VectorFloat; typedef VectorBase VectorInt16; +typedef RotationMatrix RotationMatrixFloat; #endif /* _HELPER_3DMATH_H_ */ diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 9b8a4cf..5bc36ec 100644 --- 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,7 +1018,7 @@ 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(" [")); @@ -1022,7 +1026,7 @@ class Cli 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 index 4dde78a..eb50498 100644 --- 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 index cf99a17..6e43772 100644 --- 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 index 08f7b80..496c381 100644 --- 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 index 0d402a6..1bb78a8 100644 --- 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 index 8b7db25..91221e9 100644 --- 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 new file mode 100644 index 0000000..479893d --- /dev/null +++ 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 index 863b30d..fe98aa8 100644 --- a/lib/Espfc/src/Sensor/BaseSensor.h +++ b/lib/Espfc/src/Sensor/BaseSensor.h @@ -1,9 +1,7 @@ -#ifndef _ESPFC_SENSOR_BASE_SENSOR_H_ -#define _ESPFC_SENSOR_BASE_SENSOR_H_ +#pragma once -#include -#include "Model.h" -#include "Hardware.h" +#include +#include namespace Espfc { @@ -11,69 +9,11 @@ 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 \ No newline at end of file diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 67815a3..b99f046 100644 --- 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(_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 index b5b1d4a..43bdcfc 100644 --- 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 index d183e54..99853d0 100644 --- 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])); diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index 8e3a7c9..54a060c 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -51,8 +51,8 @@ #define ESPFC_SPI_CS_BARO 11 #define ESPFC_I2C_0 -#define ESPFC_I2C_0_SDA -1 //12 //8 -#define ESPFC_I2C_0_SCL -1 //13 //9 +#define ESPFC_I2C_0_SDA 16 +#define ESPFC_I2C_0_SCL 17 #define ESPFC_BUZZER #define ESPFC_BUZZER_PIN -1 diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index e7e34fd..23c150c 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -1234,6 +1234,69 @@ void test_align_addr_to_write() TEST_ASSERT_EQUAL_UINT32(128, Math::alignAddressToWrite(100, 32, 16)); } +void test_rotation_matrix_no_rotation() +{ + VectorFloat v{1.f, 2.f, 3.f}; + RotationMatrixFloat rm; + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 2.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 3.f, r.z); +} + +void test_rotation_matrix_90_roll() +{ + VectorFloat v{0.f, 0.f, 1.f}; + RotationMatrixFloat rm; + rm.init(VectorFloat{ + Math::toRad(90), + Math::toRad(0), + Math::toRad(0), + }); + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.z); +} + +void test_rotation_matrix_90_pitch() +{ + VectorFloat v{0.f, 0.f, 1.f}; + RotationMatrixFloat rm; + rm.init(VectorFloat{ + Math::toRad(0), + Math::toRad(90), + Math::toRad(0), + }); + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, -1.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.z); +} + +void test_rotation_matrix_90_yaw() +{ + VectorFloat v{1.f, 2.f, 3.f}; + RotationMatrixFloat rm; + rm.init(VectorFloat{ + Math::toRad(0), + Math::toRad(0), + Math::toRad(90), + }); + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 2.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, -1.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 3.f, r.z); +} + int main(int argc, char **argv) { UNITY_BEGIN(); @@ -1306,5 +1369,10 @@ int main(int argc, char **argv) RUN_TEST(test_ring_buf2); RUN_TEST(test_align_addr_to_write); + RUN_TEST(test_rotation_matrix_no_rotation); + RUN_TEST(test_rotation_matrix_90_roll); + RUN_TEST(test_rotation_matrix_90_pitch); + RUN_TEST(test_rotation_matrix_90_yaw); + return UNITY_END(); } \ No newline at end of file