david fernandez / Mbed 2 deprecated Nucleo_i2c_master_David_4_

Multithread approach to 6DOF Mag Tracker

Dependencies:   mbed mbed-rtos

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?

UserRevisionLine numberNew 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 }