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:
- 134:d57c6b2a706b
- Parent:
- 133:346ce20b3950
- Child:
- 135:49f8916588da
--- a/transferData.cpp Tue Nov 30 07:58:46 2021 +0000 +++ b/transferData.cpp Tue Nov 30 11:27:09 2021 +0000 @@ -19,22 +19,18 @@ { Matrix pihat = eskf.getPihat(); Matrix vihat = eskf.getVihat(); - Matrix accBias = eskf.getAccBias(); - Matrix gyroBias = eskf.getGyroBias(); - Matrix gravity = eskf.getGravity(); - //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]); + tp.time=_t.read(); + tp.hertz = 1.0f/att_dt; + tp.gpsFix = float(gps.gpsFix); + for(int i = 0;i<3;i++){ + tp.rpy[i] = euler(i+1,1)*180.0f/M_PI; + tp.pihat[i] = pihat(i+1,1); + tp.vihat[i] = vihat(i+1,1); + } + tp.dynaccNorm = sqrt(dynaccnorm2); - //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.Send(0000, &(tp)); - //twelite.printf("%d %f %f %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),gps.Longitude,gps.Latitude); - //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); - //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)); - //twelite.printf("%f %f %f %f %f %f\r\n",pi.x,pi.y,pi.z,vi.x,vi.y,vi.z); - //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); } void writeSDcard()