HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@140:53dbdb207542, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 11:37:55 2021 +0000
Revision:
140:53dbdb207542
Parent:
139:b378528c05f2
Child:
141:725321fe2949
servo transferData imu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 140:53dbdb207542 1 #include "global.hpp"
cocorlow 140:53dbdb207542 2
cocorlow 140:53dbdb207542 3 void sendData2PC()
cocorlow 140:53dbdb207542 4 {
cocorlow 140:53dbdb207542 5 sp.da = da;
cocorlow 140:53dbdb207542 6 sp.de = de;
cocorlow 140:53dbdb207542 7 sp.dT = dT;
cocorlow 140:53dbdb207542 8 sp.rpy[0] = rpy(0)*180.0f/M_PI_F;
cocorlow 140:53dbdb207542 9 sp.rpy[1] = rpy(1)*180.0f/M_PI_F;
cocorlow 140:53dbdb207542 10 sp.rpy[2] = rpy(2)*180.0f/M_PI_F;
cocorlow 140:53dbdb207542 11 Vector3f vihat = eskf.getVihat();
cocorlow 140:53dbdb207542 12 sp.vihat[0] = vihat(0);
cocorlow 140:53dbdb207542 13 sp.vihat[1] = vihat(1);
cocorlow 140:53dbdb207542 14 sp.vihat[2] = vihat(2);
cocorlow 140:53dbdb207542 15 pc.Send(0000, &(sp));
cocorlow 140:53dbdb207542 16 }
cocorlow 140:53dbdb207542 17
cocorlow 140:53dbdb207542 18 void sendTelemetry()
cocorlow 140:53dbdb207542 19 {
cocorlow 140:53dbdb207542 20 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 21 Vector3f vihat = eskf.getVihat();
cocorlow 140:53dbdb207542 22 tp.time=_t.read();
cocorlow 140:53dbdb207542 23 tp.hertz = 1.0f/att_dt;
cocorlow 140:53dbdb207542 24 tp.gpsFix = float(gps.gpsFix);
cocorlow 140:53dbdb207542 25 for(int i = 0;i<3;i++){
cocorlow 140:53dbdb207542 26 tp.rpy[i] = euler(i*180.0f/M_PI_F);
cocorlow 140:53dbdb207542 27 tp.pihat[i] = pihat(i);
cocorlow 140:53dbdb207542 28 tp.vihat[i] = vihat(i);
cocorlow 140:53dbdb207542 29 }
cocorlow 140:53dbdb207542 30 tp.dynaccNorm = std::sqrt(dynaccnorm2);
cocorlow 140:53dbdb207542 31
cocorlow 140:53dbdb207542 32 twelite.Send(0000, &(tp));
cocorlow 140:53dbdb207542 33
cocorlow 140:53dbdb207542 34 }
cocorlow 140:53dbdb207542 35
cocorlow 140:53dbdb207542 36 void writeSDcard()
cocorlow 140:53dbdb207542 37 {
cocorlow 140:53dbdb207542 38 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 39 Vector3f vihat = eskf.getVihat();
cocorlow 140:53dbdb207542 40
cocorlow 140:53dbdb207542 41 lp.time = _t.read();
cocorlow 140:53dbdb207542 42 lp.hertz = 1.0f/att_dt;
cocorlow 140:53dbdb207542 43 lp.gpsFix = float(gps.gpsFix);
cocorlow 140:53dbdb207542 44 lp.da = da;
cocorlow 140:53dbdb207542 45 lp.de = de;
cocorlow 140:53dbdb207542 46 lp.dT = dT;
cocorlow 140:53dbdb207542 47 for(int i = 0;i<16;i++){
cocorlow 140:53dbdb207542 48 lp.rc[i] = rc[i];
cocorlow 140:53dbdb207542 49 }
cocorlow 140:53dbdb207542 50 for(int i = 0;i<3;i++){
cocorlow 140:53dbdb207542 51 lp.rpy[i] = euler(i);
cocorlow 140:53dbdb207542 52 lp.pihat[i] = pihat(i);
cocorlow 140:53dbdb207542 53 lp.vihat[i] = vihat(i);
cocorlow 140:53dbdb207542 54 }
cocorlow 140:53dbdb207542 55 lp.pi[0] = pi(0);
cocorlow 140:53dbdb207542 56 lp.pi[1] = pi(1);
cocorlow 140:53dbdb207542 57 lp.pi[2] = pi(2);
cocorlow 140:53dbdb207542 58 lp.vi[0] = vi(0);
cocorlow 140:53dbdb207542 59 lp.vi[1] = vi(1);
cocorlow 140:53dbdb207542 60 lp.vi[2] = vi(2);
cocorlow 140:53dbdb207542 61 lp.acc[0] = acc(0);
cocorlow 140:53dbdb207542 62 lp.acc[1] = acc(1);
cocorlow 140:53dbdb207542 63 lp.acc[2] = acc(2);
cocorlow 140:53dbdb207542 64 lp.gyro[0] = gyro(0);
cocorlow 140:53dbdb207542 65 lp.gyro[1] = gyro(1);
cocorlow 140:53dbdb207542 66 lp.gyro[2] = gyro(2);
cocorlow 140:53dbdb207542 67 lp.mag[0] = mag(0);
cocorlow 140:53dbdb207542 68 lp.mag[1] = mag(1);
cocorlow 140:53dbdb207542 69 lp.mag[2] = mag(2);
cocorlow 140:53dbdb207542 70 lp.palt = palt;
cocorlow 140:53dbdb207542 71
cocorlow 140:53dbdb207542 72 //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);
cocorlow 140:53dbdb207542 73 //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));
cocorlow 140:53dbdb207542 74 sd.Send(0000, &(lp));
cocorlow 140:53dbdb207542 75 //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);
cocorlow 140:53dbdb207542 76 }