HAPSRG / Mbed 2 deprecated read_Pmod

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?

UserRevisionLine numberNew 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 }