solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: transferData.cpp
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
- Child:
- 144:b3a713b4f1c4
--- a/transferData.cpp Fri Dec 10 11:20:13 2021 +0000 +++ b/transferData.cpp Fri Jun 24 05:44:34 2022 +0000 @@ -5,42 +5,160 @@ 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)); + 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(); -// 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)); - + + 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.gpsFix = float(gps.gpsFix); lp.da = da; lp.de = de; lp.dT = dT; @@ -68,6 +186,12 @@ 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));