![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PmodNAV test
Dependencies: mbed LPS25HB_I2C LSM9DS1 UsaPack
main.cpp@2:20d44bbe80e4, 2021-11-05 (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Nov 05 13:49:27 2021 +0000
- Revision:
- 2:20d44bbe80e4
- Parent:
- 0:c99eaed54c50
mag calibration test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
osaka | 0:c99eaed54c50 | 1 | #include "mbed.h" |
osaka | 0:c99eaed54c50 | 2 | #include "LSM9DS1.h" |
osaka | 0:c99eaed54c50 | 3 | #include "LPS.h" |
NaotoMorita | 2:20d44bbe80e4 | 4 | #include "Matrix.h" |
NaotoMorita | 2:20d44bbe80e4 | 5 | #include <cmath> |
NaotoMorita | 2:20d44bbe80e4 | 6 | #include "MatrixMath.h" |
osaka | 0:c99eaed54c50 | 7 | |
NaotoMorita | 2:20d44bbe80e4 | 8 | Serial pc(USBTX, USBRX, 115200); |
NaotoMorita | 2:20d44bbe80e4 | 9 | I2C i2c(PB_9, PB_8); |
osaka | 0:c99eaed54c50 | 10 | LSM9DS1 lsm(i2c); |
osaka | 0:c99eaed54c50 | 11 | LPS lps(i2c); |
osaka | 0:c99eaed54c50 | 12 | |
NaotoMorita | 2:20d44bbe80e4 | 13 | Matrix K(3,3); |
NaotoMorita | 2:20d44bbe80e4 | 14 | Matrix beta(3,1); |
NaotoMorita | 2:20d44bbe80e4 | 15 | float R; |
NaotoMorita | 2:20d44bbe80e4 | 16 | Matrix gamma(10,1); |
NaotoMorita | 2:20d44bbe80e4 | 17 | Matrix jac(10,1); |
NaotoMorita | 2:20d44bbe80e4 | 18 | float b; |
NaotoMorita | 2:20d44bbe80e4 | 19 | |
NaotoMorita | 2:20d44bbe80e4 | 20 | void setBetaK() |
NaotoMorita | 2:20d44bbe80e4 | 21 | { |
NaotoMorita | 2:20d44bbe80e4 | 22 | K(1,1) = gamma(1,1); |
NaotoMorita | 2:20d44bbe80e4 | 23 | K(1,2) = gamma(2,1); |
NaotoMorita | 2:20d44bbe80e4 | 24 | K(2,1) = gamma(2,1); |
NaotoMorita | 2:20d44bbe80e4 | 25 | K(1,3) = gamma(3,1); |
NaotoMorita | 2:20d44bbe80e4 | 26 | K(3,1) = gamma(3,1); |
NaotoMorita | 2:20d44bbe80e4 | 27 | K(2,2) = gamma(4,1); |
NaotoMorita | 2:20d44bbe80e4 | 28 | K(2,3) = gamma(5,1); |
NaotoMorita | 2:20d44bbe80e4 | 29 | K(3,2) = gamma(5,1); |
NaotoMorita | 2:20d44bbe80e4 | 30 | K(3,3) = gamma(6,1); |
NaotoMorita | 2:20d44bbe80e4 | 31 | beta(1,1) = gamma(7,1); |
NaotoMorita | 2:20d44bbe80e4 | 32 | beta(2,1) = gamma(8,1); |
NaotoMorita | 2:20d44bbe80e4 | 33 | beta(3,1) = gamma(9,1); |
NaotoMorita | 2:20d44bbe80e4 | 34 | R = gamma(10,1); |
NaotoMorita | 2:20d44bbe80e4 | 35 | } |
NaotoMorita | 2:20d44bbe80e4 | 36 | |
NaotoMorita | 2:20d44bbe80e4 | 37 | void calcMagCalUpdate(Matrix H) |
NaotoMorita | 2:20d44bbe80e4 | 38 | { |
NaotoMorita | 2:20d44bbe80e4 | 39 | float A = K(1,1)*(H(1,1)+beta(1,1)) + K(1,2)*(H(2,1)+beta(2,1)) + K(1,3)*(H(3,1)+beta(3,1)) ; |
NaotoMorita | 2:20d44bbe80e4 | 40 | float B = K(2,1)*(H(1,1)+beta(1,1)) + K(2,2)*(H(2,1)+beta(2,1)) + K(2,3)*(H(3,1)+beta(3,1)) ; |
NaotoMorita | 2:20d44bbe80e4 | 41 | float C = K(3,1)*(H(1,1)+beta(1,1)) + K(3,2)*(H(2,1)+beta(2,1)) + K(3,3)*(H(3,1)+beta(3,1)) ; |
NaotoMorita | 2:20d44bbe80e4 | 42 | Matrix bvec = K * (H+beta); |
NaotoMorita | 2:20d44bbe80e4 | 43 | b = sqrt(bvec(1,1)*bvec(1,1)+bvec(2,1)*bvec(2,1)+bvec(3,1)*bvec(3,1)); |
NaotoMorita | 2:20d44bbe80e4 | 44 | |
NaotoMorita | 2:20d44bbe80e4 | 45 | jac(1,1) = - (H(1,1) + beta(1,1))*A/b; |
NaotoMorita | 2:20d44bbe80e4 | 46 | jac(2,1) = - (H(2,1) + beta(2,1))*B/b; |
NaotoMorita | 2:20d44bbe80e4 | 47 | jac(3,1) = - (H(3,1) + beta(3,1))*C/b; |
NaotoMorita | 2:20d44bbe80e4 | 48 | jac(4,1) = - ((H(2,1) + beta(2,1))*A+(H(1,1) + beta(1,1))*B)/b; |
NaotoMorita | 2:20d44bbe80e4 | 49 | jac(5,1) = - ((H(3,1) + beta(3,1))*A+(H(1,1) + beta(1,1))*C)/b; |
NaotoMorita | 2:20d44bbe80e4 | 50 | jac(6,1) = - ((H(3,1) + beta(3,1))*B+(H(2,1) + beta(2,1))*C)/b; |
NaotoMorita | 2:20d44bbe80e4 | 51 | |
NaotoMorita | 2:20d44bbe80e4 | 52 | jac(7,1) = - (K(1,1)*(H(1,1) + beta(1,1))+K(1,2)*(H(2,1) + beta(2,1)) + K(1,3)*(H(3,1) + beta(3,1)))/b; |
NaotoMorita | 2:20d44bbe80e4 | 53 | jac(8,1) = - (K(2,1)*(H(1,1) + beta(1,1))+K(2,2)*(H(2,1) + beta(2,1)) + K(2,3)*(H(3,1) + beta(3,1)))/b; |
NaotoMorita | 2:20d44bbe80e4 | 54 | jac(9,1) = - (K(3,1)*(H(1,1) + beta(1,1))+K(3,2)*(H(2,1) + beta(2,1)) + K(3,3)*(H(3,1) + beta(3,1)))/b; |
NaotoMorita | 2:20d44bbe80e4 | 55 | jac(10,1) = 1.0f; |
NaotoMorita | 2:20d44bbe80e4 | 56 | |
NaotoMorita | 2:20d44bbe80e4 | 57 | gamma = gamma - 0.001f*jac*(R-b); |
NaotoMorita | 2:20d44bbe80e4 | 58 | //gamma = gamma - 0.001f*jac; |
NaotoMorita | 2:20d44bbe80e4 | 59 | |
NaotoMorita | 2:20d44bbe80e4 | 60 | } |
osaka | 0:c99eaed54c50 | 61 | |
osaka | 0:c99eaed54c50 | 62 | int main() |
osaka | 0:c99eaed54c50 | 63 | { |
osaka | 0:c99eaed54c50 | 64 | uint16_t reg = lsm.begin(); |
osaka | 0:c99eaed54c50 | 65 | //printf("%x\n", reg); |
osaka | 0:c99eaed54c50 | 66 | if (!lps.init()){ |
osaka | 0:c99eaed54c50 | 67 | printf("Failed to autodetect pressure sensor!\r\n"); |
osaka | 0:c99eaed54c50 | 68 | while (1); |
osaka | 0:c99eaed54c50 | 69 | } |
osaka | 0:c99eaed54c50 | 70 | lps.enableDefault(); |
NaotoMorita | 2:20d44bbe80e4 | 71 | |
NaotoMorita | 2:20d44bbe80e4 | 72 | //初期値 |
NaotoMorita | 2:20d44bbe80e4 | 73 | R = 1.0f; |
NaotoMorita | 2:20d44bbe80e4 | 74 | gamma(1,1) = 1.0f; |
NaotoMorita | 2:20d44bbe80e4 | 75 | gamma(4,1) = 1.0f; |
NaotoMorita | 2:20d44bbe80e4 | 76 | gamma(6,1) = 1.0f; |
NaotoMorita | 2:20d44bbe80e4 | 77 | setBetaK(); |
NaotoMorita | 2:20d44bbe80e4 | 78 | |
osaka | 0:c99eaed54c50 | 79 | wait_ms(100); |
osaka | 0:c99eaed54c50 | 80 | while(true) |
osaka | 0:c99eaed54c50 | 81 | { |
osaka | 0:c99eaed54c50 | 82 | lsm.readAccel(); |
osaka | 0:c99eaed54c50 | 83 | lsm.readMag(); |
osaka | 0:c99eaed54c50 | 84 | lsm.readGyro(); |
NaotoMorita | 2:20d44bbe80e4 | 85 | //printf("%f %f %f %f %f %f %f %f %f\r\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz); |
NaotoMorita | 2:20d44bbe80e4 | 86 | //printf("%f %f %f\r\n", lsm.gx, lsm.gy, lsm.gz); |
NaotoMorita | 2:20d44bbe80e4 | 87 | //printf("%f %f %f\r\n", lsm.mx, lsm.my, lsm.mz); |
osaka | 0:c99eaed54c50 | 88 | float pressure = lps.readPressureMillibars(); |
osaka | 0:c99eaed54c50 | 89 | float altitude = lps.pressureToAltitudeMeters(pressure); |
osaka | 0:c99eaed54c50 | 90 | float temperature = lps.readTemperatureC(); |
NaotoMorita | 2:20d44bbe80e4 | 91 | Matrix h(3,1); |
NaotoMorita | 2:20d44bbe80e4 | 92 | h << lsm.mx << lsm.my << lsm.mz; |
NaotoMorita | 2:20d44bbe80e4 | 93 | Matrix hmod = K*(h+beta); |
NaotoMorita | 2:20d44bbe80e4 | 94 | float res = R*R-(hmod(1,1)*hmod(1,1)+hmod(2,1)*hmod(2,1)+hmod(3,1)*hmod(3,1)); |
NaotoMorita | 2:20d44bbe80e4 | 95 | if(abs(res) >0.01f){ |
NaotoMorita | 2:20d44bbe80e4 | 96 | calcMagCalUpdate(h); |
NaotoMorita | 2:20d44bbe80e4 | 97 | setBetaK(); |
NaotoMorita | 2:20d44bbe80e4 | 98 | } |
NaotoMorita | 2:20d44bbe80e4 | 99 | |
NaotoMorita | 2:20d44bbe80e4 | 100 | |
osaka | 0:c99eaed54c50 | 101 | //printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature); |
NaotoMorita | 2:20d44bbe80e4 | 102 | printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", hmod(1,1),hmod(2,1),hmod(3,1),res,gamma(1,1),gamma(2,1),gamma(3,1),gamma(4,1),gamma(5,1),gamma(6,1),gamma(7,1),gamma(8,1),gamma(9,1),gamma(10,1)); |
NaotoMorita | 2:20d44bbe80e4 | 103 | wait(0.005); |
osaka | 0:c99eaed54c50 | 104 | } |
osaka | 0:c99eaed54c50 | 105 | } |