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:
- NaotoMorita
- Date:
- 2022-06-24
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
File content as of revision 143:53808e4e684c:
#include "global.hpp" void sendData2PC() { sp.da = da; sp.de = de; sp.dT = dT; sp.euler[0] = euler(0)*180.0f/M_PI_F; sp.euler[1] = euler(1)*180.0f/M_PI_F; sp.euler[2] = euler(2)*180.0f/M_PI_F; VectorXf state = eskf.getState(); VectorXf variance = eskf.getVariance(); switch(hilDataOut){ case 1 : //posNED for (int i = 0; i < 3; i++){ sp.state[i] = state(i); } sp.state[3] = 1.0f/att_dt; for (int i = 0; i < 3; i++){ sp.variance[i] = variance(i); } break; case 2 : //velNED for (int i = 0; i < 3; i++){ sp.state[i] = state(i+3); } sp.state[3] = 1.0f/att_dt; for (int i = 0; i < 3; i++){ sp.variance[i] = variance(i+3); } break; case 3 : //quaternion for (int i = 0; i < 4; i++){ sp.state[i] = state(i+6); } for (int i = 0; i < 3; i++){ sp.variance[i] = variance(i+6); } break; case 4 : //accBias for (int i = 0; i < 3; i++){ sp.state[i] = state(i+10); } sp.state[3] = 1.0f/att_dt; for (int i = 0; i < 3; i++){ sp.variance[i] = variance(i+9); } break; case 5 : //gyroBias for (int i = 0; i < 3; i++){ sp.state[i] = state(i+13); } sp.state[3] = 1.0f/att_dt; for (int i = 0; i < 3; i++){ sp.variance[i] = variance(i+12); } break; case 6 : //gyroBias for (int i = 0; i < 3; i++){ sp.state[i] = state(i+16); } sp.state[3] = 1.0f/att_dt; for (int i = 0; i < 3; i++){ sp.variance[i] = variance(i+15); } break; } if(hilFlag == true){ pc.Send(0000, &(sp)); }else{ pc.serial.printf("%f %f %f %f %f\r\n",sp.euler[0],sp.euler[1],sp.euler[2],1.0f/att_dt,std::atan2(-mag(1),mag(0))*180.0f/M_PI); } } void sendTelemetry() { Vector3f pihat = eskf.getPihat(); Vector3f vihat = eskf.getVihat(); //Matrix wind = eskf.getWind(); tp.pi[0] = (float)pihat(0); tp.pi[1] = (float)pihat(1); tp.pi[2] = (float)pihat(2); tp.rpy_l[0] = euler(0); tp.rpy_l[1] = euler(1); tp.rpy_l[2] = euler(2); tp.rpy_c[0] = euler(0); tp.rpy_c[1] = euler(1); tp.rpy_c[2] = euler(2); tp.rpy_r[0] = euler(0); tp.rpy_r[1] = euler(1); tp.rpy_r[2] = euler(2); tp.vi[0] = (float)vihat(0); tp.vi[1] = (float)vihat(1); tp.vi[2] = (float)vihat(2); tp.palt = -palt; tp.gps_fix = (float)gps.gpsFix; /* if (preflightFlag) { tp.mode = (float)0.0f; } else if (posValues[0].ap_flag == 0 || posValues[1].ap_flag == 0 || posValues[2].ap_flag == 0 ) { tp.mode = (float)1.0f; } else { tp.mode = (float)2.0f; } */ tp.mode = (float)1.0f; tp.time = _t.read(); tp.gps_acc = gps.Hacc; tp.vx_opt = 0.0f; tp.vy_opt = 0.0f; tp.dist_ir = 0.0f; tp.voltage[0] = 0.0f; tp.voltage[1] = 0.0f; tp.current[0] = 0.0f; tp.current[1] = 0.0f; tp.wind[0] = 0.0f; tp.wind[1] = 0.0f; tp.wind[2] = 0.0f; tp.pitch_obj = pitch_obj; tp.roll_obj = roll_obj; tp.u_pitot = 0.0f; if (hinf_flag) { tp.hinf = 1.0f; } else { tp.hinf = 0.0f; } wait(0.001f); twelite.Send(0000, &(tp)); //pc.printf("r: %f %f %f p: %f %f %f y: %f %f %f de: %f %f %f\r\n",posValues[0].rpy[0]*180.0f/M_PI,posValues[1].rpy[0]*180.0f/M_PI,posValues[2].rpy[0]*180.0f/M_PI,posValues[0].rpy[1]*180.0f/M_PI,posValues[1].rpy[1]*180.0f/M_PI,posValues[2].rpy[1]*180.0f/M_PI,posValues[0].rpy[2]*180.0f/M_PI,posValues[1].rpy[2]*180.0f/M_PI,posValues[2].rpy[2]*180.0f/M_PI,posValues[0].de,posValues[1].de,posValues[2].de); } void writeSDcard() { Vector3f pihat = eskf.getPihat(); Vector3f vihat = eskf.getVihat(); //Matrix wind = eskf.getWind(); 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; lp.wind[0] = 0.0f; lp.wind[1] = 0.0f; lp.wind[2] = 0.0f; lp.pitch_obj = pitch_obj; lp.roll_obj = roll_obj; lp.u_pitot = 0.0f; //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); }