Revision 0:b177004d9e60, committed 2018-05-25

Comitter:

xuweiqian9999

Date:

Fri May 25 23:22:31 2018 +0000

Commit message:

blah;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXAS21002.lib	Fri May 25 23:22:31 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/xuweiqian9999/code/FXAS21002/#c910725da6d1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXOS8700.lib	Fri May 25 23:22:31 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AswinSivakumar/code/FXOS8700/#98ea52282575

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 25 23:22:31 2018 +0000
@@ -0,0 +1,446 @@
+#include "mbed.h"
+#include "FXOS8700.h"
+#include "FXAS21002.h"
+#include "math.h"
+#include "stdio.h"
+
+#define MAINWAIT 1
+#define CALIBTIMES 50
+#define SAMPLEPERIOD_S 3
+#define MOVINGAVRBUFSIZE 10
+#define MOVINGENDBUFSIZE 32
+#define TRENDPROTECTBUFSIZE 60
+#define YMOVENDTHRESHOLD 0.09
+#define ZMOVENDTHRESHOLD 0.09
+#define YMOVENDBIASNUM 30
+#define ZMOVENDBIASNUM 30
+#define XTRENDPROTECTTHRESHOLD 0.05
+#define YTRENDPROTECTTHRESHOLD 0.05
+#define MAXBUFFERSIZE 3000
+#define XMECHANICAL 0.005
+#define YMECHANICAL 0.005
+#define ZMECHANICAL 0.005
+
+// Initialize Serial port
+Serial pc(USBTX, USBRX);
+
+// Pin connections & address for Hexiwear
+FXOS8700 accel(PTC11, PTC10);
+FXOS8700 mag(PTC11, PTC10);
+FXAS21002 gyro(PTC11,PTC10);
+
+int calibFlag = 0;
+
+void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) {
+    cur_speed[0] = cur_speed[0] + duration * instant_accel[0];
+    cur_speed[1] = cur_speed[1] + duration * instant_accel[1];
+    cur_speed[2] = cur_speed[2] + duration * instant_accel[2];
+}
+
+void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data) {
+    accel.acquire_accel_data_g(accel_data);
+    gyro.acquire_gyro_data_dps(gyro_data);
+    int precision = 1000;
+    
+    if (calibFlag && (calib_data != NULL)) {
+        int i;
+        for (i = 0; i < 3; i++) {
+            accel_data[i] -= calib_data[i];
+            gyro_data[i] -= calib_data[i + 3];
+
+            if (accel_data[i] < 0) {
+                accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision);
+            } else {
+                accel_data[i] = floor(precision*accel_data[i])/precision;
+            }
+            
+            if (gyro_data[i] < 0) {
+                gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision);
+            } else {
+                gyro_data[i] = floor(precision*gyro_data[i])/precision;
+            }
+        }
+    }
+}
+
+//void apply_mechanical_filter(float *accel_data) {
+//    if (abs(accel_data[1]) < (float) YMECHANICAL) {
+//        accel_data[1] = 0.0f;
+//    }
+//    
+//    if (abs(accel_data[2]) < (float) ZMECHANICAL) {
+//        accel_data[2] = 0.0f;
+//    }
+//}
+    
+void get_caliberate_data(float *caliberate) {
+    int i;
+    int j;
+    float accel_data[3];
+    float gyro_data[3];
+    for (i = 0; i < CALIBTIMES; i++) {
+        acquire_sensor_data(accel_data, gyro_data, NULL);
+        for (j = 0; j < 3; j++) {
+            caliberate[j]     += accel_data[j];
+            caliberate[j + 3] += gyro_data[j];
+        }
+        wait_ms(MAINWAIT);
+    }
+    
+    for (i = 0; i < 6; i++) {
+           caliberate[i] /= (float)CALIBTIMES;
+    }
+}
+
+void load_buffer(float **all_data, float *accel_data, float time, int i) {
+    if (i == MAXBUFFERSIZE) {
+        pc.printf("Buffer full!\n\r");
+        exit(1);
+    }
+    
+    all_data[0][i] = time;
+    all_data[1][i] = accel_data[0];
+    all_data[2][i] = accel_data[1];
+    all_data[3][i] = accel_data[2];
+}
+
+void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples, 
+                         float *last_total, float **all_data, float *caliberate, 
+                         Timer *t) {
+    int i;
+    float accel_data[3];
+    float gyro_data[3];
+    float time;
+    float total_accelx = 0.0f;
+    float total_accely = 0.0f;
+    float total_accelz = 0.0f;
+    
+    for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
+        acquire_sensor_data(accel_data, gyro_data, caliberate);
+        time = t->read();
+        moving_avg_buf[0][i] = accel_data[0];
+        moving_avg_buf[1][i] = accel_data[1];
+        moving_avg_buf[2][i] = accel_data[2];
+        moving_avg_buf[3][i] = time;
+        wait_ms(MAINWAIT);
+    }
+
+    for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
+        total_accelx += moving_avg_buf[0][i];
+        total_accely += moving_avg_buf[1][i];
+        total_accelz += moving_avg_buf[2][i];
+    }
+    
+    last_total[0] = total_accelx;
+    last_total[1] = total_accely;
+    last_total[2] = total_accelz;
+    
+    all_data[0][0] = moving_avg_buf[3][0];
+    all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE;
+    all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE;
+    all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE;
+
+    (*num_samples)++;
+}
+
+void get_new_moving_average_point(float **all_data, float *last_total, 
+                                float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE], 
+                                float time, int *num_samples, int *start, 
+                                int *end) {
+    
+    last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0];
+    last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1];
+    last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2];
+    
+    all_data[0][*num_samples] = moving_avg_buf[3][*start];
+    
+    float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE;
+    float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE;
+    float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE;
+    
+    if (abs(temp_x) > (float) XMECHANICAL) {
+        all_data[1][*num_samples] = temp_x;
+    } else {
+        all_data[1][*num_samples] = 0.0f;
+    }
+    
+    if (abs(temp_y) > (float) YMECHANICAL) {
+        all_data[2][*num_samples] = temp_y;
+    } else {
+        all_data[2][*num_samples] = 0.0f;
+    }
+    
+    if (abs(temp_z) > (float) ZMECHANICAL) {
+        all_data[3][*num_samples] = temp_z;
+    } else {
+        all_data[3][*num_samples] = 0.0f;
+    }
+    
+    *start = (*start + 1) % (int) MOVINGAVRBUFSIZE;
+    *end = (*end + 1) % (int) MOVINGAVRBUFSIZE;
+    
+    moving_avg_buf[0][*end] = accel_data[0];
+    moving_avg_buf[1][*end] = accel_data[1];
+    moving_avg_buf[2][*end] = accel_data[2];
+    moving_avg_buf[3][*end] = time;
+    
+    (*num_samples) += 1;
+}
+
+void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
+                        float *additional_to_vel, float duration) {
+    
+    if (num_samples > TRENDPROTECTBUFSIZE) {
+        total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1];
+        total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1];
+        
+        total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
+        total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
+        
+        if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) {
+            additional_to_vel[0] = -1000.0f;
+        } else {
+            float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
+            additional_to_vel[0] = avg_accel * duration;
+        }
+        
+        if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) {
+            additional_to_vel[1] = -1000.0f;
+        } else {
+            float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
+            additional_to_vel[1] = avg_accel * duration;
+        }
+        
+    } else {
+        if(num_samples == 1){
+        } else {
+            total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
+            total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
+        }
+    }
+}
+        
+    
+
+void apply_move_end_check(float **all_data, int num_samples, 
+                        int moving_end_buf[][MOVINGENDBUFSIZE],
+                        int *num_unqualified, int *start, int *end,
+                        float *addition_to_vel, float duration,
+                        float *total_diff) {
+
+    if (num_samples > MOVINGENDBUFSIZE) {
+        num_unqualified[0] -= moving_end_buf[0][*start];
+        num_unqualified[1] -= moving_end_buf[1][*start];
+        *start = (*start + 1) % MOVINGENDBUFSIZE;
+        *end = (*end + 1) % MOVINGENDBUFSIZE;
+        
+        if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){
+            num_unqualified[0] += 1;
+            moving_end_buf[0][*end] = 1;
+        } else {
+            moving_end_buf[0][*end] = 0;
+        }
+        
+        if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){
+            num_unqualified[1] += 1;
+            moving_end_buf[1][*end] = 1;
+        } else {
+            moving_end_buf[1][*end] = 0;
+        }
+        
+        if (num_unqualified[0] >= (int)YMOVENDBIASNUM){
+            addition_to_vel[0] = -1000.0f;
+        }
+        
+        if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){
+            addition_to_vel[1] = -1000.0f;
+        }
+        
+        apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
+                            duration);
+        
+        if (num_unqualified[0] < (int)YMOVENDBIASNUM){
+            float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
+            addition_to_vel[0] = avg_accel * duration;
+        }
+        
+        if (num_unqualified[1] < (int)ZMOVENDBIASNUM){
+            float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
+            addition_to_vel[1] = avg_accel * duration;
+        }
+        
+    } else if (num_samples < MOVINGENDBUFSIZE) {
+        addition_to_vel[0] = -1000.0f;
+        addition_to_vel[1] = -1000.0f;
+        apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
+                            duration);
+    } else {
+        int i;
+        for (i = 0; i < MOVINGENDBUFSIZE; i++) {
+            if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) {
+                moving_end_buf[0][i] = 1;
+                num_unqualified[0] += 1;
+            } else {
+                moving_end_buf[0][i] = 0;
+            }
+            
+            if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) {
+                moving_end_buf[1][i] = 1;
+                num_unqualified[1] += 1;
+            } else {
+                moving_end_buf[1][i] = 0;
+            }
+            
+            addition_to_vel[0] = -1000.0f;
+            addition_to_vel[1] = -1000.0f;
+        }
+        apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
+                            duration);
+    }
+}
+
+void get_new_velocity (float *original_speed, float *addition_to_vel) {
+    if (addition_to_vel[0] == -1000.0f) {
+        original_speed[0] = 0.0f;
+    } else {
+        original_speed[0] += addition_to_vel[0];
+    }
+    
+    if (addition_to_vel[1] == -1000.0f) {
+        original_speed[1] = 0.0f;
+    } else {
+        original_speed[1] += addition_to_vel[1];
+    }
+}
+
+void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
+                              int num_samples) {
+    
+    vel_buffer[0][num_samples - 1] = time;
+    vel_buffer[1][num_samples - 1] = cur_vel[0];
+    vel_buffer[2][num_samples - 1] = cur_vel[1];
+}
+
+void output_all_to_serial(float **all_data, int num_samples) {
+    int i;
+    for (i = 0; i < num_samples; i++) {
+        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]);
+        wait(0.01);
+    }
+    pc.printf("Number of samples = %d\n", num_samples);
+    pc.printf ("End Transmission!\n");
+}
+
+void output_speed_to_serial(float **vel_data, int num_samples) {
+    int i;
+    for (i = 0; i < num_samples; i++) {
+        pc.printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]);
+        wait(0.01);
+    }
+    pc.printf("Number of samples = %d\n", num_samples);
+    pc.printf ("End Transmission!\n");
+}
+
+int main() {
+    float cur_speed[2] = {0.0, 0.0};
+    float accel_data[3];
+    float gyro_data[3];
+    float caliberate[6];
+    float moving_avg_buf[4][MOVINGAVRBUFSIZE];
+    float last_total[3];
+    float addition_to_vel[2];
+    float total_difference[2];
+    int moving_end_buf[2][MOVINGENDBUFSIZE];
+    int num_unqualified[] = {0,0};
+    int avg_buf_start = 0;
+    int avg_buf_end = MOVINGAVRBUFSIZE - 1;
+    int end_buf_start = 0;
+    int end_buf_end = MOVINGENDBUFSIZE - 1;
+    int num_samples = 0;
+    
+    float **all_data;
+    int i;
+    all_data = (float**) malloc(4*sizeof(float*));
+    if(all_data == NULL) {
+        pc.printf("Error allocating memory\n");
+        exit(1);
+    }
+    
+    for(i = 0; i < 4; i++) {
+        all_data[i] = (float*) malloc(MAXBUFFERSIZE * sizeof(float));
+    }
+    if(all_data[3] == NULL) {
+        pc.printf("Error allocating memory\n");
+        exit(1);
+    }
+    
+    float **vel_data;
+    vel_data = (float**) malloc(3*sizeof(float*));
+    if(vel_data == NULL) {
+        pc.printf("Error allocating memory\n");
+        exit(1);
+    }
+    
+    for(i = 0; i < 3; i++) {
+        vel_data[i] = (float*) malloc(MAXBUFFERSIZE * sizeof(float));
+    }
+    if(vel_data[2] == NULL) {
+        pc.printf("Error allocating memory\n");
+        exit(1);
+    }
+    
+    
+    Timer t;
+    
+    // Configure Accelerometer FXOS8700, Magnetometer FXOS8700
+    accel.accel_config();
+    mag.mag_config();
+    gyro.gyro_config();
+    wait(0.5);
+    get_caliberate_data(caliberate);
+    calibFlag = 1;
+    
+    wait(2);
+    pc.printf("Caliberation finished\n");
+    wait(3);
+    pc.printf("Start Recording!\n");
+    
+    t.start();
+    init_moving_avg_buf(&moving_avg_buf[0], &num_samples, last_total, 
+                        all_data, caliberate, &t);
+
+    float cur_time = t.read();
+    float last_time = cur_time;
+    float duration = 0;
+    
+    while (cur_time < (float)SAMPLEPERIOD_S) {
+        //t.reset();
+        
+        last_time = cur_time;
+        acquire_sensor_data(accel_data, gyro_data, caliberate);
+        cur_time = t.read();
+        duration = cur_time - last_time;
+        
+        get_new_moving_average_point(all_data, last_total, accel_data, &moving_avg_buf[0],
+                                    cur_time, &num_samples, &avg_buf_start, &avg_buf_end);
+                                    
+        apply_move_end_check(all_data, num_samples, &moving_end_buf[0], num_unqualified, 
+                            &end_buf_start, &end_buf_end, addition_to_vel, duration, 
+                            total_difference);
+        
+        get_new_velocity(cur_speed, addition_to_vel);
+        
+        insert_new_vel_to_buffer(vel_data, cur_time, cur_speed, num_samples);
+        
+        //acquire_new_speed(cur_speed, last_period, accel_data);
+        //load_buffer(all_data, accel_data, last_period, num_samples);
+        Thread::wait(MAINWAIT);
+    }
+    pc.printf ("Stop Recording!\n");
+    wait(3);
+    
+    output_all_to_serial(all_data, num_samples);
+    output_speed_to_serial(vel_data, num_samples);
+
+    return 0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Fri May 25 23:22:31 2018 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#0712b8adf6bbc7eb796d5dac26f95d79d40745ef