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
- Committer:
- cocorlow
- Date:
- 2021-12-06
- Revision:
- 139:b378528c05f2
- Parent:
- 135:49f8916588da
- Child:
- 140:53dbdb207542
File content as of revision 139:b378528c05f2:
//#include "global.hpp" // //void sendData2PC() //{ // sp.da = da; // sp.de = de; // sp.dT = dT; // sp.rpy[0] = rpy.x*180.0f/M_PI; // sp.rpy[1] = rpy.y*180.0f/M_PI; // sp.rpy[2] = rpy.z*180.0f/M_PI; // Matrix vihat = eskf.getVihat(); // sp.vihat[0] = vihat(1,1); // sp.vihat[1] = vihat(2,1); // sp.vihat[2] = vihat(3,1); // pc.Send(0000, &(sp)); //} // //void sendTelemetry() //{ // Matrix pihat = eskf.getPihat(); // Matrix vihat = eskf.getVihat(); // 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.Send(0000, &(tp)); // //} // //void writeSDcard() //{ // Matrix pihat = eskf.getPihat(); // Matrix vihat = eskf.getVihat(); // // lp.time = _t.read(); // lp.hertz = 1.0f/att_dt; // lp.gpsFix = float(gps.gpsFix); // 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); //}