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?
User | Revision | Line number | New 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 | } |