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
transferData.cpp
- Committer:
- cocorlow
- Date:
- 2021-12-06
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
- Child:
- 141:725321fe2949
File content as of revision 140:53dbdb207542:
#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); }