solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: transferData.cpp
- Revision:
- 132:896ad37b534b
- Parent:
- 129:a76be8380bb2
- Child:
- 133:346ce20b3950
diff -r a76be8380bb2 -r 896ad37b534b transferData.cpp --- a/transferData.cpp Mon Nov 29 09:45:44 2021 +0000 +++ b/transferData.cpp Tue Nov 30 07:11:16 2021 +0000 @@ -22,12 +22,13 @@ Matrix accBias = eskf.getAccBias(); Matrix gyroBias = eskf.getGyroBias(); Matrix gravity = eskf.getGravity(); - magCalibrator.getExtremes(magbiasMin,magbiasMax); + //magCalibrator.getExtremes(magbiasMin,magbiasMax); //twelite.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); //twelite.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); //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); //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)); + //twelite.Send(0000, &(sp)); twelite.printf("%d %f %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,_t.read(),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),sqrt(dynaccnorm2)); //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,pi.x,pi.y,pi.z,vi.x,vi.y,vi.z); @@ -40,11 +41,39 @@ { Matrix pihat = eskf.getPihat(); Matrix vihat = eskf.getVihat(); - Matrix accBias = eskf.getAccBias(); - Matrix gyroBias = eskf.getGyroBias(); - Matrix gravity = eskf.getGravity(); - //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); - sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),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)); + + lp.time = _t.read(); + lp.hertz = 1.0f/att_dt; + lp.da = da; + lp.de = de; + lp.dT = dT; + for(int i = 0;i<16;i++){ + lp.rc[i] = rc[i]; + } + for(int i = 0;i<3;i++){ + lp.rpy[i] = euler(i+1,1); + lp.pihat[i] = pihat(i+1,1); + lp.vihat[i] = vihat(i+1,1); + } + lp.pi[0] = pi.x; + lp.pi[1] = pi.y; + lp.pi[2] = pi.z; + lp.vi[0] = vi.x; + lp.vi[1] = vi.y; + lp.vi[2] = vi.z; + lp.acc[0] = acc.x; + lp.acc[1] = acc.y; + lp.acc[2] = acc.z; + lp.gyro[0] = gyro.x; + lp.gyro[1] = gyro.y; + lp.gyro[2] = gyro.z; + lp.mag[0] = mag.x; + lp.mag[1] = mag.y; + lp.mag[2] = mag.z; + lp.palt = palt; //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); + //sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),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)); + sd.Send(0000, &(lp)); + //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); }