Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: transferData.cpp
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
- Child:
- 141:725321fe2949
--- a/transferData.cpp Mon Dec 06 08:26:16 2021 +0000 +++ b/transferData.cpp Mon Dec 06 11:37:55 2021 +0000 @@ -1,76 +1,76 @@ -//#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); -//} +#include "global.hpp" + +void sendData2PC() +{ + sp.da = da; + sp.de = de; + sp.dT = dT; + sp.rpy[0] = rpy(0)*180.0f/M_PI_F; + sp.rpy[1] = rpy(1)*180.0f/M_PI_F; + sp.rpy[2] = rpy(2)*180.0f/M_PI_F; + Vector3f vihat = eskf.getVihat(); + sp.vihat[0] = vihat(0); + sp.vihat[1] = vihat(1); + sp.vihat[2] = vihat(2); + pc.Send(0000, &(sp)); +} + +void sendTelemetry() +{ + Vector3f pihat = eskf.getPihat(); + Vector3f 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*180.0f/M_PI_F); + tp.pihat[i] = pihat(i); + tp.vihat[i] = vihat(i); + } + tp.dynaccNorm = std::sqrt(dynaccnorm2); + + twelite.Send(0000, &(tp)); + +} + +void writeSDcard() +{ + Vector3f pihat = eskf.getPihat(); + Vector3f 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); + lp.pihat[i] = pihat(i); + lp.vihat[i] = vihat(i); + } + lp.pi[0] = pi(0); + lp.pi[1] = pi(1); + lp.pi[2] = pi(2); + lp.vi[0] = vi(0); + lp.vi[1] = vi(1); + lp.vi[2] = vi(2); + lp.acc[0] = acc(0); + lp.acc[1] = acc(1); + lp.acc[2] = acc(2); + lp.gyro[0] = gyro(0); + lp.gyro[1] = gyro(1); + lp.gyro[2] = gyro(2); + lp.mag[0] = mag(0); + lp.mag[1] = mag(1); + lp.mag[2] = mag(2); + 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); +}