david fernandez
/
Nucleo_i2c_master_David_4_
Multithread approach to 6DOF Mag Tracker
main.cpp@0:9621bbc04b9b, 2020-05-21 (annotated)
- Committer:
- darkraxx
- Date:
- Thu May 21 13:10:58 2020 +0000
- Revision:
- 0:9621bbc04b9b
Multithread approach Mag Track
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
darkraxx | 0:9621bbc04b9b | 1 | #include "mbed.h" |
darkraxx | 0:9621bbc04b9b | 2 | #include "rtos.h" |
darkraxx | 0:9621bbc04b9b | 3 | #include <math.h> |
darkraxx | 0:9621bbc04b9b | 4 | #include "MadgwickAHRS.h" |
darkraxx | 0:9621bbc04b9b | 5 | #include "Mag_track.h" |
darkraxx | 0:9621bbc04b9b | 6 | #include "Scattered_interpolator.h" |
darkraxx | 0:9621bbc04b9b | 7 | |
darkraxx | 0:9621bbc04b9b | 8 | //---------------------------------------------------------------------------------------------------- |
darkraxx | 0:9621bbc04b9b | 9 | // Global ariable declaration |
darkraxx | 0:9621bbc04b9b | 10 | DigitalOut D_1(PA_7); |
darkraxx | 0:9621bbc04b9b | 11 | DigitalOut D_2(PA_6); |
darkraxx | 0:9621bbc04b9b | 12 | Thread thread; |
darkraxx | 0:9621bbc04b9b | 13 | Ticker read_ticker; |
darkraxx | 0:9621bbc04b9b | 14 | |
darkraxx | 0:9621bbc04b9b | 15 | /////// Bounce button interrupt |
darkraxx | 0:9621bbc04b9b | 16 | InterruptIn button1(USER_BUTTON); |
darkraxx | 0:9621bbc04b9b | 17 | volatile bool button1_pressed = false; // Used in the main loop |
darkraxx | 0:9621bbc04b9b | 18 | volatile bool button1_enabled = true; // Used for debouncing |
darkraxx | 0:9621bbc04b9b | 19 | Timeout button1_timeout; // Used for debouncing |
darkraxx | 0:9621bbc04b9b | 20 | |
darkraxx | 0:9621bbc04b9b | 21 | /////// Flags |
darkraxx | 0:9621bbc04b9b | 22 | bool flag = false; |
darkraxx | 0:9621bbc04b9b | 23 | bool flag_data_ready = false; |
darkraxx | 0:9621bbc04b9b | 24 | bool go_flag = false; // 10 sec flag for Qinit set up |
darkraxx | 0:9621bbc04b9b | 25 | |
darkraxx | 0:9621bbc04b9b | 26 | /////// External signal buffers |
darkraxx | 0:9621bbc04b9b | 27 | int16_t Mag_read_buff[24];// 3*8 signed int_16 buffer read from sensor |
darkraxx | 0:9621bbc04b9b | 28 | int16_t Acc_read[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 29 | int16_t Gyr_read[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 30 | float q_init[4]={1,0,0,0}; |
darkraxx | 0:9621bbc04b9b | 31 | float q_init_n[4]={1,0,0,0};//{-0.8489, 0.0046, -0.001, 0.5286};//{0,0,0,0}; //; |
darkraxx | 0:9621bbc04b9b | 32 | |
darkraxx | 0:9621bbc04b9b | 33 | /////// Counters |
darkraxx | 0:9621bbc04b9b | 34 | int16_t count_1=1; |
darkraxx | 0:9621bbc04b9b | 35 | int16_t count_2=1; |
darkraxx | 0:9621bbc04b9b | 36 | |
darkraxx | 0:9621bbc04b9b | 37 | |
darkraxx | 0:9621bbc04b9b | 38 | |
darkraxx | 0:9621bbc04b9b | 39 | //--------------------------------------------------------------------------------------------------- |
darkraxx | 0:9621bbc04b9b | 40 | // Function declarations |
darkraxx | 0:9621bbc04b9b | 41 | |
darkraxx | 0:9621bbc04b9b | 42 | // Enables button when bouncing is over |
darkraxx | 0:9621bbc04b9b | 43 | void button1_enabled_cb(void) |
darkraxx | 0:9621bbc04b9b | 44 | { |
darkraxx | 0:9621bbc04b9b | 45 | button1_enabled = true; |
darkraxx | 0:9621bbc04b9b | 46 | } |
darkraxx | 0:9621bbc04b9b | 47 | |
darkraxx | 0:9621bbc04b9b | 48 | // ISR handling button pressed event |
darkraxx | 0:9621bbc04b9b | 49 | void button1_onpressed_cb(void) |
darkraxx | 0:9621bbc04b9b | 50 | { |
darkraxx | 0:9621bbc04b9b | 51 | if (button1_enabled) { // Disabled while the button is bouncing |
darkraxx | 0:9621bbc04b9b | 52 | button1_enabled = false; |
darkraxx | 0:9621bbc04b9b | 53 | button1_pressed = true; // To be read by the main loop |
darkraxx | 0:9621bbc04b9b | 54 | button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms |
darkraxx | 0:9621bbc04b9b | 55 | } |
darkraxx | 0:9621bbc04b9b | 56 | } |
darkraxx | 0:9621bbc04b9b | 57 | |
darkraxx | 0:9621bbc04b9b | 58 | /////// Toggle flag |
darkraxx | 0:9621bbc04b9b | 59 | void toggle_flag() { |
darkraxx | 0:9621bbc04b9b | 60 | flag = !flag; |
darkraxx | 0:9621bbc04b9b | 61 | //myled = !myled; |
darkraxx | 0:9621bbc04b9b | 62 | } |
darkraxx | 0:9621bbc04b9b | 63 | |
darkraxx | 0:9621bbc04b9b | 64 | |
darkraxx | 0:9621bbc04b9b | 65 | ////////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 66 | // Sensors reading thread |
darkraxx | 0:9621bbc04b9b | 67 | ////////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 68 | void read_thread(){ |
darkraxx | 0:9621bbc04b9b | 69 | PC.printf("Hello World 30/11/18 DAFG!\n"); |
darkraxx | 0:9621bbc04b9b | 70 | int16_t i, j; |
darkraxx | 0:9621bbc04b9b | 71 | // Raw and Filtered signals buffers |
darkraxx | 0:9621bbc04b9b | 72 | memset( Mag_read_buff, 0, sizeof( Mag_read_buff ) ); |
darkraxx | 0:9621bbc04b9b | 73 | int16_t Mag_filt[24]; // 24 signed int_16 output values from filter |
darkraxx | 0:9621bbc04b9b | 74 | int16_t Mag_filt_lp[24];// 24 signed int_16 output values from filter |
darkraxx | 0:9621bbc04b9b | 75 | int16_t M_LP[3]; // 3 signed int_16 output values from filter |
darkraxx | 0:9621bbc04b9b | 76 | // Quaternion update buffers |
darkraxx | 0:9621bbc04b9b | 77 | float a[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 78 | float g[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 79 | float m[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 80 | int32_t q[4]={0, 0, 0, 0}; |
darkraxx | 0:9621bbc04b9b | 81 | int32_t Q_out[4]={0, 0, 0, 0}; |
darkraxx | 0:9621bbc04b9b | 82 | float Q[4]={0,0,0,0}; |
darkraxx | 0:9621bbc04b9b | 83 | //Buffers for magnetic signal rotation |
darkraxx | 0:9621bbc04b9b | 84 | float v_I[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 85 | float v_rotX[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 86 | int16_t v_rotX_int[24]; |
darkraxx | 0:9621bbc04b9b | 87 | memset( v_rotX_int, 0, sizeof( v_rotX_int ) ); |
darkraxx | 0:9621bbc04b9b | 88 | float Mag_filt_f[3]; |
darkraxx | 0:9621bbc04b9b | 89 | // Buffers for envelope extraction |
darkraxx | 0:9621bbc04b9b | 90 | int16_t maximum[6]={0,0,0,0,0,0}; |
darkraxx | 0:9621bbc04b9b | 91 | int16_t peak_index[6]={0,0,0,0,0,0}; |
darkraxx | 0:9621bbc04b9b | 92 | int16_t Env[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 93 | double R[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 94 | double phase[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 95 | int16_t Sign[2]={1,1}; |
darkraxx | 0:9621bbc04b9b | 96 | // ID word buffer |
darkraxx | 0:9621bbc04b9b | 97 | char data_out[4]; |
darkraxx | 0:9621bbc04b9b | 98 | data_out[0] = 0xDA; |
darkraxx | 0:9621bbc04b9b | 99 | data_out[1] = 0x51; //DA51 = 55889 |
darkraxx | 0:9621bbc04b9b | 100 | data_out[2] = 0xDA; |
darkraxx | 0:9621bbc04b9b | 101 | data_out[3] = 0xF6; // DAF6 = 56054 |
darkraxx | 0:9621bbc04b9b | 102 | |
darkraxx | 0:9621bbc04b9b | 103 | |
darkraxx | 0:9621bbc04b9b | 104 | |
darkraxx | 0:9621bbc04b9b | 105 | |
darkraxx | 0:9621bbc04b9b | 106 | while (1) { |
darkraxx | 0:9621bbc04b9b | 107 | Thread::signal_wait(0x1); |
darkraxx | 0:9621bbc04b9b | 108 | D_2=1; |
darkraxx | 0:9621bbc04b9b | 109 | flag_data_ready = false; |
darkraxx | 0:9621bbc04b9b | 110 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 111 | //// RUOTARE IL CAMPO |
darkraxx | 0:9621bbc04b9b | 112 | Q[0]=q0; Q[1]=-q1; Q[2]=-q2; Q[3]=-q3; |
darkraxx | 0:9621bbc04b9b | 113 | for(i=0;i<8;i++){ |
darkraxx | 0:9621bbc04b9b | 114 | Mag_filt_f[0]= (float)Mag_read_buff[i]; Mag_filt_f[1]= (float)Mag_read_buff[8+i]; Mag_filt_f[2]= (float)Mag_read_buff[16+i]; |
darkraxx | 0:9621bbc04b9b | 115 | Qrotate(Mag_filt_f,Q,v_I); |
darkraxx | 0:9621bbc04b9b | 116 | Qrotate(v_I,q_init,v_rotX); |
darkraxx | 0:9621bbc04b9b | 117 | v_rotX_int[i]=(int16_t)v_rotX[0]; v_rotX_int[8+i]=(int16_t)v_rotX[1]; v_rotX_int[16+i]=(int16_t)v_rotX[2]; |
darkraxx | 0:9621bbc04b9b | 118 | } |
darkraxx | 0:9621bbc04b9b | 119 | |
darkraxx | 0:9621bbc04b9b | 120 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 121 | //Filter block |
darkraxx | 0:9621bbc04b9b | 122 | for(i=0;i<3;i++){ |
darkraxx | 0:9621bbc04b9b | 123 | for(j=0;j<8;j++){ |
darkraxx | 0:9621bbc04b9b | 124 | //Band-Pass |
darkraxx | 0:9621bbc04b9b | 125 | firFixed( coeffs_bp, &v_rotX_int[i*8 + j], &Mag_filt[i*8 + j], 1, FILTER_LEN_BP, i, insamp); |
darkraxx | 0:9621bbc04b9b | 126 | //Low-Pass |
darkraxx | 0:9621bbc04b9b | 127 | firFixed( coeffs_lp, &Mag_read_buff[i*8 + j], &Mag_filt_lp[i*8 + j], 1, FILTER_LEN_LP, i,lp_insamp); |
darkraxx | 0:9621bbc04b9b | 128 | } |
darkraxx | 0:9621bbc04b9b | 129 | M_LP[i]=Mag_filt_lp[i*8 + 4]; |
darkraxx | 0:9621bbc04b9b | 130 | } |
darkraxx | 0:9621bbc04b9b | 131 | |
darkraxx | 0:9621bbc04b9b | 132 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 133 | /// ENVELOPE EXTRACTION |
darkraxx | 0:9621bbc04b9b | 134 | |
darkraxx | 0:9621bbc04b9b | 135 | Env_extraction(Env, Mag_filt, maximum, peak_index, R, phase, Sign); |
darkraxx | 0:9621bbc04b9b | 136 | |
darkraxx | 0:9621bbc04b9b | 137 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 138 | /// Quaternion computing |
darkraxx | 0:9621bbc04b9b | 139 | QUpdate(q, m, a, g, M_LP, Acc_read, Gyr_read, q_init_n, Q_out); |
darkraxx | 0:9621bbc04b9b | 140 | |
darkraxx | 0:9621bbc04b9b | 141 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 142 | ///Send serial data: 20954 [1]| Envelope[3] | -2342 [1] | Quat [8] |
darkraxx | 0:9621bbc04b9b | 143 | |
darkraxx | 0:9621bbc04b9b | 144 | if(go_flag){ |
darkraxx | 0:9621bbc04b9b | 145 | |
darkraxx | 0:9621bbc04b9b | 146 | for(i=0; i<2; i++){ //// PAROLA 20954 |
darkraxx | 0:9621bbc04b9b | 147 | PC.printf("%c",data_out[i]); |
darkraxx | 0:9621bbc04b9b | 148 | } |
darkraxx | 0:9621bbc04b9b | 149 | for(i=0; i<3; i++){ // ENVELOPE |
darkraxx | 0:9621bbc04b9b | 150 | PC.printf("%c",(char)(Env[i] & 0xff)); |
darkraxx | 0:9621bbc04b9b | 151 | PC.printf("%c",(char)(Env[i]>>8 & 0xff)); |
darkraxx | 0:9621bbc04b9b | 152 | } |
darkraxx | 0:9621bbc04b9b | 153 | for(i=2; i<4; i++){ //// PAROLA -2342 |
darkraxx | 0:9621bbc04b9b | 154 | PC.printf("%c",data_out[i]); |
darkraxx | 0:9621bbc04b9b | 155 | } |
darkraxx | 0:9621bbc04b9b | 156 | for(i=0;i<4;i++){ |
darkraxx | 0:9621bbc04b9b | 157 | PC.printf("%c",(char)(Q_out[i] & 0xff)); |
darkraxx | 0:9621bbc04b9b | 158 | PC.printf("%c",(char)(Q_out[i]>>8 & 0xff)); |
darkraxx | 0:9621bbc04b9b | 159 | PC.printf("%c",(char)(Q_out[i]>>16 & 0xff)); |
darkraxx | 0:9621bbc04b9b | 160 | PC.printf("%c",(char)(Q_out[i]>>24 & 0xff)); |
darkraxx | 0:9621bbc04b9b | 161 | } |
darkraxx | 0:9621bbc04b9b | 162 | |
darkraxx | 0:9621bbc04b9b | 163 | } |
darkraxx | 0:9621bbc04b9b | 164 | D_2=0; |
darkraxx | 0:9621bbc04b9b | 165 | |
darkraxx | 0:9621bbc04b9b | 166 | } |
darkraxx | 0:9621bbc04b9b | 167 | |
darkraxx | 0:9621bbc04b9b | 168 | } |
darkraxx | 0:9621bbc04b9b | 169 | |
darkraxx | 0:9621bbc04b9b | 170 | |
darkraxx | 0:9621bbc04b9b | 171 | ////////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 172 | // Main Code |
darkraxx | 0:9621bbc04b9b | 173 | ////////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 174 | int main(){ |
darkraxx | 0:9621bbc04b9b | 175 | PC.baud(921600); //115200 921600 |
darkraxx | 0:9621bbc04b9b | 176 | /////// Internal signal buffers |
darkraxx | 0:9621bbc04b9b | 177 | int16_t Mag_read_in_buff[24]; |
darkraxx | 0:9621bbc04b9b | 178 | memset( Mag_read_in_buff, 0, sizeof( Mag_read_in_buff ) ); |
darkraxx | 0:9621bbc04b9b | 179 | int16_t Mag_in_read[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 180 | int16_t Acc_in_read[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 181 | int16_t Gyr_in_read[3]={0,0,0}; |
darkraxx | 0:9621bbc04b9b | 182 | int i; |
darkraxx | 0:9621bbc04b9b | 183 | button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event |
darkraxx | 0:9621bbc04b9b | 184 | // Init FIR filters |
darkraxx | 0:9621bbc04b9b | 185 | firFixedInit(); |
darkraxx | 0:9621bbc04b9b | 186 | PC.printf("Press button start!\n"); |
darkraxx | 0:9621bbc04b9b | 187 | D_1=0; |
darkraxx | 0:9621bbc04b9b | 188 | D_2=0; |
darkraxx | 0:9621bbc04b9b | 189 | while (!button1_pressed){ |
darkraxx | 0:9621bbc04b9b | 190 | myled = !myled; |
darkraxx | 0:9621bbc04b9b | 191 | wait(0.1); |
darkraxx | 0:9621bbc04b9b | 192 | } |
darkraxx | 0:9621bbc04b9b | 193 | //Configure sensors |
darkraxx | 0:9621bbc04b9b | 194 | sensor_setup(); |
darkraxx | 0:9621bbc04b9b | 195 | //Start sensor reading thread |
darkraxx | 0:9621bbc04b9b | 196 | thread.start(read_thread); |
darkraxx | 0:9621bbc04b9b | 197 | //Timer begins, Freq = 800 |
darkraxx | 0:9621bbc04b9b | 198 | read_ticker.attach_us(&toggle_flag, 1250); |
darkraxx | 0:9621bbc04b9b | 199 | while(1){ |
darkraxx | 0:9621bbc04b9b | 200 | if(flag){ |
darkraxx | 0:9621bbc04b9b | 201 | D_1=1; |
darkraxx | 0:9621bbc04b9b | 202 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 203 | // Read Mag register and Calibration Correction |
darkraxx | 0:9621bbc04b9b | 204 | Read_Mag(Mag_in_read); |
darkraxx | 0:9621bbc04b9b | 205 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 206 | // Read acc + gyr register |
darkraxx | 0:9621bbc04b9b | 207 | if(count_1 == 1){ |
darkraxx | 0:9621bbc04b9b | 208 | Read_Acc(Acc_in_read); |
darkraxx | 0:9621bbc04b9b | 209 | } |
darkraxx | 0:9621bbc04b9b | 210 | if(count_1 == 5){ |
darkraxx | 0:9621bbc04b9b | 211 | Read_Gyr(Gyr_in_read); |
darkraxx | 0:9621bbc04b9b | 212 | } |
darkraxx | 0:9621bbc04b9b | 213 | |
darkraxx | 0:9621bbc04b9b | 214 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 215 | // Read Mag internal buffer fill |
darkraxx | 0:9621bbc04b9b | 216 | for(i=0;i<3;i++){ |
darkraxx | 0:9621bbc04b9b | 217 | Mag_read_in_buff[8*i + count_1] = Mag_in_read[i]; |
darkraxx | 0:9621bbc04b9b | 218 | } |
darkraxx | 0:9621bbc04b9b | 219 | |
darkraxx | 0:9621bbc04b9b | 220 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 221 | // External buffers updates and Signal Processing flag act |
darkraxx | 0:9621bbc04b9b | 222 | if(count_1 == 8){ |
darkraxx | 0:9621bbc04b9b | 223 | count_1 = 0; |
darkraxx | 0:9621bbc04b9b | 224 | for(i=0;i<24;i++){ |
darkraxx | 0:9621bbc04b9b | 225 | Mag_read_buff[i]=Mag_read_in_buff[i]; |
darkraxx | 0:9621bbc04b9b | 226 | } |
darkraxx | 0:9621bbc04b9b | 227 | for(i=0;i<3;i++){ |
darkraxx | 0:9621bbc04b9b | 228 | Acc_read[i]=Acc_in_read[i]; |
darkraxx | 0:9621bbc04b9b | 229 | Gyr_read[i]=Gyr_in_read[i]; |
darkraxx | 0:9621bbc04b9b | 230 | } |
darkraxx | 0:9621bbc04b9b | 231 | thread.signal_set(0x1); |
darkraxx | 0:9621bbc04b9b | 232 | } |
darkraxx | 0:9621bbc04b9b | 233 | |
darkraxx | 0:9621bbc04b9b | 234 | /////////////////////////////////////////////////////////// |
darkraxx | 0:9621bbc04b9b | 235 | // Counter updates and flag management |
darkraxx | 0:9621bbc04b9b | 236 | count_1++; |
darkraxx | 0:9621bbc04b9b | 237 | if (count_2==8001){ |
darkraxx | 0:9621bbc04b9b | 238 | go_flag=true; |
darkraxx | 0:9621bbc04b9b | 239 | beta = 0.1;//{-0.8489, 0.0046, -0.001, 0.5286} |
darkraxx | 0:9621bbc04b9b | 240 | q_init[0]=q0; q_init[1]=q1; q_init[2]=q2; q_init[3]=q3; |
darkraxx | 0:9621bbc04b9b | 241 | q_init_n[0]=q0; q_init_n[1]=-q1; q_init_n[2]=-q2; q_init_n[3]=-q3; |
darkraxx | 0:9621bbc04b9b | 242 | } |
darkraxx | 0:9621bbc04b9b | 243 | if (count_2 <=8001){ |
darkraxx | 0:9621bbc04b9b | 244 | count_2++; |
darkraxx | 0:9621bbc04b9b | 245 | } |
darkraxx | 0:9621bbc04b9b | 246 | |
darkraxx | 0:9621bbc04b9b | 247 | flag=0; |
darkraxx | 0:9621bbc04b9b | 248 | D_1=0; |
darkraxx | 0:9621bbc04b9b | 249 | } |
darkraxx | 0:9621bbc04b9b | 250 | } |
darkraxx | 0:9621bbc04b9b | 251 | |
darkraxx | 0:9621bbc04b9b | 252 | } |