HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

transferData.cpp@127:d73a6233ee4b, 2021-11-22 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 22 09:51:51 2021 +0000
Revision:
127:d73a6233ee4b
Parent:
119:a21e283730d1
Child:
129:a76be8380bb2
MAG Fuse OK

Who changed what in which revision?

UserRevisionLine numberNew contents of line
osaka 87:89bbbcdb667b 1 #include "global.hpp"
osaka 87:89bbbcdb667b 2
osaka 87:89bbbcdb667b 3 void sendData2PC()
osaka 87:89bbbcdb667b 4 {
osaka 87:89bbbcdb667b 5 sp.da = da;
osaka 87:89bbbcdb667b 6 sp.de = de;
osaka 87:89bbbcdb667b 7 sp.dT = dT;
osaka 87:89bbbcdb667b 8 sp.rpy[0] = rpy.x*180.0f/M_PI;
osaka 87:89bbbcdb667b 9 sp.rpy[1] = rpy.y*180.0f/M_PI;
osaka 87:89bbbcdb667b 10 sp.rpy[2] = rpy.z*180.0f/M_PI;
NaotoMorita 93:b827f78a717a 11 Matrix vihat = eskf.getVihat();
NaotoMorita 93:b827f78a717a 12 sp.vihat[0] = vihat(1,1);
NaotoMorita 93:b827f78a717a 13 sp.vihat[1] = vihat(2,1);
NaotoMorita 93:b827f78a717a 14 sp.vihat[2] = vihat(3,1);
osaka 87:89bbbcdb667b 15 pc.Send(0000, &(sp));
osaka 87:89bbbcdb667b 16 }
osaka 87:89bbbcdb667b 17
osaka 87:89bbbcdb667b 18 void sendTelemetry()
osaka 87:89bbbcdb667b 19 {
NaotoMorita 93:b827f78a717a 20 Matrix pihat = eskf.getPihat();
NaotoMorita 93:b827f78a717a 21 Matrix vihat = eskf.getVihat();
NaotoMorita 93:b827f78a717a 22 Matrix accBias = eskf.getAccBias();
NaotoMorita 93:b827f78a717a 23 Matrix gyroBias = eskf.getGyroBias();
NaotoMorita 93:b827f78a717a 24 Matrix gravity = eskf.getGravity();
NaotoMorita 119:a21e283730d1 25 magCalibrator.getExtremes(magbiasMin,magbiasMax);
NaotoMorita 119:a21e283730d1 26 //twelite.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 119:a21e283730d1 27 //twelite.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 93:b827f78a717a 28
osaka 88:be349faa1976 29 //twelite.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
NaotoMorita 94:579e875a4244 30 //twelite.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1),accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1),gravity(1,1),gravity(2,1),gravity(3,1));
NaotoMorita 127:d73a6233ee4b 31 twelite.printf("%d %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1));
NaotoMorita 93:b827f78a717a 32 //twelite.printf("%f %f %f %f %f %f\r\n",accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1));
NaotoMorita 92:00460f6df439 33 //twelite.printf("%f %f %f %f %f %f\r\n",pi.x,pi.y,pi.z,vi.x,vi.y,vi.z);
NaotoMorita 102:1c77ff6e2a85 34 //twelite.printf("%f %f %f %f %f %f %f \r\n", magGamma(7,1), magGamma(8,1), magGamma(9,1), mag.x, mag.y, mag.z,magres);
osaka 87:89bbbcdb667b 35 }
osaka 87:89bbbcdb667b 36
osaka 87:89bbbcdb667b 37 void writeSDcard()
osaka 87:89bbbcdb667b 38 {
NaotoMorita 94:579e875a4244 39 Matrix pihat = eskf.getPihat();
NaotoMorita 94:579e875a4244 40 Matrix vihat = eskf.getVihat();
NaotoMorita 94:579e875a4244 41 Matrix accBias = eskf.getAccBias();
NaotoMorita 94:579e875a4244 42 Matrix gyroBias = eskf.getGyroBias();
NaotoMorita 94:579e875a4244 43 Matrix gravity = eskf.getGravity();
NaotoMorita 94:579e875a4244 44 //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
NaotoMorita 119:a21e283730d1 45 sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),1.0f/att_dt,da,de,dT,rc[0],rc[1],rc[2],rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1));
NaotoMorita 94:579e875a4244 46
osaka 88:be349faa1976 47 //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
osaka 87:89bbbcdb667b 48 }