hige dura
Dependencies: FatFileSystem mbed
main.cpp@2:307500c991dd, 2012-04-21 (annotated)
- Committer:
- higedura
- Date:
- Sat Apr 21 14:07:27 2012 +0000
- Revision:
- 2:307500c991dd
- Parent:
- 1:8048a8bcde59
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| higedura | 0:813b917360ac | 1 | // dot-HR EKF |
| higedura | 0:813b917360ac | 2 | |
| higedura | 0:813b917360ac | 3 | #include "mbed.h" |
| higedura | 0:813b917360ac | 4 | #include "SDFileSystem.h" |
| higedura | 0:813b917360ac | 5 | #include "ADXL345_I2C.h" |
| higedura | 0:813b917360ac | 6 | #include "ITG3200.h" |
| higedura | 0:813b917360ac | 7 | #include "HMC5883L.h" |
| higedura | 0:813b917360ac | 8 | |
| higedura | 0:813b917360ac | 9 | Serial pc(USBTX, USBRX); |
| higedura | 0:813b917360ac | 10 | DigitalOut led1(LED1); |
| higedura | 0:813b917360ac | 11 | DigitalOut led2(LED2); |
| higedura | 0:813b917360ac | 12 | DigitalOut led3(LED3); |
| higedura | 0:813b917360ac | 13 | DigitalOut led4(LED4); |
| higedura | 0:813b917360ac | 14 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board |
| higedura | 0:813b917360ac | 15 | ADXL345_I2C accelerometer(p9, p10); |
| higedura | 0:813b917360ac | 16 | ITG3200 gyro(p9, p10); |
| higedura | 0:813b917360ac | 17 | HMC5883L compass(p9, p10); |
| higedura | 0:813b917360ac | 18 | Serial xbee(p13, p14); |
| higedura | 0:813b917360ac | 19 | DigitalIn stop(p20); |
| higedura | 0:813b917360ac | 20 | PwmOut ESC1(p21); |
| higedura | 0:813b917360ac | 21 | PwmOut ESC2(p22); |
| higedura | 0:813b917360ac | 22 | PwmOut ESC3(p23); |
| higedura | 0:813b917360ac | 23 | PwmOut ESC4(p24); |
| higedura | 0:813b917360ac | 24 | PwmOut ESC5(p25); |
| higedura | 0:813b917360ac | 25 | PwmOut ESC6(p26); |
| higedura | 0:813b917360ac | 26 | Serial navi(p28, p27); // tx, rx |
| higedura | 0:813b917360ac | 27 | DigitalOut navi_flag(p29); |
| higedura | 0:813b917360ac | 28 | |
| higedura | 0:813b917360ac | 29 | #define pi 3.14159265 |
| higedura | 0:813b917360ac | 30 | |
| higedura | 0:813b917360ac | 31 | #define N 3 // phi, the, psi |
| higedura | 0:813b917360ac | 32 | #define M 6 // The numbers of rotors |
| higedura | 0:813b917360ac | 33 | #define L 5 // The numbers of state quantities (optimal regulator) |
| higedura | 0:813b917360ac | 34 | #define chan 4 // The numbers of channels |
| higedura | 0:813b917360ac | 35 | #define chan_buf 17 |
| higedura | 0:813b917360ac | 36 | #define N_LWMA 100 |
| higedura | 0:813b917360ac | 37 | |
| higedura | 0:813b917360ac | 38 | int preparing_acc(); |
| higedura | 0:813b917360ac | 39 | double* calib(); |
| higedura | 0:813b917360ac | 40 | double* RK4( double, double[N], double[N] ); |
| higedura | 0:813b917360ac | 41 | double* func( double[N], double[N] ); |
| higedura | 0:813b917360ac | 42 | double* LWMA( double[N] ); |
| higedura | 0:813b917360ac | 43 | double* EKF_predict( double[N], double[N] ); |
| higedura | 0:813b917360ac | 44 | double* EKF_correct( double[N], double[N], double[N] ); |
| higedura | 0:813b917360ac | 45 | |
| higedura | 0:813b917360ac | 46 | // 0 1 2 |
| higedura | 0:813b917360ac | 47 | // [ accXn-1 accXn-2 ... ] 0 |
| higedura | 0:813b917360ac | 48 | // zBuf = [ accYn-1 accYn-2 ... ] 1 |
| higedura | 0:813b917360ac | 49 | // [ accZn-1 accZn-2 ... ] 2 |
| higedura | 0:813b917360ac | 50 | double z_buf[N][N_LWMA] = {0}; // For LWMA |
| higedura | 0:813b917360ac | 51 | |
| higedura | 0:813b917360ac | 52 | double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF |
| higedura | 0:813b917360ac | 53 | |
| higedura | 0:813b917360ac | 54 | int main(){ |
| higedura | 0:813b917360ac | 55 | |
| higedura | 0:813b917360ac | 56 | pc.baud(921600); |
| higedura | 0:813b917360ac | 57 | navi.baud(57600); |
| higedura | 0:813b917360ac | 58 | xbee.baud(57600); |
| higedura | 0:813b917360ac | 59 | |
| higedura | 0:813b917360ac | 60 | FILE *fp = fopen("/sd/sdtest.txt", "w"); |
| higedura | 0:813b917360ac | 61 | if(fp == NULL) { |
| higedura | 0:813b917360ac | 62 | error("Could not open file for write\n"); |
| higedura | 0:813b917360ac | 63 | while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; } |
| higedura | 0:813b917360ac | 64 | } |
| higedura | 0:813b917360ac | 65 | |
| higedura | 1:8048a8bcde59 | 66 | int correct_period = 100; |
| higedura | 0:813b917360ac | 67 | int navi_period = 10; |
| higedura | 0:813b917360ac | 68 | int xbee_period = 100; |
| higedura | 1:8048a8bcde59 | 69 | int correct_loop = 0; |
| higedura | 0:813b917360ac | 70 | int navi_loop = 10; |
| higedura | 0:813b917360ac | 71 | int xbee_loop = 100; |
| higedura | 0:813b917360ac | 72 | |
| higedura | 2:307500c991dd | 73 | double dt_wait = 0.00043; |
| higedura | 0:813b917360ac | 74 | //double dt_wait = 0.01; |
| higedura | 0:813b917360ac | 75 | double dt = 0.01; |
| higedura | 0:813b917360ac | 76 | double t = 0; |
| higedura | 0:813b917360ac | 77 | |
| higedura | 0:813b917360ac | 78 | int bit_acc[N] = {0}; // Buffer of the accelerometer |
| higedura | 0:813b917360ac | 79 | int get_mag[N] = {0}; // Buffer of the compass |
| higedura | 0:813b917360ac | 80 | |
| higedura | 0:813b917360ac | 81 | // Calibration routine |
| higedura | 0:813b917360ac | 82 | double calib_acc[N] = {0}; |
| higedura | 0:813b917360ac | 83 | double calib_gyro[N] = {0}; |
| higedura | 2:307500c991dd | 84 | //double compass_basis_rad = 0; |
| higedura | 0:813b917360ac | 85 | |
| higedura | 0:813b917360ac | 86 | // Getting data |
| higedura | 0:813b917360ac | 87 | double acc[N] = {0, 0, 1}; |
| higedura | 0:813b917360ac | 88 | double acc0[N] = {0, 0, 1}; |
| higedura | 0:813b917360ac | 89 | double d_acc[N] = {0}; |
| higedura | 0:813b917360ac | 90 | double gyro_deg[N] = {0}; |
| higedura | 0:813b917360ac | 91 | double gyro_rad[N] = {0}; |
| higedura | 2:307500c991dd | 92 | //int mag[N] = {0}; |
| higedura | 0:813b917360ac | 93 | |
| higedura | 0:813b917360ac | 94 | // Measurement algorithm |
| higedura | 2:307500c991dd | 95 | //double angle_acc[2] = {0}; |
| higedura | 0:813b917360ac | 96 | double angle_deg[N] = {0}; |
| higedura | 0:813b917360ac | 97 | double angle_rad[N] = {0}; |
| higedura | 0:813b917360ac | 98 | double zLWMA[N] = {0}; |
| higedura | 2:307500c991dd | 99 | //double compass_rad = 0; |
| higedura | 2:307500c991dd | 100 | //double compass_deg = 0; |
| higedura | 0:813b917360ac | 101 | |
| higedura | 0:813b917360ac | 102 | // Gravity z |
| higedura | 0:813b917360ac | 103 | for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; } |
| higedura | 0:813b917360ac | 104 | |
| higedura | 0:813b917360ac | 105 | double* p_calib; |
| higedura | 0:813b917360ac | 106 | double* p_RK4; |
| higedura | 0:813b917360ac | 107 | double* p_EKF; |
| higedura | 0:813b917360ac | 108 | double* p_zLWMA; |
| higedura | 0:813b917360ac | 109 | |
| higedura | 0:813b917360ac | 110 | // Optimal regulator |
| higedura | 0:813b917360ac | 111 | double state[L]; |
| higedura | 1:8048a8bcde59 | 112 | // Gain (q1=10, q2=10, q3=10, q4=10, q5=1) |
| higedura | 1:8048a8bcde59 | 113 | /* double Kr[M][L] = {{ 0, 1.826, 0, 1.984, 0.408}, |
| higedura | 1:8048a8bcde59 | 114 | {-1.581, 0.913,-1.718, 0.992,-0.408}, |
| higedura | 1:8048a8bcde59 | 115 | {-1.581,-0.913,-1.718,-0.992, 0.408}, |
| higedura | 1:8048a8bcde59 | 116 | { 0,-1.826, 0,-1.984,-0.408}, |
| higedura | 1:8048a8bcde59 | 117 | { 1.581,-0.913, 1.718,-0.992, 0.408}, |
| higedura | 1:8048a8bcde59 | 118 | { 1.581, 0.913, 1.718, 0.992,-0.408}}; |
| higedura | 2:307500c991dd | 119 | */ |
| higedura | 2:307500c991dd | 120 | // Gain (q1=100, q2=100, q3=1, q4=1, q5=1) |
| higedura | 2:307500c991dd | 121 | /* double Kr[M][L] = {{ 0, 5.774, 0, 1.496, 0.408}, |
| higedura | 2:307500c991dd | 122 | {-5, 2.887,-1.296, 0.748,-0.408}, |
| higedura | 2:307500c991dd | 123 | {-5,-2.887,-1.296,-0.748, 0.408}, |
| higedura | 2:307500c991dd | 124 | { 0,-5.774, 0,-1.496,-0.408}, |
| higedura | 2:307500c991dd | 125 | { 5,-2.887, 1.296,-0.748, 0.408}, |
| higedura | 2:307500c991dd | 126 | { 5, 2.887, 1.296, 0.748,-0.408}}; |
| higedura | 2:307500c991dd | 127 | */ |
| higedura | 1:8048a8bcde59 | 128 | // Gain (q1=100, q2=100, q3=10, q4=10, q5=1) |
| higedura | 1:8048a8bcde59 | 129 | /* double Kr[M][L] = {{ 0, 5.774, 0, 2.288, 0.408}, |
| higedura | 1:8048a8bcde59 | 130 | {-5, 2.887,-1.982, 1.144,-0.408}, |
| higedura | 1:8048a8bcde59 | 131 | {-5,-2.887,-1.982,-1.144, 0.408}, |
| higedura | 1:8048a8bcde59 | 132 | { 0,-5.774, 0,-2.288,-0.408}, |
| higedura | 1:8048a8bcde59 | 133 | { 5,-2.887, 1.982,-1.144, 0.408}, |
| higedura | 1:8048a8bcde59 | 134 | { 5, 2.887, 1.982, 1.144,-0.408}}; |
| higedura | 1:8048a8bcde59 | 135 | */ |
| higedura | 2:307500c991dd | 136 | // Gain (q1=100, q2=100, q3=50, q4=50, q5=1) |
| higedura | 2:307500c991dd | 137 | double Kr[M][L] = {{ 0, 5.774, 0, 4.309, 0.408}, |
| higedura | 2:307500c991dd | 138 | {-5, 2.887,-3.732, 2.155,-0.408}, |
| higedura | 2:307500c991dd | 139 | {-5,-2.887,-3.732,-2.155, 0.408}, |
| higedura | 2:307500c991dd | 140 | { 0,-5.774, 0,-4.309,-0.408}, |
| higedura | 2:307500c991dd | 141 | { 5,-2.887, 3.732,-2.155, 0.408}, |
| higedura | 2:307500c991dd | 142 | { 5, 2.887, 3.732, 2.155,-0.408}}; |
| higedura | 0:813b917360ac | 143 | |
| higedura | 1:8048a8bcde59 | 144 | // Gain (q1=100, q2=100, q3=100, q4=100, q5=1) |
| higedura | 2:307500c991dd | 145 | /* double Kr[M][L] = {{ 0, 5.774, 0, 5.936, 0.408}, |
| higedura | 1:8048a8bcde59 | 146 | {-5, 2.887,-5.141, 2.968,-0.408}, |
| higedura | 1:8048a8bcde59 | 147 | {-5,-2.887,-5.141,-2.968, 0.408}, |
| higedura | 1:8048a8bcde59 | 148 | { 0,-5.774, 0,-5.936,-0.408}, |
| higedura | 1:8048a8bcde59 | 149 | { 5,-2.887, 5.141,-2.968, 0.408}, |
| higedura | 1:8048a8bcde59 | 150 | { 5, 2.887, 5.141, 2.968,-0.408}}; |
| higedura | 2:307500c991dd | 151 | */ |
| higedura | 2:307500c991dd | 152 | // Gain (q1=300, q2=300, q3=100, q4=100, q5=1) |
| higedura | 2:307500c991dd | 153 | /* double Kr[M][L] = {{ 0, 10, 0, 6.052, 0.408}, |
| higedura | 2:307500c991dd | 154 | {-8.66, 5,-5.242, 3.026,-0.408}, |
| higedura | 2:307500c991dd | 155 | {-8.66, -5,-5.242,-3.026, 0.408}, |
| higedura | 2:307500c991dd | 156 | { 0,-10, 0,-6.052,-0.408}, |
| higedura | 2:307500c991dd | 157 | { 8.66, -5, 5.242,-3.026, 0.408}, |
| higedura | 2:307500c991dd | 158 | { 8.66, 5, 5.242, 3.026,-0.408}}; |
| higedura | 2:307500c991dd | 159 | */ |
| higedura | 2:307500c991dd | 160 | // Gain (q1=400, q2=400, q3=100, q4=100, q5=1) |
| higedura | 2:307500c991dd | 161 | /* double Kr[M][L] = {{ 0, 11.547, 0, 6.094, 0.408}, |
| higedura | 2:307500c991dd | 162 | {-10, 5.774,-5.278, 3.047,-0.408}, |
| higedura | 2:307500c991dd | 163 | {-10, -5.774,-5.278,-3.047, 0.408}, |
| higedura | 2:307500c991dd | 164 | { 0,-11.547, 0,-6.094,-0.408}, |
| higedura | 2:307500c991dd | 165 | { 10, -5.774, 5.278,-3.047, 0.408}, |
| higedura | 2:307500c991dd | 166 | { 10, 5.774, 5.278, 3.047,-0.408}}; |
| higedura | 1:8048a8bcde59 | 167 | */ |
| higedura | 2:307500c991dd | 168 | // Gain (q1=500, q2=500, q3=50, q4=50, q5=1) |
| higedura | 2:307500c991dd | 169 | /* double Kr[M][L] = {{ 0, 12.910, 0, 4.574, 0.408}, |
| higedura | 2:307500c991dd | 170 | {-11.18, 6.455,-3.962, 2.287,-0.408}, |
| higedura | 2:307500c991dd | 171 | {-11.18, -6.455,-3.962,-2.287, 0.408}, |
| higedura | 2:307500c991dd | 172 | { 0,-12.910, 0,-4.574,-0.408}, |
| higedura | 2:307500c991dd | 173 | { 11.18, -6.455, 3.962,-2.287, 0.408}, |
| higedura | 2:307500c991dd | 174 | { 11.18, 6.455, 3.962, 2.287,-0.408}}; |
| higedura | 2:307500c991dd | 175 | */ |
| higedura | 2:307500c991dd | 176 | // Gain (q1=500, q2=500, q3=100, q4=100, q5=1) |
| higedura | 2:307500c991dd | 177 | /* double Kr[M][L] = {{ 0, 12.910, 0, 6.131, 0.408}, |
| higedura | 2:307500c991dd | 178 | {-11.18, 6.455,-5.31, 3.066,-0.408}, |
| higedura | 2:307500c991dd | 179 | {-11.18, -6.455,-5.31,-3.066, 0.408}, |
| higedura | 2:307500c991dd | 180 | { 0,-12.910, 0,-6.131,-0.408}, |
| higedura | 2:307500c991dd | 181 | { 11.18, -6.455, 5.31,-3.066, 0.408}, |
| higedura | 2:307500c991dd | 182 | { 11.18, 6.455, 5.31, 3.066,-0.408}}; |
| higedura | 2:307500c991dd | 183 | */ |
| higedura | 2:307500c991dd | 184 | double Ccom_r = 0.2; |
| higedura | 2:307500c991dd | 185 | double Ccom_p = 0.2; |
| higedura | 2:307500c991dd | 186 | double Ccom_y = 0.1; |
| higedura | 2:307500c991dd | 187 | |
| higedura | 0:813b917360ac | 188 | double motor[M] = {0}; |
| higedura | 2:307500c991dd | 189 | double motor_control[M] = {0}; // Pulth width generated by the transmitter |
| higedura | 2:307500c991dd | 190 | double motor_attitude[M] = {0}; // Pulth width generated by the attitude control |
| higedura | 0:813b917360ac | 191 | |
| higedura | 0:813b917360ac | 192 | // Coefficient of transfering Force to Puls |
| higedura | 0:813b917360ac | 193 | double Cfp = 2*pow(10.0, -7); |
| higedura | 0:813b917360ac | 194 | |
| higedura | 0:813b917360ac | 195 | // Navigation |
| higedura | 0:813b917360ac | 196 | char command_buf1[chan_buf] = {0}; |
| higedura | 0:813b917360ac | 197 | int command_buf2[chan_buf] = {0}; |
| higedura | 0:813b917360ac | 198 | double command[chan] = {0}; |
| higedura | 0:813b917360ac | 199 | int set_navi_flag = 0; |
| higedura | 0:813b917360ac | 200 | |
| higedura | 0:813b917360ac | 201 | // *** Setting up sensors *** |
| higedura | 0:813b917360ac | 202 | //pc.printf("\r\n\r\nSetting up sensors\r\n"); |
| higedura | 0:813b917360ac | 203 | xbee.printf("\r\n\r\nSetting up sensors\r\n"); |
| higedura | 0:813b917360ac | 204 | |
| higedura | 0:813b917360ac | 205 | // These are here to test whether any of the initialization fails. It will print the failure. |
| higedura | 0:813b917360ac | 206 | if (accelerometer.setPowerControl(0x00)){ |
| higedura | 0:813b917360ac | 207 | //pc.printf("didn't intitialize power control\n\r"); |
| higedura | 0:813b917360ac | 208 | xbee.printf("didn't intitialize power control\n\r"); |
| higedura | 0:813b917360ac | 209 | return 0; |
| higedura | 0:813b917360ac | 210 | } |
| higedura | 0:813b917360ac | 211 | // Full resolution, +/-16g, 4mg/LSB. |
| higedura | 0:813b917360ac | 212 | wait(.1); |
| higedura | 0:813b917360ac | 213 | |
| higedura | 0:813b917360ac | 214 | if(accelerometer.setDataFormatControl(0x0B)){ |
| higedura | 0:813b917360ac | 215 | //pc.printf("didn't set data format\n\r"); |
| higedura | 0:813b917360ac | 216 | xbee.printf("didn't set data format\n\r"); |
| higedura | 0:813b917360ac | 217 | return 0; |
| higedura | 0:813b917360ac | 218 | } |
| higedura | 0:813b917360ac | 219 | wait(.1); |
| higedura | 0:813b917360ac | 220 | |
| higedura | 0:813b917360ac | 221 | // 3.2kHz data rate. |
| higedura | 0:813b917360ac | 222 | if(accelerometer.setDataRate(ADXL345_3200HZ)){ |
| higedura | 0:813b917360ac | 223 | //pc.printf("didn't set data rate\n\r"); |
| higedura | 0:813b917360ac | 224 | xbee.printf("didn't set data rate\n\r"); |
| higedura | 0:813b917360ac | 225 | return 0; |
| higedura | 0:813b917360ac | 226 | } |
| higedura | 0:813b917360ac | 227 | wait(.1); |
| higedura | 0:813b917360ac | 228 | |
| higedura | 0:813b917360ac | 229 | if(accelerometer.setPowerControl(MeasurementMode)) { |
| higedura | 0:813b917360ac | 230 | //pc.printf("didn't set the power control to measurement\n\r"); |
| higedura | 0:813b917360ac | 231 | xbee.printf("didn't set the power control to measurement\n\r"); |
| higedura | 0:813b917360ac | 232 | return 0; |
| higedura | 0:813b917360ac | 233 | } |
| higedura | 0:813b917360ac | 234 | wait(.1); |
| higedura | 0:813b917360ac | 235 | |
| higedura | 0:813b917360ac | 236 | gyro.setLpBandwidth(LPFBW_42HZ); |
| higedura | 0:813b917360ac | 237 | compass.setDefault(); |
| higedura | 0:813b917360ac | 238 | wait(.1); // Wait some time for all sensors (Need at least 5ms) |
| higedura | 0:813b917360ac | 239 | |
| higedura | 0:813b917360ac | 240 | // *** Setting up motors *** |
| higedura | 0:813b917360ac | 241 | //pc.printf("Setting up motors\r\n"); |
| higedura | 0:813b917360ac | 242 | xbee.printf("Setting up motors\r\n"); |
| higedura | 0:813b917360ac | 243 | ESC1.period(0.016044); ESC2.period(0.016044); ESC3.period(0.016044); ESC4.period(0.016044); ESC5.period(0.016044); ESC6.period(0.016044); |
| higedura | 0:813b917360ac | 244 | // pulsewidth 0.0011~0.00195 |
| higedura | 0:813b917360ac | 245 | ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001); |
| higedura | 0:813b917360ac | 246 | // Wait some time for ESC (about 10s) |
| higedura | 0:813b917360ac | 247 | wait(5); |
| higedura | 0:813b917360ac | 248 | |
| higedura | 0:813b917360ac | 249 | // *** Setting up navigator *** |
| higedura | 0:813b917360ac | 250 | //pc.printf("Setting up navigator\r\n"); |
| higedura | 0:813b917360ac | 251 | xbee.printf("Setting up navigator\r\n"); |
| higedura | 0:813b917360ac | 252 | while( set_navi_flag==0 ){ |
| higedura | 0:813b917360ac | 253 | navi_flag = 1; |
| higedura | 0:813b917360ac | 254 | for( int i=0;i<chan_buf;i++ ){ |
| higedura | 0:813b917360ac | 255 | navi.scanf("%c",&command_buf1[i]); |
| higedura | 0:813b917360ac | 256 | if(command_buf1[i]=='a'){ |
| higedura | 0:813b917360ac | 257 | set_navi_flag=1; |
| higedura | 0:813b917360ac | 258 | break; |
| higedura | 0:813b917360ac | 259 | } |
| higedura | 0:813b917360ac | 260 | } |
| higedura | 0:813b917360ac | 261 | navi_flag = 0; |
| higedura | 0:813b917360ac | 262 | wait(.1); |
| higedura | 0:813b917360ac | 263 | } |
| higedura | 0:813b917360ac | 264 | |
| higedura | 0:813b917360ac | 265 | // *** Calibration routine *** |
| higedura | 0:813b917360ac | 266 | p_calib = calib(); |
| higedura | 0:813b917360ac | 267 | for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; } |
| higedura | 0:813b917360ac | 268 | for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; } |
| higedura | 2:307500c991dd | 269 | //compass_basis_rad = *p_calib; |
| higedura | 0:813b917360ac | 270 | |
| higedura | 0:813b917360ac | 271 | led1 = 1; led2 = 1; led3 = 1; led4 = 1; |
| higedura | 0:813b917360ac | 272 | |
| higedura | 0:813b917360ac | 273 | //pc.printf("Starting IMU unit\n\r"); |
| higedura | 0:813b917360ac | 274 | xbee.printf("Starting IMU unit\n\r"); |
| higedura | 1:8048a8bcde59 | 275 | //pc.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r"); |
| higedura | 1:8048a8bcde59 | 276 | xbee.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r"); |
| higedura | 0:813b917360ac | 277 | |
| higedura | 0:813b917360ac | 278 | //while(1){ |
| higedura | 0:813b917360ac | 279 | while( stop==0 ){ |
| higedura | 0:813b917360ac | 280 | |
| higedura | 0:813b917360ac | 281 | // Navigation |
| higedura | 0:813b917360ac | 282 | if( navi_loop>=navi_period ){ |
| higedura | 0:813b917360ac | 283 | |
| higedura | 0:813b917360ac | 284 | navi_flag = 1; |
| higedura | 0:813b917360ac | 285 | |
| higedura | 0:813b917360ac | 286 | for( int i=0;i<chan_buf;i++ ){ navi.scanf("%c",&command_buf1[i]); } |
| higedura | 0:813b917360ac | 287 | for( int i=0;i<chan_buf;i++ ){ |
| higedura | 0:813b917360ac | 288 | command_buf2[i] = (int)command_buf1[i]-48; |
| higedura | 0:813b917360ac | 289 | if( command_buf2[i]==-16 ){ command_buf2[i]=0; } |
| higedura | 0:813b917360ac | 290 | } |
| higedura | 2:307500c991dd | 291 | //for( int i=0;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]+0.001; } |
| higedura | 2:307500c991dd | 292 | command[0] = 0.0001*command_buf2[1]+0.00001*command_buf2[2]+0.000001*command_buf2[3]+0.001; |
| higedura | 2:307500c991dd | 293 | for( int i=1;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]-0.0005; } |
| higedura | 0:813b917360ac | 294 | |
| higedura | 0:813b917360ac | 295 | navi_flag = 0; |
| higedura | 0:813b917360ac | 296 | navi_loop = 0; |
| higedura | 0:813b917360ac | 297 | |
| higedura | 0:813b917360ac | 298 | } |
| higedura | 0:813b917360ac | 299 | navi_loop++; |
| higedura | 0:813b917360ac | 300 | //command[0] = 0.0016; // Shiyougo comentout surukoto!! |
| higedura | 0:813b917360ac | 301 | |
| higedura | 0:813b917360ac | 302 | // Updating accelerometer and compass |
| higedura | 0:813b917360ac | 303 | accelerometer.getOutput(bit_acc); |
| higedura | 0:813b917360ac | 304 | compass.readData(get_mag); |
| higedura | 0:813b917360ac | 305 | |
| higedura | 0:813b917360ac | 306 | for( int i=0;i<N;i++ ){ acc0[i] = acc[i]; } |
| higedura | 0:813b917360ac | 307 | |
| higedura | 0:813b917360ac | 308 | // Transfering units and Coordinate transform |
| higedura | 0:813b917360ac | 309 | acc[0] = (((int16_t)bit_acc[0]-calib_acc[0])-((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0); |
| higedura | 0:813b917360ac | 310 | acc[1] = (((int16_t)bit_acc[0]-calib_acc[0])+((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0); |
| higedura | 0:813b917360ac | 311 | acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1; |
| higedura | 0:813b917360ac | 312 | |
| higedura | 0:813b917360ac | 313 | gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); |
| higedura | 0:813b917360ac | 314 | gyro_deg[1] = (-(gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); // Modify the differencial of the sensor axis |
| higedura | 0:813b917360ac | 315 | gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375; |
| higedura | 1:8048a8bcde59 | 316 | /* |
| higedura | 1:8048a8bcde59 | 317 | for( int i=0;i<N;i++ ){ acc[i] = (int16_t)bit_acc[i]; } |
| higedura | 1:8048a8bcde59 | 318 | gyro_deg[0] = gyro.getGyroX(); |
| higedura | 1:8048a8bcde59 | 319 | gyro_deg[1] = gyro.getGyroY(); |
| higedura | 1:8048a8bcde59 | 320 | gyro_deg[2] = gyro.getGyroZ(); |
| higedura | 1:8048a8bcde59 | 321 | */ |
| higedura | 2:307500c991dd | 322 | //for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; } |
| higedura | 0:813b917360ac | 323 | |
| higedura | 0:813b917360ac | 324 | // Low pass filter for acc |
| higedura | 0:813b917360ac | 325 | //if( -0.05<acc[0] && acc[0]<0.05 ){ acc[0]=0; } |
| higedura | 0:813b917360ac | 326 | //if( -0.05<acc[1] && acc[1]<0.05 ){ acc[1]=0; } |
| higedura | 0:813b917360ac | 327 | //if( 0.95<acc[2] && acc[2]<1.05 ){ acc[2]=0; } |
| higedura | 0:813b917360ac | 328 | // Limitter for acc |
| higedura | 0:813b917360ac | 329 | if( acc[0]<-1 ){ acc[0]=-1; } if( 1<acc[0] ){ acc[0]=1; } |
| higedura | 0:813b917360ac | 330 | if( acc[1]<-1 ){ acc[1]=-1; } if( 1<acc[1] ){ acc[1]=1; } |
| higedura | 0:813b917360ac | 331 | if( acc[2]<-0 ){ acc[2]=-0; } if( 2<acc[2] ){ acc[2]=2; } |
| higedura | 0:813b917360ac | 332 | |
| higedura | 0:813b917360ac | 333 | for( int i=0;i<N;i++ ){ d_acc[i] = acc[i]-acc0[i]; } |
| higedura | 0:813b917360ac | 334 | if( abs(d_acc[0])>=0.5 && abs(acc[0])>=0.3 ){ acc[0] = acc0[0]; } |
| higedura | 0:813b917360ac | 335 | if( abs(d_acc[1])>=0.5 && abs(acc[1])>=0.3 ){ acc[1] = acc0[1]; } |
| higedura | 0:813b917360ac | 336 | if( abs(d_acc[2])>=0.5 ){ acc[2] = acc0[2]; } |
| higedura | 0:813b917360ac | 337 | |
| higedura | 0:813b917360ac | 338 | // Low pass filter for gyro |
| higedura | 0:813b917360ac | 339 | //for( int i=0;i<N;i++ ){if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; }} |
| higedura | 0:813b917360ac | 340 | // Limitter for gyro |
| higedura | 0:813b917360ac | 341 | //for( int i=0;i<N;i++ ){if( gyro_deg[i]<-90 ){ gyro_deg[i]=-90; }} for( int i=0;i<N;i++ ){if( 90<gyro_deg[i] ){ gyro_deg[i]=90; }} |
| higedura | 0:813b917360ac | 342 | |
| higedura | 0:813b917360ac | 343 | for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; } |
| higedura | 0:813b917360ac | 344 | /* |
| higedura | 0:813b917360ac | 345 | // Compass yaw |
| higedura | 0:813b917360ac | 346 | compass_rad = (double)mag[1]/mag[0]; |
| higedura | 0:813b917360ac | 347 | compass_rad = atan(compass_rad); |
| higedura | 0:813b917360ac | 348 | //compass_rad = compass_rad-compass_basis_rad; |
| higedura | 0:813b917360ac | 349 | compass_deg = compass_rad*180/pi; |
| higedura | 0:813b917360ac | 350 | */ |
| higedura | 0:813b917360ac | 351 | // LWMA (Observation) |
| higedura | 0:813b917360ac | 352 | p_zLWMA = LWMA(acc); |
| higedura | 0:813b917360ac | 353 | for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; } |
| higedura | 0:813b917360ac | 354 | // LWMA angle |
| higedura | 0:813b917360ac | 355 | //angle_acc[0] = asin(zLWMA[1])*180/pi; |
| higedura | 0:813b917360ac | 356 | //angle_acc[1] = asin(zLWMA[0])*180/pi; |
| higedura | 0:813b917360ac | 357 | |
| higedura | 0:813b917360ac | 358 | // RK4 (Prediction) |
| higedura | 0:813b917360ac | 359 | p_RK4 = RK4(dt,angle_rad,gyro_rad); |
| higedura | 0:813b917360ac | 360 | for( int i=0;i<N;i++ ){ angle_rad[i] = *p_RK4; p_RK4 = p_RK4+1; } |
| higedura | 0:813b917360ac | 361 | |
| higedura | 0:813b917360ac | 362 | // EKF (Correction) |
| higedura | 0:813b917360ac | 363 | EKF_predict(angle_rad,gyro_rad); |
| higedura | 0:813b917360ac | 364 | if ( correct_loop>=correct_period ){ |
| higedura | 0:813b917360ac | 365 | p_EKF = EKF_correct(angle_rad,gyro_rad,zLWMA); |
| higedura | 0:813b917360ac | 366 | for ( int i=0;i<N;i++ ){ angle_rad[i] = *p_EKF; p_EKF = p_EKF+1; } |
| higedura | 0:813b917360ac | 367 | correct_loop = 0; |
| higedura | 0:813b917360ac | 368 | } |
| higedura | 0:813b917360ac | 369 | correct_loop++; |
| higedura | 0:813b917360ac | 370 | |
| higedura | 0:813b917360ac | 371 | for( int i=0;i<N;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; } |
| higedura | 0:813b917360ac | 372 | |
| higedura | 0:813b917360ac | 373 | state[0] = angle_deg[0]; state[1] = angle_deg[1]; |
| higedura | 0:813b917360ac | 374 | state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2]; |
| higedura | 0:813b917360ac | 375 | |
| higedura | 0:813b917360ac | 376 | for( int i=0;i<M;i++ ){ |
| higedura | 0:813b917360ac | 377 | motor_attitude[i] = 0; |
| higedura | 0:813b917360ac | 378 | for( int j=0;j<L;j++ ){ motor_attitude[i] += -Kr[i][j]*state[j]; } |
| higedura | 0:813b917360ac | 379 | } |
| higedura | 0:813b917360ac | 380 | |
| higedura | 2:307500c991dd | 381 | motor_control[0] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]+1*Ccom_y*command[3]; |
| higedura | 2:307500c991dd | 382 | motor_control[1] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3]; |
| higedura | 2:307500c991dd | 383 | motor_control[2] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3]; |
| higedura | 2:307500c991dd | 384 | motor_control[3] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]-1*Ccom_y*command[3]; |
| higedura | 2:307500c991dd | 385 | motor_control[4] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3]; |
| higedura | 2:307500c991dd | 386 | motor_control[5] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3]; |
| higedura | 2:307500c991dd | 387 | |
| higedura | 2:307500c991dd | 388 | for( int i=0;i<M;i++ ){ motor[i] = motor_control[i]+motor_attitude[i]*Cfp; } |
| higedura | 1:8048a8bcde59 | 389 | //for( int i=0;i<M;i++ ){ motor[i] = command[0]; } |
| higedura | 0:813b917360ac | 390 | |
| higedura | 0:813b917360ac | 391 | // pulsewidth 0.0011~0.0019 (0.001~0.002?) |
| higedura | 0:813b917360ac | 392 | for( int i=0;i<M;i++ ){ |
| higedura | 0:813b917360ac | 393 | if( motor[i]>0.00195 ){ motor[i]=0.00195; } |
| higedura | 0:813b917360ac | 394 | if( motor[i]<0.0011 ){ motor[i]=0.00105; } |
| higedura | 0:813b917360ac | 395 | if( command[0]<0.0011 ){ motor[i]=0.00105; } |
| higedura | 0:813b917360ac | 396 | } |
| higedura | 0:813b917360ac | 397 | |
| higedura | 0:813b917360ac | 398 | ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]); |
| higedura | 0:813b917360ac | 399 | |
| higedura | 2:307500c991dd | 400 | //for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; } |
| higedura | 0:813b917360ac | 401 | |
| higedura | 0:813b917360ac | 402 | //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]); |
| higedura | 0:813b917360ac | 403 | //pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000); |
| higedura | 1:8048a8bcde59 | 404 | //fprintf(fp,"%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %6.3f\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], command[0]*1000); |
| higedura | 1:8048a8bcde59 | 405 | fprintf(fp,"%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000); |
| higedura | 0:813b917360ac | 406 | if( xbee_loop>=xbee_period ){ |
| higedura | 1:8048a8bcde59 | 407 | xbee.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000); |
| higedura | 0:813b917360ac | 408 | xbee_loop = 0; |
| higedura | 0:813b917360ac | 409 | } |
| higedura | 0:813b917360ac | 410 | xbee_loop++; |
| higedura | 0:813b917360ac | 411 | wait(dt_wait); |
| higedura | 0:813b917360ac | 412 | t += dt; |
| higedura | 0:813b917360ac | 413 | |
| higedura | 0:813b917360ac | 414 | } |
| higedura | 0:813b917360ac | 415 | |
| higedura | 0:813b917360ac | 416 | // pulsewidth 0.0011~0.00195 |
| higedura | 0:813b917360ac | 417 | ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001); |
| higedura | 0:813b917360ac | 418 | led1 = 0; led2 = 0; led3 = 0; led4 = 0; |
| higedura | 0:813b917360ac | 419 | fclose(fp); |
| higedura | 0:813b917360ac | 420 | |
| higedura | 0:813b917360ac | 421 | } |
| higedura | 0:813b917360ac | 422 | |
| higedura | 0:813b917360ac | 423 | double* EKF_predict( double y[N], double x[N] ){ |
| higedura | 0:813b917360ac | 424 | // x = F * x; |
| higedura | 0:813b917360ac | 425 | // P = F * P * F' + G * Q * G'; |
| higedura | 0:813b917360ac | 426 | |
| higedura | 0:813b917360ac | 427 | //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} }; |
| higedura | 1:8048a8bcde59 | 428 | double Q[N][N] = { {5198, 0, 0}, {0, 5518, 0}, {0, 0, 5722} }; |
| higedura | 0:813b917360ac | 429 | |
| higedura | 0:813b917360ac | 430 | double Fjac[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}}; |
| higedura | 0:813b917360ac | 431 | double Fjac_t[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}}; |
| higedura | 0:813b917360ac | 432 | double Gjac[N][N] = {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}}; |
| higedura | 0:813b917360ac | 433 | double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}}; |
| higedura | 0:813b917360ac | 434 | |
| higedura | 0:813b917360ac | 435 | double FPF[N][N] = {0}; |
| higedura | 0:813b917360ac | 436 | double GQG[N][N] = {0}; |
| higedura | 0:813b917360ac | 437 | |
| higedura | 0:813b917360ac | 438 | double FP[N][N] = {0}; |
| higedura | 0:813b917360ac | 439 | double GQ[N][N] = {0}; |
| higedura | 0:813b917360ac | 440 | |
| higedura | 0:813b917360ac | 441 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 442 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 443 | for( int k=0;k<N;k++ ){ |
| higedura | 0:813b917360ac | 444 | FP[i][j] += Fjac[i][k]*P[k][j]; |
| higedura | 0:813b917360ac | 445 | GQ[i][j] += Gjac[i][k]*Q[k][j]; |
| higedura | 0:813b917360ac | 446 | |
| higedura | 0:813b917360ac | 447 | } |
| higedura | 0:813b917360ac | 448 | } |
| higedura | 0:813b917360ac | 449 | } |
| higedura | 0:813b917360ac | 450 | |
| higedura | 0:813b917360ac | 451 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 452 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 453 | for( int k=0;k<N;k++ ){ |
| higedura | 0:813b917360ac | 454 | FPF[i][j] += FP[i][k]*Fjac_t[k][j]; |
| higedura | 0:813b917360ac | 455 | GQG[i][j] += GQ[i][k]*Gjac_t[k][j]; |
| higedura | 0:813b917360ac | 456 | } |
| higedura | 0:813b917360ac | 457 | } |
| higedura | 0:813b917360ac | 458 | } |
| higedura | 0:813b917360ac | 459 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 460 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 461 | P[i][j] = FPF[i][j]+GQG[i][j]; |
| higedura | 0:813b917360ac | 462 | } |
| higedura | 0:813b917360ac | 463 | } |
| higedura | 0:813b917360ac | 464 | |
| higedura | 0:813b917360ac | 465 | return 0; |
| higedura | 0:813b917360ac | 466 | |
| higedura | 0:813b917360ac | 467 | } |
| higedura | 0:813b917360ac | 468 | |
| higedura | 0:813b917360ac | 469 | double* EKF_correct( double y[N], double x[N], double z[N] ){ |
| higedura | 0:813b917360ac | 470 | // K = P * H' / ( H * P * H' + R ) |
| higedura | 0:813b917360ac | 471 | // x = x + K * ( yobs(t) - H * x ) |
| higedura | 0:813b917360ac | 472 | // P = P - K * H * P |
| higedura | 0:813b917360ac | 473 | |
| higedura | 0:813b917360ac | 474 | //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} }; |
| higedura | 1:8048a8bcde59 | 475 | //double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} }; |
| higedura | 1:8048a8bcde59 | 476 | double R[N][N] = { {272528, 0, 0}, {0, 295812, 0}, {0, 0, 908451} }; |
| higedura | 0:813b917360ac | 477 | |
| higedura | 0:813b917360ac | 478 | double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}}; |
| higedura | 0:813b917360ac | 479 | double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}}; |
| higedura | 0:813b917360ac | 480 | double K[N][N] = {0}; // Kalman gain |
| higedura | 0:813b917360ac | 481 | double K_deno[N][N] = {0}; // Denominator of the kalman gain |
| higedura | 0:813b917360ac | 482 | double det_K_deno_inv = 0; |
| higedura | 0:813b917360ac | 483 | double K_deno_inv[N][N] = {0}; |
| higedura | 0:813b917360ac | 484 | double HPH[N][N] = {0}; |
| higedura | 0:813b917360ac | 485 | double HP[N][N] = {0}; |
| higedura | 0:813b917360ac | 486 | double PH[N][N] = {0}; |
| higedura | 0:813b917360ac | 487 | double KHP[N][N] = {0}; |
| higedura | 0:813b917360ac | 488 | |
| higedura | 0:813b917360ac | 489 | double Hx[N] = {0}; |
| higedura | 0:813b917360ac | 490 | double z_Hx[N] = {0}; |
| higedura | 0:813b917360ac | 491 | double Kz_Hx[N] = {0}; |
| higedura | 0:813b917360ac | 492 | |
| higedura | 0:813b917360ac | 493 | double* py = y; |
| higedura | 0:813b917360ac | 494 | |
| higedura | 0:813b917360ac | 495 | // Kalman gain |
| higedura | 0:813b917360ac | 496 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 497 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 498 | for( int k=0;k<N;k++ ){ |
| higedura | 0:813b917360ac | 499 | HP[i][j] += Hjac[i][k]*P[k][j]; |
| higedura | 0:813b917360ac | 500 | PH[i][j] += P[i][k]*Hjac_t[k][j]; |
| higedura | 0:813b917360ac | 501 | } |
| higedura | 0:813b917360ac | 502 | } |
| higedura | 0:813b917360ac | 503 | } |
| higedura | 0:813b917360ac | 504 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 505 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 506 | for( int k=0;k<N;k++ ){ |
| higedura | 0:813b917360ac | 507 | HPH[i][j] += HP[i][k]*Hjac_t[k][j]; |
| higedura | 0:813b917360ac | 508 | } |
| higedura | 0:813b917360ac | 509 | } |
| higedura | 0:813b917360ac | 510 | } |
| higedura | 0:813b917360ac | 511 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 512 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 513 | K_deno[i][j] = HPH[i][j]+R[i][j]; |
| higedura | 0:813b917360ac | 514 | } |
| higedura | 0:813b917360ac | 515 | } |
| higedura | 0:813b917360ac | 516 | // Inverce matrix |
| higedura | 0:813b917360ac | 517 | det_K_deno_inv = K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2]; |
| higedura | 0:813b917360ac | 518 | K_deno_inv[0][0] = (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 519 | K_deno_inv[0][1] = (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 520 | K_deno_inv[0][2] = (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 521 | K_deno_inv[1][0] = (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 522 | K_deno_inv[1][1] = (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 523 | K_deno_inv[1][2] = (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 524 | K_deno_inv[2][0] = (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 525 | K_deno_inv[2][1] = (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 526 | K_deno_inv[2][2] = (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv; |
| higedura | 0:813b917360ac | 527 | |
| higedura | 0:813b917360ac | 528 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 529 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 530 | for( int k=0;k<N;k++ ){ |
| higedura | 0:813b917360ac | 531 | K[i][j] += PH[i][k]*K_deno_inv[k][j]; |
| higedura | 0:813b917360ac | 532 | } |
| higedura | 0:813b917360ac | 533 | } |
| higedura | 0:813b917360ac | 534 | } |
| higedura | 0:813b917360ac | 535 | |
| higedura | 0:813b917360ac | 536 | // Filtering |
| higedura | 0:813b917360ac | 537 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 538 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 539 | Hx[i] += Hjac[i][j]*y[j]; |
| higedura | 0:813b917360ac | 540 | } |
| higedura | 0:813b917360ac | 541 | } |
| higedura | 0:813b917360ac | 542 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 543 | z_Hx[i] = z[i]-Hx[i]; |
| higedura | 0:813b917360ac | 544 | } |
| higedura | 0:813b917360ac | 545 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 546 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 547 | Kz_Hx[i] += K[i][j]*z_Hx[j]; |
| higedura | 0:813b917360ac | 548 | } |
| higedura | 0:813b917360ac | 549 | } |
| higedura | 0:813b917360ac | 550 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 551 | y[i] = y[i]+Kz_Hx[i]; |
| higedura | 0:813b917360ac | 552 | } |
| higedura | 0:813b917360ac | 553 | |
| higedura | 0:813b917360ac | 554 | // Covarience |
| higedura | 0:813b917360ac | 555 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 556 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 557 | for( int k=0;k<N;k++ ){ |
| higedura | 0:813b917360ac | 558 | KHP[i][j] += K[i][k]*HP[k][j]; |
| higedura | 0:813b917360ac | 559 | } |
| higedura | 0:813b917360ac | 560 | } |
| higedura | 0:813b917360ac | 561 | } |
| higedura | 0:813b917360ac | 562 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 563 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 564 | P[i][j] = P[i][j]-KHP[i][j]; |
| higedura | 0:813b917360ac | 565 | } |
| higedura | 0:813b917360ac | 566 | } |
| higedura | 0:813b917360ac | 567 | |
| higedura | 0:813b917360ac | 568 | return py; |
| higedura | 0:813b917360ac | 569 | |
| higedura | 0:813b917360ac | 570 | } |
| higedura | 0:813b917360ac | 571 | |
| higedura | 0:813b917360ac | 572 | double* LWMA( double z[N] ){ |
| higedura | 0:813b917360ac | 573 | |
| higedura | 0:813b917360ac | 574 | double zLWMA[N] = {0}; |
| higedura | 0:813b917360ac | 575 | double zLWMA_num[N] = {0}; |
| higedura | 0:813b917360ac | 576 | double zLWMA_den = 0; |
| higedura | 0:813b917360ac | 577 | |
| higedura | 0:813b917360ac | 578 | double* pzLWMA = zLWMA; |
| higedura | 0:813b917360ac | 579 | |
| higedura | 0:813b917360ac | 580 | for( int i=1;i<N_LWMA;i++ ){ |
| higedura | 0:813b917360ac | 581 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 582 | z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1]; |
| higedura | 0:813b917360ac | 583 | } |
| higedura | 0:813b917360ac | 584 | } |
| higedura | 0:813b917360ac | 585 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 586 | z_buf[i][0] = z[i]; |
| higedura | 0:813b917360ac | 587 | } |
| higedura | 0:813b917360ac | 588 | |
| higedura | 0:813b917360ac | 589 | for( int i=0;i<N_LWMA;i++ ){ |
| higedura | 0:813b917360ac | 590 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 591 | zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i]; |
| higedura | 0:813b917360ac | 592 | } |
| higedura | 0:813b917360ac | 593 | zLWMA_den += N_LWMA-i; |
| higedura | 0:813b917360ac | 594 | } |
| higedura | 0:813b917360ac | 595 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 596 | zLWMA[i] = zLWMA_num[i]/zLWMA_den; |
| higedura | 0:813b917360ac | 597 | } |
| higedura | 0:813b917360ac | 598 | |
| higedura | 0:813b917360ac | 599 | return pzLWMA; |
| higedura | 0:813b917360ac | 600 | |
| higedura | 0:813b917360ac | 601 | } |
| higedura | 0:813b917360ac | 602 | |
| higedura | 0:813b917360ac | 603 | double* RK4( double dt, double y[N], double x[N] ){ |
| higedura | 0:813b917360ac | 604 | |
| higedura | 0:813b917360ac | 605 | double yBuf[N] = {0}; |
| higedura | 0:813b917360ac | 606 | double k[N][4] = {0}; |
| higedura | 0:813b917360ac | 607 | |
| higedura | 0:813b917360ac | 608 | double* p_y = y; |
| higedura | 0:813b917360ac | 609 | |
| higedura | 0:813b917360ac | 610 | double* pk1; |
| higedura | 0:813b917360ac | 611 | double* pk2; |
| higedura | 0:813b917360ac | 612 | double* pk3; |
| higedura | 0:813b917360ac | 613 | double* pk4; |
| higedura | 0:813b917360ac | 614 | |
| higedura | 0:813b917360ac | 615 | for( int i=0;i<N;i++){ yBuf[i] = y[i]; } |
| higedura | 0:813b917360ac | 616 | pk1 = func (yBuf,x); |
| higedura | 0:813b917360ac | 617 | for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; } |
| higedura | 0:813b917360ac | 618 | |
| higedura | 0:813b917360ac | 619 | for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; } |
| higedura | 0:813b917360ac | 620 | pk2 = func (yBuf,x); |
| higedura | 0:813b917360ac | 621 | for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; } |
| higedura | 0:813b917360ac | 622 | |
| higedura | 0:813b917360ac | 623 | for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; } |
| higedura | 0:813b917360ac | 624 | pk3 = func (yBuf,x); |
| higedura | 0:813b917360ac | 625 | for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; } |
| higedura | 0:813b917360ac | 626 | |
| higedura | 0:813b917360ac | 627 | for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; } |
| higedura | 0:813b917360ac | 628 | pk4 = func (yBuf,x); |
| higedura | 0:813b917360ac | 629 | for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; } |
| higedura | 0:813b917360ac | 630 | |
| higedura | 0:813b917360ac | 631 | for( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; } |
| higedura | 0:813b917360ac | 632 | |
| higedura | 0:813b917360ac | 633 | return p_y; |
| higedura | 0:813b917360ac | 634 | |
| higedura | 0:813b917360ac | 635 | } |
| higedura | 0:813b917360ac | 636 | |
| higedura | 0:813b917360ac | 637 | double* func( double y[N], double x[N] ){ |
| higedura | 0:813b917360ac | 638 | |
| higedura | 0:813b917360ac | 639 | double f[N] = {0}; |
| higedura | 0:813b917360ac | 640 | double* p_f = f; |
| higedura | 0:813b917360ac | 641 | |
| higedura | 0:813b917360ac | 642 | f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]); |
| higedura | 0:813b917360ac | 643 | f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]); |
| higedura | 0:813b917360ac | 644 | f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]); |
| higedura | 0:813b917360ac | 645 | |
| higedura | 0:813b917360ac | 646 | return p_f; |
| higedura | 0:813b917360ac | 647 | |
| higedura | 0:813b917360ac | 648 | } |
| higedura | 0:813b917360ac | 649 | |
| higedura | 0:813b917360ac | 650 | double* calib(){ |
| higedura | 0:813b917360ac | 651 | |
| higedura | 0:813b917360ac | 652 | int calib_loop = 1000; |
| higedura | 0:813b917360ac | 653 | |
| higedura | 0:813b917360ac | 654 | int bit_acc[N] = {0}; // Buffer of the accelerometer |
| higedura | 0:813b917360ac | 655 | int get_mag[N] = {0}; // Buffer of the compass |
| higedura | 0:813b917360ac | 656 | |
| higedura | 0:813b917360ac | 657 | double calib_acc[N] = {0}; |
| higedura | 0:813b917360ac | 658 | double calib_gyro_buf[N] = {0}; |
| higedura | 0:813b917360ac | 659 | double calib_gyro[N] = {0}; |
| higedura | 0:813b917360ac | 660 | double compass_basis_buf[N] = {0}; |
| higedura | 0:813b917360ac | 661 | double compass_basis_rad = 0; |
| higedura | 0:813b917360ac | 662 | double calib_result[7] = {0}; |
| higedura | 0:813b917360ac | 663 | |
| higedura | 0:813b917360ac | 664 | double* p_calib_result = calib_result; |
| higedura | 0:813b917360ac | 665 | |
| higedura | 0:813b917360ac | 666 | pc.printf("Don't touch... Calibrating now!!\n\r"); |
| higedura | 0:813b917360ac | 667 | xbee.printf("Don't touch... Calibrating now!!\n\r"); |
| higedura | 0:813b917360ac | 668 | led1 = 1; |
| higedura | 0:813b917360ac | 669 | |
| higedura | 0:813b917360ac | 670 | for( int i=0;i<calib_loop;i++ ){ |
| higedura | 0:813b917360ac | 671 | |
| higedura | 0:813b917360ac | 672 | accelerometer.getOutput(bit_acc); |
| higedura | 0:813b917360ac | 673 | compass.readData(get_mag); |
| higedura | 0:813b917360ac | 674 | |
| higedura | 0:813b917360ac | 675 | calib_gyro_buf[0] = gyro.getGyroX(); |
| higedura | 0:813b917360ac | 676 | calib_gyro_buf[1] = gyro.getGyroY(); |
| higedura | 0:813b917360ac | 677 | calib_gyro_buf[2] = gyro.getGyroZ(); |
| higedura | 0:813b917360ac | 678 | |
| higedura | 0:813b917360ac | 679 | for( int j=0;j<N;j++ ){ |
| higedura | 0:813b917360ac | 680 | calib_acc[j] += (int16_t)bit_acc[j]; |
| higedura | 0:813b917360ac | 681 | calib_gyro[j] += calib_gyro_buf[j]; |
| higedura | 0:813b917360ac | 682 | compass_basis_buf[j] += (int16_t)get_mag[j]; |
| higedura | 0:813b917360ac | 683 | } |
| higedura | 0:813b917360ac | 684 | |
| higedura | 0:813b917360ac | 685 | if( i>calib_loop*3/4 ){ |
| higedura | 0:813b917360ac | 686 | led4 = 1; |
| higedura | 0:813b917360ac | 687 | }else if( i>calib_loop/2 ){ |
| higedura | 0:813b917360ac | 688 | led3 = 1; |
| higedura | 0:813b917360ac | 689 | }else if( i>calib_loop/4 ){ |
| higedura | 0:813b917360ac | 690 | led2 = 1; |
| higedura | 0:813b917360ac | 691 | } |
| higedura | 0:813b917360ac | 692 | |
| higedura | 0:813b917360ac | 693 | } |
| higedura | 0:813b917360ac | 694 | |
| higedura | 0:813b917360ac | 695 | for( int i=0;i<N;i++ ){ |
| higedura | 0:813b917360ac | 696 | calib_acc[i] = calib_acc[i]/calib_loop; |
| higedura | 0:813b917360ac | 697 | calib_gyro[i] = calib_gyro[i]/calib_loop; |
| higedura | 0:813b917360ac | 698 | compass_basis_buf[i] = compass_basis_buf[i]/calib_loop; |
| higedura | 0:813b917360ac | 699 | } |
| higedura | 0:813b917360ac | 700 | |
| higedura | 0:813b917360ac | 701 | compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0]; |
| higedura | 0:813b917360ac | 702 | compass_basis_rad = atan(compass_basis_rad); |
| higedura | 0:813b917360ac | 703 | led1 = 0; led2 = 0; led3 = 0; led4 = 0; |
| higedura | 0:813b917360ac | 704 | |
| higedura | 0:813b917360ac | 705 | pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); |
| higedura | 0:813b917360ac | 706 | xbee.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r"); |
| higedura | 0:813b917360ac | 707 | pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi); |
| higedura | 0:813b917360ac | 708 | xbee.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi); |
| higedura | 0:813b917360ac | 709 | |
| higedura | 0:813b917360ac | 710 | for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; } |
| higedura | 0:813b917360ac | 711 | for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; } |
| higedura | 0:813b917360ac | 712 | calib_result[6] = compass_basis_rad; |
| higedura | 0:813b917360ac | 713 | |
| higedura | 0:813b917360ac | 714 | if( calib_result[0]==0 && calib_result[1]==0 && calib_result[2]==0 ){ |
| higedura | 0:813b917360ac | 715 | pc.printf("Accelerometer is not available.\r\n"); |
| higedura | 0:813b917360ac | 716 | xbee.printf("Accelerometer is not available.\r\n"); |
| higedura | 0:813b917360ac | 717 | } |
| higedura | 0:813b917360ac | 718 | |
| higedura | 0:813b917360ac | 719 | for( int i=0;i<3;i++ ){ |
| higedura | 0:813b917360ac | 720 | wait(.5); |
| higedura | 0:813b917360ac | 721 | led1 = 1; led2 = 1; led3 = 1; led4 = 1; |
| higedura | 0:813b917360ac | 722 | wait(.5); |
| higedura | 0:813b917360ac | 723 | led1 = 0; led2 = 0; led3 = 0; led4 = 0; |
| higedura | 0:813b917360ac | 724 | } |
| higedura | 0:813b917360ac | 725 | |
| higedura | 0:813b917360ac | 726 | return p_calib_result; |
| higedura | 0:813b917360ac | 727 | |
| higedura | 0:813b917360ac | 728 | } |