HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

transferData.cpp@144:b3a713b4f1c4, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jun 29 07:57:10 2022 +0000
Revision:
144:b3a713b4f1c4
Parent:
143:53808e4e684c 
can fly

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 {
NaotoMorita 144:b3a713b4f1c4 5
NaotoMorita 144:b3a713b4f1c4 6 for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){
NaotoMorita 144:b3a713b4f1c4 7 sp.motor[i] = scaledMotorOut[i];
NaotoMorita 144:b3a713b4f1c4 8 }
NaotoMorita 143:53808e4e684c 9 sp.euler[0] = euler(0)*180.0f/M_PI_F;
NaotoMorita 143:53808e4e684c 10 sp.euler[1] = euler(1)*180.0f/M_PI_F;
NaotoMorita 143:53808e4e684c 11 sp.euler[2] = euler(2)*180.0f/M_PI_F;
NaotoMorita 143:53808e4e684c 12 VectorXf state = eskf.getState();
NaotoMorita 143:53808e4e684c 13 VectorXf variance = eskf.getVariance();
NaotoMorita 143:53808e4e684c 14
NaotoMorita 143:53808e4e684c 15 switch(hilDataOut){
NaotoMorita 143:53808e4e684c 16 case 1 :
NaotoMorita 143:53808e4e684c 17 //posNED
NaotoMorita 143:53808e4e684c 18 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 19 sp.state[i] = state(i);
NaotoMorita 143:53808e4e684c 20 }
NaotoMorita 143:53808e4e684c 21 sp.state[3] = 1.0f/att_dt;
NaotoMorita 143:53808e4e684c 22 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 23 sp.variance[i] = variance(i);
NaotoMorita 143:53808e4e684c 24 }
NaotoMorita 143:53808e4e684c 25 break;
NaotoMorita 143:53808e4e684c 26 case 2 :
NaotoMorita 143:53808e4e684c 27 //velNED
NaotoMorita 143:53808e4e684c 28 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 29 sp.state[i] = state(i+3);
NaotoMorita 143:53808e4e684c 30 }
NaotoMorita 143:53808e4e684c 31 sp.state[3] = 1.0f/att_dt;
NaotoMorita 143:53808e4e684c 32 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 33 sp.variance[i] = variance(i+3);
NaotoMorita 143:53808e4e684c 34 }
NaotoMorita 143:53808e4e684c 35 break;
NaotoMorita 143:53808e4e684c 36 case 3 :
NaotoMorita 143:53808e4e684c 37 //quaternion
NaotoMorita 143:53808e4e684c 38 for (int i = 0; i < 4; i++){
NaotoMorita 143:53808e4e684c 39 sp.state[i] = state(i+6);
NaotoMorita 143:53808e4e684c 40 }
NaotoMorita 143:53808e4e684c 41 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 42 sp.variance[i] = variance(i+6);
NaotoMorita 143:53808e4e684c 43 }
NaotoMorita 143:53808e4e684c 44 break;
NaotoMorita 143:53808e4e684c 45 case 4 :
NaotoMorita 143:53808e4e684c 46 //accBias
NaotoMorita 143:53808e4e684c 47 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 48 sp.state[i] = state(i+10);
NaotoMorita 143:53808e4e684c 49 }
NaotoMorita 143:53808e4e684c 50 sp.state[3] = 1.0f/att_dt;
NaotoMorita 143:53808e4e684c 51 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 52 sp.variance[i] = variance(i+9);
NaotoMorita 143:53808e4e684c 53 }
NaotoMorita 143:53808e4e684c 54 break;
NaotoMorita 143:53808e4e684c 55 case 5 :
NaotoMorita 143:53808e4e684c 56 //gyroBias
NaotoMorita 143:53808e4e684c 57 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 58 sp.state[i] = state(i+13);
NaotoMorita 143:53808e4e684c 59 }
NaotoMorita 143:53808e4e684c 60 sp.state[3] = 1.0f/att_dt;
NaotoMorita 143:53808e4e684c 61 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 62 sp.variance[i] = variance(i+12);
NaotoMorita 143:53808e4e684c 63 }
NaotoMorita 143:53808e4e684c 64 break;
NaotoMorita 143:53808e4e684c 65 case 6 :
NaotoMorita 144:b3a713b4f1c4 66 //gravity
NaotoMorita 143:53808e4e684c 67 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 68 sp.state[i] = state(i+16);
NaotoMorita 143:53808e4e684c 69 }
NaotoMorita 143:53808e4e684c 70 sp.state[3] = 1.0f/att_dt;
NaotoMorita 143:53808e4e684c 71 for (int i = 0; i < 3; i++){
NaotoMorita 143:53808e4e684c 72 sp.variance[i] = variance(i+15);
NaotoMorita 143:53808e4e684c 73 }
NaotoMorita 143:53808e4e684c 74 break;
NaotoMorita 143:53808e4e684c 75 }
NaotoMorita 143:53808e4e684c 76
NaotoMorita 143:53808e4e684c 77 if(hilFlag == true){
NaotoMorita 143:53808e4e684c 78 pc.Send(0000, &(sp));
NaotoMorita 143:53808e4e684c 79 }else{
NaotoMorita 143:53808e4e684c 80 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);
NaotoMorita 143:53808e4e684c 81 }
NaotoMorita 143:53808e4e684c 82
cocorlow 140:53dbdb207542 83 }
cocorlow 140:53dbdb207542 84
cocorlow 140:53dbdb207542 85 void sendTelemetry()
cocorlow 140:53dbdb207542 86 {
NaotoMorita 143:53808e4e684c 87
NaotoMorita 143:53808e4e684c 88 Vector3f pihat = eskf.getPihat();
NaotoMorita 143:53808e4e684c 89 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 90 //Matrix wind = eskf.getWind();
NaotoMorita 143:53808e4e684c 91
NaotoMorita 143:53808e4e684c 92 tp.pi[0] = (float)pihat(0);
NaotoMorita 143:53808e4e684c 93 tp.pi[1] = (float)pihat(1);
NaotoMorita 143:53808e4e684c 94 tp.pi[2] = (float)pihat(2);
NaotoMorita 143:53808e4e684c 95 tp.rpy_l[0] = euler(0);
NaotoMorita 143:53808e4e684c 96 tp.rpy_l[1] = euler(1);
NaotoMorita 143:53808e4e684c 97 tp.rpy_l[2] = euler(2);
NaotoMorita 143:53808e4e684c 98 tp.rpy_c[0] = euler(0);
NaotoMorita 143:53808e4e684c 99 tp.rpy_c[1] = euler(1);
NaotoMorita 143:53808e4e684c 100 tp.rpy_c[2] = euler(2);
NaotoMorita 143:53808e4e684c 101 tp.rpy_r[0] = euler(0);
NaotoMorita 143:53808e4e684c 102 tp.rpy_r[1] = euler(1);
NaotoMorita 143:53808e4e684c 103 tp.rpy_r[2] = euler(2);
NaotoMorita 143:53808e4e684c 104 tp.vi[0] = (float)vihat(0);
NaotoMorita 143:53808e4e684c 105 tp.vi[1] = (float)vihat(1);
NaotoMorita 143:53808e4e684c 106 tp.vi[2] = (float)vihat(2);
NaotoMorita 143:53808e4e684c 107 tp.palt = -palt;
NaotoMorita 143:53808e4e684c 108 tp.gps_fix = (float)gps.gpsFix;
NaotoMorita 143:53808e4e684c 109 /*
NaotoMorita 143:53808e4e684c 110 if (preflightFlag)
NaotoMorita 143:53808e4e684c 111 {
NaotoMorita 143:53808e4e684c 112 tp.mode = (float)0.0f;
NaotoMorita 143:53808e4e684c 113 }
NaotoMorita 143:53808e4e684c 114 else if (posValues[0].ap_flag == 0 || posValues[1].ap_flag == 0 || posValues[2].ap_flag == 0 )
NaotoMorita 143:53808e4e684c 115 {
NaotoMorita 143:53808e4e684c 116 tp.mode = (float)1.0f;
NaotoMorita 143:53808e4e684c 117 }
NaotoMorita 143:53808e4e684c 118 else {
NaotoMorita 143:53808e4e684c 119 tp.mode = (float)2.0f;
NaotoMorita 143:53808e4e684c 120 }
NaotoMorita 143:53808e4e684c 121 */
NaotoMorita 143:53808e4e684c 122 tp.mode = (float)1.0f;
NaotoMorita 143:53808e4e684c 123 tp.time = _t.read();
NaotoMorita 143:53808e4e684c 124 tp.gps_acc = gps.Hacc;
NaotoMorita 143:53808e4e684c 125 tp.vx_opt = 0.0f;
NaotoMorita 143:53808e4e684c 126 tp.vy_opt = 0.0f;
NaotoMorita 143:53808e4e684c 127 tp.dist_ir = 0.0f;
NaotoMorita 143:53808e4e684c 128 tp.voltage[0] = 0.0f;
NaotoMorita 143:53808e4e684c 129 tp.voltage[1] = 0.0f;
NaotoMorita 143:53808e4e684c 130 tp.current[0] = 0.0f;
NaotoMorita 143:53808e4e684c 131 tp.current[1] = 0.0f;
NaotoMorita 143:53808e4e684c 132 tp.wind[0] = 0.0f;
NaotoMorita 143:53808e4e684c 133 tp.wind[1] = 0.0f;
NaotoMorita 143:53808e4e684c 134 tp.wind[2] = 0.0f;
NaotoMorita 144:b3a713b4f1c4 135 tp.pitch_obj = 0.0f;
NaotoMorita 144:b3a713b4f1c4 136 tp.roll_obj = 0.0f;
NaotoMorita 143:53808e4e684c 137 tp.u_pitot = 0.0f;
NaotoMorita 144:b3a713b4f1c4 138 tp.hinf = 0.0f;
NaotoMorita 143:53808e4e684c 139
NaotoMorita 143:53808e4e684c 140 wait(0.001f);
NaotoMorita 143:53808e4e684c 141
NaotoMorita 143:53808e4e684c 142 twelite.Send(0000, &(tp));
NaotoMorita 143:53808e4e684c 143 //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);
cocorlow 140:53dbdb207542 144 }
cocorlow 140:53dbdb207542 145
cocorlow 140:53dbdb207542 146 void writeSDcard()
cocorlow 140:53dbdb207542 147 {
cocorlow 140:53dbdb207542 148 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 149 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 150 //Matrix wind = eskf.getWind();
cocorlow 140:53dbdb207542 151
cocorlow 140:53dbdb207542 152 lp.time = _t.read();
cocorlow 140:53dbdb207542 153 lp.hertz = 1.0f/att_dt;
NaotoMorita 143:53808e4e684c 154 lp.gpsFix = float(gps.gpsFix);
cocorlow 140:53dbdb207542 155 lp.da = da;
cocorlow 140:53dbdb207542 156 lp.de = de;
cocorlow 140:53dbdb207542 157 lp.dT = dT;
cocorlow 140:53dbdb207542 158 for(int i = 0;i<16;i++){
cocorlow 140:53dbdb207542 159 lp.rc[i] = rc[i];
cocorlow 140:53dbdb207542 160 }
cocorlow 140:53dbdb207542 161 for(int i = 0;i<3;i++){
cocorlow 140:53dbdb207542 162 lp.rpy[i] = euler(i);
cocorlow 140:53dbdb207542 163 lp.pihat[i] = pihat(i);
cocorlow 140:53dbdb207542 164 lp.vihat[i] = vihat(i);
cocorlow 140:53dbdb207542 165 }
cocorlow 140:53dbdb207542 166 lp.pi[0] = pi(0);
cocorlow 140:53dbdb207542 167 lp.pi[1] = pi(1);
cocorlow 140:53dbdb207542 168 lp.pi[2] = pi(2);
cocorlow 140:53dbdb207542 169 lp.vi[0] = vi(0);
cocorlow 140:53dbdb207542 170 lp.vi[1] = vi(1);
cocorlow 140:53dbdb207542 171 lp.vi[2] = vi(2);
cocorlow 140:53dbdb207542 172 lp.acc[0] = acc(0);
cocorlow 140:53dbdb207542 173 lp.acc[1] = acc(1);
cocorlow 140:53dbdb207542 174 lp.acc[2] = acc(2);
cocorlow 140:53dbdb207542 175 lp.gyro[0] = gyro(0);
cocorlow 140:53dbdb207542 176 lp.gyro[1] = gyro(1);
cocorlow 140:53dbdb207542 177 lp.gyro[2] = gyro(2);
cocorlow 140:53dbdb207542 178 lp.mag[0] = mag(0);
cocorlow 140:53dbdb207542 179 lp.mag[1] = mag(1);
cocorlow 140:53dbdb207542 180 lp.mag[2] = mag(2);
cocorlow 140:53dbdb207542 181 lp.palt = palt;
NaotoMorita 143:53808e4e684c 182 lp.wind[0] = 0.0f;
NaotoMorita 143:53808e4e684c 183 lp.wind[1] = 0.0f;
NaotoMorita 143:53808e4e684c 184 lp.wind[2] = 0.0f;
NaotoMorita 144:b3a713b4f1c4 185 lp.pitch_obj = 0.0f;
NaotoMorita 144:b3a713b4f1c4 186 lp.roll_obj = 0.0f;
NaotoMorita 143:53808e4e684c 187 lp.u_pitot = 0.0f;
cocorlow 140:53dbdb207542 188
cocorlow 140:53dbdb207542 189 //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 190 //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 191 sd.Send(0000, &(lp));
cocorlow 140:53dbdb207542 192 //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 193 }