main.cpp

00001 #include "mbed.h"
00002 #include "FXOS8700.h"
00003 #include "FXAS21002.h"
00004 #include "math.h"
00005 #include "stdio.h"
00006 
00007 #define MAINWAIT 1
00008 #define CALIBTIMES 50
00009 #define SAMPLEPERIOD_S 3
00010 #define MOVINGAVRBUFSIZE 10
00011 #define MOVINGENDBUFSIZE 32
00012 #define TRENDPROTECTBUFSIZE 60
00013 #define YMOVENDTHRESHOLD 0.09
00014 #define ZMOVENDTHRESHOLD 0.09
00015 #define YMOVENDBIASNUM 30
00016 #define ZMOVENDBIASNUM 30
00017 #define XTRENDPROTECTTHRESHOLD 0.05
00018 #define YTRENDPROTECTTHRESHOLD 0.05
00019 #define MAXBUFFERSIZE 3000
00020 #define XMECHANICAL 0.005
00021 #define YMECHANICAL 0.005
00022 #define ZMECHANICAL 0.005
00023 
00024 // Initialize Serial port
00025 Serial pc(USBTX, USBRX);
00026 
00027 // Pin connections & address for Hexiwear
00028 FXOS8700 accel(PTC11, PTC10);
00029 FXOS8700 mag(PTC11, PTC10);
00030 FXAS21002 gyro(PTC11,PTC10);
00031 
00032 int calibFlag = 0;
00033 
00034 void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) {
00035     cur_speed[0] = cur_speed[0] + duration * instant_accel[0];
00036     cur_speed[1] = cur_speed[1] + duration * instant_accel[1];
00037     cur_speed[2] = cur_speed[2] + duration * instant_accel[2];
00038 }
00039 
00040 void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data) {
00041     accel.acquire_accel_data_g(accel_data);
00042     gyro.acquire_gyro_data_dps(gyro_data);
00043     int precision = 1000;
00044     
00045     if (calibFlag && (calib_data != NULL)) {
00046         int i;
00047         for (i = 0; i < 3; i++) {
00048             accel_data[i] -= calib_data[i];
00049             gyro_data[i] -= calib_data[i + 3];
00050 
00051             if (accel_data[i] < 0) {
00052                 accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision);
00053             } else {
00054                 accel_data[i] = floor(precision*accel_data[i])/precision;
00055             }
00056             
00057             if (gyro_data[i] < 0) {
00058                 gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision);
00059             } else {
00060                 gyro_data[i] = floor(precision*gyro_data[i])/precision;
00061             }
00062         }
00063     }
00064 }
00065 
00066 //void apply_mechanical_filter(float *accel_data) {
00067 //    if (abs(accel_data[1]) < (float) YMECHANICAL) {
00068 //        accel_data[1] = 0.0f;
00069 //    }
00070 //    
00071 //    if (abs(accel_data[2]) < (float) ZMECHANICAL) {
00072 //        accel_data[2] = 0.0f;
00073 //    }
00074 //}
00075     
00076 void get_caliberate_data(float *caliberate) {
00077     int i;
00078     int j;
00079     float accel_data[3];
00080     float gyro_data[3];
00081     for (i = 0; i < CALIBTIMES; i++) {
00082         acquire_sensor_data(accel_data, gyro_data, NULL);
00083         for (j = 0; j < 3; j++) {
00084             caliberate[j]     += accel_data[j];
00085             caliberate[j + 3] += gyro_data[j];
00086         }
00087         wait_ms(MAINWAIT);
00088     }
00089     
00090     for (i = 0; i < 6; i++) {
00091            caliberate[i] /= (float)CALIBTIMES;
00092     }
00093 }
00094 
00095 void load_buffer(float **all_data, float *accel_data, float time, int i) {
00096     if (i == MAXBUFFERSIZE) {
00097         pc.printf("Buffer full!\n\r");
00098         exit(1);
00099     }
00100     
00101     all_data[0][i] = time;
00102     all_data[1][i] = accel_data[0];
00103     all_data[2][i] = accel_data[1];
00104     all_data[3][i] = accel_data[2];
00105 }
00106 
00107 void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples, 
00108                          float *last_total, float **all_data, float *caliberate, 
00109                          Timer *t) {
00110     int i;
00111     float accel_data[3];
00112     float gyro_data[3];
00113     float time;
00114     float total_accelx = 0.0f;
00115     float total_accely = 0.0f;
00116     float total_accelz = 0.0f;
00117     
00118     for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
00119         acquire_sensor_data(accel_data, gyro_data, caliberate);
00120         time = t->read();
00121         moving_avg_buf[0][i] = accel_data[0];
00122         moving_avg_buf[1][i] = accel_data[1];
00123         moving_avg_buf[2][i] = accel_data[2];
00124         moving_avg_buf[3][i] = time;
00125         wait_ms(MAINWAIT);
00126     }
00127 
00128     for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
00129         total_accelx += moving_avg_buf[0][i];
00130         total_accely += moving_avg_buf[1][i];
00131         total_accelz += moving_avg_buf[2][i];
00132     }
00133     
00134     last_total[0] = total_accelx;
00135     last_total[1] = total_accely;
00136     last_total[2] = total_accelz;
00137     
00138     all_data[0][0] = moving_avg_buf[3][0];
00139     all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE;
00140     all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE;
00141     all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE;
00142 
00143     (*num_samples)++;
00144 }
00145 
00146 void get_new_moving_average_point(float **all_data, float *last_total, 
00147                                 float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE], 
00148                                 float time, int *num_samples, int *start, 
00149                                 int *end) {
00150     
00151     last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0];
00152     last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1];
00153     last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2];
00154     
00155     all_data[0][*num_samples] = moving_avg_buf[3][*start];
00156     
00157     float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE;
00158     float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE;
00159     float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE;
00160     
00161     if (abs(temp_x) > (float) XMECHANICAL) {
00162         all_data[1][*num_samples] = temp_x;
00163     } else {
00164         all_data[1][*num_samples] = 0.0f;
00165     }
00166     
00167     if (abs(temp_y) > (float) YMECHANICAL) {
00168         all_data[2][*num_samples] = temp_y;
00169     } else {
00170         all_data[2][*num_samples] = 0.0f;
00171     }
00172     
00173     if (abs(temp_z) > (float) ZMECHANICAL) {
00174         all_data[3][*num_samples] = temp_z;
00175     } else {
00176         all_data[3][*num_samples] = 0.0f;
00177     }
00178     
00179     *start = (*start + 1) % (int) MOVINGAVRBUFSIZE;
00180     *end = (*end + 1) % (int) MOVINGAVRBUFSIZE;
00181     
00182     moving_avg_buf[0][*end] = accel_data[0];
00183     moving_avg_buf[1][*end] = accel_data[1];
00184     moving_avg_buf[2][*end] = accel_data[2];
00185     moving_avg_buf[3][*end] = time;
00186     
00187     (*num_samples) += 1;
00188 }
00189 
00190 void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
00191                         float *additional_to_vel, float duration) {
00192     
00193     if (num_samples > TRENDPROTECTBUFSIZE) {
00194         total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1];
00195         total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1];
00196         
00197         total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
00198         total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
00199         
00200         if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) {
00201             additional_to_vel[0] = -1000.0f;
00202         } else {
00203             float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
00204             additional_to_vel[0] = avg_accel * duration;
00205         }
00206         
00207         if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) {
00208             additional_to_vel[1] = -1000.0f;
00209         } else {
00210             float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
00211             additional_to_vel[1] = avg_accel * duration;
00212         }
00213         
00214     } else {
00215         if(num_samples == 1){
00216         } else {
00217             total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
00218             total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
00219         }
00220     }
00221 }
00222         
00223     
00224 
00225 void apply_move_end_check(float **all_data, int num_samples, 
00226                         int moving_end_buf[][MOVINGENDBUFSIZE],
00227                         int *num_unqualified, int *start, int *end,
00228                         float *addition_to_vel, float duration,
00229                         float *total_diff) {
00230 
00231     if (num_samples > MOVINGENDBUFSIZE) {
00232         num_unqualified[0] -= moving_end_buf[0][*start];
00233         num_unqualified[1] -= moving_end_buf[1][*start];
00234         *start = (*start + 1) % MOVINGENDBUFSIZE;
00235         *end = (*end + 1) % MOVINGENDBUFSIZE;
00236         
00237         if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){
00238             num_unqualified[0] += 1;
00239             moving_end_buf[0][*end] = 1;
00240         } else {
00241             moving_end_buf[0][*end] = 0;
00242         }
00243         
00244         if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){
00245             num_unqualified[1] += 1;
00246             moving_end_buf[1][*end] = 1;
00247         } else {
00248             moving_end_buf[1][*end] = 0;
00249         }
00250         
00251         if (num_unqualified[0] >= (int)YMOVENDBIASNUM){
00252             addition_to_vel[0] = -1000.0f;
00253         }
00254         
00255         if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){
00256             addition_to_vel[1] = -1000.0f;
00257         }
00258         
00259         apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
00260                             duration);
00261         
00262         if (num_unqualified[0] < (int)YMOVENDBIASNUM){
00263             float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
00264             addition_to_vel[0] = avg_accel * duration;
00265         }
00266         
00267         if (num_unqualified[1] < (int)ZMOVENDBIASNUM){
00268             float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
00269             addition_to_vel[1] = avg_accel * duration;
00270         }
00271         
00272     } else if (num_samples < MOVINGENDBUFSIZE) {
00273         addition_to_vel[0] = -1000.0f;
00274         addition_to_vel[1] = -1000.0f;
00275         apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
00276                             duration);
00277     } else {
00278         int i;
00279         for (i = 0; i < MOVINGENDBUFSIZE; i++) {
00280             if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) {
00281                 moving_end_buf[0][i] = 1;
00282                 num_unqualified[0] += 1;
00283             } else {
00284                 moving_end_buf[0][i] = 0;
00285             }
00286             
00287             if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) {
00288                 moving_end_buf[1][i] = 1;
00289                 num_unqualified[1] += 1;
00290             } else {
00291                 moving_end_buf[1][i] = 0;
00292             }
00293             
00294             addition_to_vel[0] = -1000.0f;
00295             addition_to_vel[1] = -1000.0f;
00296         }
00297         apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
00298                             duration);
00299     }
00300 }
00301 
00302 void get_new_velocity (float *original_speed, float *addition_to_vel) {
00303     if (addition_to_vel[0] == -1000.0f) {
00304         original_speed[0] = 0.0f;
00305     } else {
00306         original_speed[0] += addition_to_vel[0];
00307     }
00308     
00309     if (addition_to_vel[1] == -1000.0f) {
00310         original_speed[1] = 0.0f;
00311     } else {
00312         original_speed[1] += addition_to_vel[1];
00313     }
00314 }
00315 
00316 void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
00317                               int num_samples) {
00318     
00319     vel_buffer[0][num_samples - 1] = time;
00320     vel_buffer[1][num_samples - 1] = cur_vel[0];
00321     vel_buffer[2][num_samples - 1] = cur_vel[1];
00322 }
00323 
00324 void output_all_to_serial(float **all_data, int num_samples) {
00325     int i;
00326     for (i = 0; i < num_samples; i++) {
00327         pc.printf("%6.3f,%7.5f,%7.5f,%7.5f\n",all_data[0][i],all_data[1][i],all_data[2][i],all_data[3][i]);
00328         wait(0.01);
00329     }
00330     pc.printf("Number of samples = %d\n", num_samples);
00331     pc.printf ("End Transmission!\n");
00332 }
00333 
00334 void output_speed_to_serial(float **vel_data, int num_samples) {
00335     int i;
00336     for (i = 0; i < num_samples; i++) {
00337         pc.printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]);
00338         wait(0.01);
00339     }
00340     pc.printf("Number of samples = %d\n", num_samples);
00341     pc.printf ("End Transmission!\n");
00342 }
00343 
00344 int main() {
00345     float cur_speed[2] = {0.0, 0.0};
00346     float accel_data[3];
00347     float gyro_data[3];
00348     float caliberate[6];
00349     float moving_avg_buf[4][MOVINGAVRBUFSIZE];
00350     float last_total[3];
00351     float addition_to_vel[2];
00352     float total_difference[2];
00353     int moving_end_buf[2][MOVINGENDBUFSIZE];
00354     int num_unqualified[] = {0,0};
00355     int avg_buf_start = 0;
00356     int avg_buf_end = MOVINGAVRBUFSIZE - 1;
00357     int end_buf_start = 0;
00358     int end_buf_end = MOVINGENDBUFSIZE - 1;
00359     int num_samples = 0;
00360     
00361     float **all_data;
00362     int i;
00363     all_data = (float**) malloc(4*sizeof(float*));
00364     if(all_data == NULL) {
00365         pc.printf("Error allocating memory\n");
00366         exit(1);
00367     }
00368     
00369     for(i = 0; i < 4; i++) {
00370         all_data[i] = (float*) malloc(MAXBUFFERSIZE * sizeof(float));
00371     }
00372     if(all_data[3] == NULL) {
00373         pc.printf("Error allocating memory\n");
00374         exit(1);
00375     }
00376     
00377     float **vel_data;
00378     vel_data = (float**) malloc(3*sizeof(float*));
00379     if(vel_data == NULL) {
00380         pc.printf("Error allocating memory\n");
00381         exit(1);
00382     }
00383     
00384     for(i = 0; i < 3; i++) {
00385         vel_data[i] = (float*) malloc(MAXBUFFERSIZE * sizeof(float));
00386     }
00387     if(vel_data[2] == NULL) {
00388         pc.printf("Error allocating memory\n");
00389         exit(1);
00390     }
00391     
00392     
00393     Timer t;
00394     
00395     // Configure Accelerometer FXOS8700, Magnetometer FXOS8700
00396     accel.accel_config();
00397     mag.mag_config();
00398     gyro.gyro_config();
00399     wait(0.5);
00400     get_caliberate_data(caliberate);
00401     calibFlag = 1;
00402     
00403     wait(2);
00404     pc.printf("Caliberation finished\n");
00405     wait(3);
00406     pc.printf("Start Recording!\n");
00407     
00408     t.start();
00409     init_moving_avg_buf(&moving_avg_buf[0], &num_samples, last_total, 
00410                         all_data, caliberate, &t);
00411 
00412     float cur_time = t.read();
00413     float last_time = cur_time;
00414     float duration = 0;
00415     
00416     while (cur_time < (float)SAMPLEPERIOD_S) {
00417         //t.reset();
00418         
00419         last_time = cur_time;
00420         acquire_sensor_data(accel_data, gyro_data, caliberate);
00421         cur_time = t.read();
00422         duration = cur_time - last_time;
00423         
00424         get_new_moving_average_point(all_data, last_total, accel_data, &moving_avg_buf[0],
00425                                     cur_time, &num_samples, &avg_buf_start, &avg_buf_end);
00426                                     
00427         apply_move_end_check(all_data, num_samples, &moving_end_buf[0], num_unqualified, 
00428                             &end_buf_start, &end_buf_end, addition_to_vel, duration, 
00429                             total_difference);
00430         
00431         get_new_velocity(cur_speed, addition_to_vel);
00432         
00433         insert_new_vel_to_buffer(vel_data, cur_time, cur_speed, num_samples);
00434         
00435         //acquire_new_speed(cur_speed, last_period, accel_data);
00436         //load_buffer(all_data, accel_data, last_period, num_samples);
00437         Thread::wait(MAINWAIT);
00438     }
00439     pc.printf ("Stop Recording!\n");
00440     wait(3);
00441     
00442     output_all_to_serial(all_data, num_samples);
00443     output_speed_to_serial(vel_data, num_samples);
00444 
00445     return 0;
00446 }