Weiqian Xu / Mbed OS motion_tracking

blah

Dependencies:   FXAS21002 FXOS8700

main.cpp@0:b177004d9e60, 2018-05-25 (annotated)

Committer:
xuweiqian9999
Date:
Fri May 25 23:22:31 2018 +0000
Revision:
0:b177004d9e60
blah;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xuweiqian9999 0:b177004d9e60 1 #include "mbed.h"
xuweiqian9999 0:b177004d9e60 2 #include "FXOS8700.h"
xuweiqian9999 0:b177004d9e60 3 #include "FXAS21002.h"
xuweiqian9999 0:b177004d9e60 4 #include "math.h"
xuweiqian9999 0:b177004d9e60 5 #include "stdio.h"
xuweiqian9999 0:b177004d9e60 6
xuweiqian9999 0:b177004d9e60 7 #define MAINWAIT 1
xuweiqian9999 0:b177004d9e60 8 #define CALIBTIMES 50
xuweiqian9999 0:b177004d9e60 9 #define SAMPLEPERIOD_S 3
xuweiqian9999 0:b177004d9e60 10 #define MOVINGAVRBUFSIZE 10
xuweiqian9999 0:b177004d9e60 11 #define MOVINGENDBUFSIZE 32
xuweiqian9999 0:b177004d9e60 12 #define TRENDPROTECTBUFSIZE 60
xuweiqian9999 0:b177004d9e60 13 #define YMOVENDTHRESHOLD 0.09
xuweiqian9999 0:b177004d9e60 14 #define ZMOVENDTHRESHOLD 0.09
xuweiqian9999 0:b177004d9e60 15 #define YMOVENDBIASNUM 30
xuweiqian9999 0:b177004d9e60 16 #define ZMOVENDBIASNUM 30
xuweiqian9999 0:b177004d9e60 17 #define XTRENDPROTECTTHRESHOLD 0.05
xuweiqian9999 0:b177004d9e60 18 #define YTRENDPROTECTTHRESHOLD 0.05
xuweiqian9999 0:b177004d9e60 19 #define MAXBUFFERSIZE 3000
xuweiqian9999 0:b177004d9e60 20 #define XMECHANICAL 0.005
xuweiqian9999 0:b177004d9e60 21 #define YMECHANICAL 0.005
xuweiqian9999 0:b177004d9e60 22 #define ZMECHANICAL 0.005
xuweiqian9999 0:b177004d9e60 23
xuweiqian9999 0:b177004d9e60 24 // Initialize Serial port
xuweiqian9999 0:b177004d9e60 25 Serial pc(USBTX, USBRX);
xuweiqian9999 0:b177004d9e60 26
xuweiqian9999 0:b177004d9e60 27 // Pin connections & address for Hexiwear
xuweiqian9999 0:b177004d9e60 28 FXOS8700 accel(PTC11, PTC10);
xuweiqian9999 0:b177004d9e60 29 FXOS8700 mag(PTC11, PTC10);
xuweiqian9999 0:b177004d9e60 30 FXAS21002 gyro(PTC11,PTC10);
xuweiqian9999 0:b177004d9e60 31
xuweiqian9999 0:b177004d9e60 32 int calibFlag = 0;
xuweiqian9999 0:b177004d9e60 33
xuweiqian9999 0:b177004d9e60 34 void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) {
xuweiqian9999 0:b177004d9e60 35 cur_speed[0] = cur_speed[0] + duration * instant_accel[0];
xuweiqian9999 0:b177004d9e60 36 cur_speed[1] = cur_speed[1] + duration * instant_accel[1];
xuweiqian9999 0:b177004d9e60 37 cur_speed[2] = cur_speed[2] + duration * instant_accel[2];
xuweiqian9999 0:b177004d9e60 38 }
xuweiqian9999 0:b177004d9e60 39
xuweiqian9999 0:b177004d9e60 40 void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data) {
xuweiqian9999 0:b177004d9e60 41 accel.acquire_accel_data_g(accel_data);
xuweiqian9999 0:b177004d9e60 42 gyro.acquire_gyro_data_dps(gyro_data);
xuweiqian9999 0:b177004d9e60 43 int precision = 1000;
xuweiqian9999 0:b177004d9e60 44
xuweiqian9999 0:b177004d9e60 45 if (calibFlag && (calib_data != NULL)) {
xuweiqian9999 0:b177004d9e60 46 int i;
xuweiqian9999 0:b177004d9e60 47 for (i = 0; i < 3; i++) {
xuweiqian9999 0:b177004d9e60 48 accel_data[i] -= calib_data[i];
xuweiqian9999 0:b177004d9e60 49 gyro_data[i] -= calib_data[i + 3];
xuweiqian9999 0:b177004d9e60 50
xuweiqian9999 0:b177004d9e60 51 if (accel_data[i] < 0) {
xuweiqian9999 0:b177004d9e60 52 accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision);
xuweiqian9999 0:b177004d9e60 53 } else {
xuweiqian9999 0:b177004d9e60 54 accel_data[i] = floor(precision*accel_data[i])/precision;
xuweiqian9999 0:b177004d9e60 55 }
xuweiqian9999 0:b177004d9e60 56
xuweiqian9999 0:b177004d9e60 57 if (gyro_data[i] < 0) {
xuweiqian9999 0:b177004d9e60 58 gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision);
xuweiqian9999 0:b177004d9e60 59 } else {
xuweiqian9999 0:b177004d9e60 60 gyro_data[i] = floor(precision*gyro_data[i])/precision;
xuweiqian9999 0:b177004d9e60 61 }
xuweiqian9999 0:b177004d9e60 62 }
xuweiqian9999 0:b177004d9e60 63 }
xuweiqian9999 0:b177004d9e60 64 }
xuweiqian9999 0:b177004d9e60 65
xuweiqian9999 0:b177004d9e60 66 //void apply_mechanical_filter(float *accel_data) {
xuweiqian9999 0:b177004d9e60 67 // if (abs(accel_data[1]) < (float) YMECHANICAL) {
xuweiqian9999 0:b177004d9e60 68 // accel_data[1] = 0.0f;
xuweiqian9999 0:b177004d9e60 69 // }
xuweiqian9999 0:b177004d9e60 70 //
xuweiqian9999 0:b177004d9e60 71 // if (abs(accel_data[2]) < (float) ZMECHANICAL) {
xuweiqian9999 0:b177004d9e60 72 // accel_data[2] = 0.0f;
xuweiqian9999 0:b177004d9e60 73 // }
xuweiqian9999 0:b177004d9e60 74 //}
xuweiqian9999 0:b177004d9e60 75
xuweiqian9999 0:b177004d9e60 76 void get_caliberate_data(float *caliberate) {
xuweiqian9999 0:b177004d9e60 77 int i;
xuweiqian9999 0:b177004d9e60 78 int j;
xuweiqian9999 0:b177004d9e60 79 float accel_data[3];
xuweiqian9999 0:b177004d9e60 80 float gyro_data[3];
xuweiqian9999 0:b177004d9e60 81 for (i = 0; i < CALIBTIMES; i++) {
xuweiqian9999 0:b177004d9e60 82 acquire_sensor_data(accel_data, gyro_data, NULL);
xuweiqian9999 0:b177004d9e60 83 for (j = 0; j < 3; j++) {
xuweiqian9999 0:b177004d9e60 84 caliberate[j] += accel_data[j];
xuweiqian9999 0:b177004d9e60 85 caliberate[j + 3] += gyro_data[j];
xuweiqian9999 0:b177004d9e60 86 }
xuweiqian9999 0:b177004d9e60 87 wait_ms(MAINWAIT);
xuweiqian9999 0:b177004d9e60 88 }
xuweiqian9999 0:b177004d9e60 89
xuweiqian9999 0:b177004d9e60 90 for (i = 0; i < 6; i++) {
xuweiqian9999 0:b177004d9e60 91 caliberate[i] /= (float)CALIBTIMES;
xuweiqian9999 0:b177004d9e60 92 }
xuweiqian9999 0:b177004d9e60 93 }
xuweiqian9999 0:b177004d9e60 94
xuweiqian9999 0:b177004d9e60 95 void load_buffer(float **all_data, float *accel_data, float time, int i) {
xuweiqian9999 0:b177004d9e60 96 if (i == MAXBUFFERSIZE) {
xuweiqian9999 0:b177004d9e60 97 pc.printf("Buffer full!\n\r");
xuweiqian9999 0:b177004d9e60 98 exit(1);
xuweiqian9999 0:b177004d9e60 99 }
xuweiqian9999 0:b177004d9e60 100
xuweiqian9999 0:b177004d9e60 101 all_data[0][i] = time;
xuweiqian9999 0:b177004d9e60 102 all_data[1][i] = accel_data[0];
xuweiqian9999 0:b177004d9e60 103 all_data[2][i] = accel_data[1];
xuweiqian9999 0:b177004d9e60 104 all_data[3][i] = accel_data[2];
xuweiqian9999 0:b177004d9e60 105 }
xuweiqian9999 0:b177004d9e60 106
xuweiqian9999 0:b177004d9e60 107 void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples,
xuweiqian9999 0:b177004d9e60 108 float *last_total, float **all_data, float *caliberate,
xuweiqian9999 0:b177004d9e60 109 Timer *t) {
xuweiqian9999 0:b177004d9e60 110 int i;
xuweiqian9999 0:b177004d9e60 111 float accel_data[3];
xuweiqian9999 0:b177004d9e60 112 float gyro_data[3];
xuweiqian9999 0:b177004d9e60 113 float time;
xuweiqian9999 0:b177004d9e60 114 float total_accelx = 0.0f;
xuweiqian9999 0:b177004d9e60 115 float total_accely = 0.0f;
xuweiqian9999 0:b177004d9e60 116 float total_accelz = 0.0f;
xuweiqian9999 0:b177004d9e60 117
xuweiqian9999 0:b177004d9e60 118 for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
xuweiqian9999 0:b177004d9e60 119 acquire_sensor_data(accel_data, gyro_data, caliberate);
xuweiqian9999 0:b177004d9e60 120 time = t->read();
xuweiqian9999 0:b177004d9e60 121 moving_avg_buf[0][i] = accel_data[0];
xuweiqian9999 0:b177004d9e60 122 moving_avg_buf[1][i] = accel_data[1];
xuweiqian9999 0:b177004d9e60 123 moving_avg_buf[2][i] = accel_data[2];
xuweiqian9999 0:b177004d9e60 124 moving_avg_buf[3][i] = time;
xuweiqian9999 0:b177004d9e60 125 wait_ms(MAINWAIT);
xuweiqian9999 0:b177004d9e60 126 }
xuweiqian9999 0:b177004d9e60 127
xuweiqian9999 0:b177004d9e60 128 for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
xuweiqian9999 0:b177004d9e60 129 total_accelx += moving_avg_buf[0][i];
xuweiqian9999 0:b177004d9e60 130 total_accely += moving_avg_buf[1][i];
xuweiqian9999 0:b177004d9e60 131 total_accelz += moving_avg_buf[2][i];
xuweiqian9999 0:b177004d9e60 132 }
xuweiqian9999 0:b177004d9e60 133
xuweiqian9999 0:b177004d9e60 134 last_total[0] = total_accelx;
xuweiqian9999 0:b177004d9e60 135 last_total[1] = total_accely;
xuweiqian9999 0:b177004d9e60 136 last_total[2] = total_accelz;
xuweiqian9999 0:b177004d9e60 137
xuweiqian9999 0:b177004d9e60 138 all_data[0][0] = moving_avg_buf[3][0];
xuweiqian9999 0:b177004d9e60 139 all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 140 all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 141 all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 142
xuweiqian9999 0:b177004d9e60 143 (*num_samples)++;
xuweiqian9999 0:b177004d9e60 144 }
xuweiqian9999 0:b177004d9e60 145
xuweiqian9999 0:b177004d9e60 146 void get_new_moving_average_point(float **all_data, float *last_total,
xuweiqian9999 0:b177004d9e60 147 float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE],
xuweiqian9999 0:b177004d9e60 148 float time, int *num_samples, int *start,
xuweiqian9999 0:b177004d9e60 149 int *end) {
xuweiqian9999 0:b177004d9e60 150
xuweiqian9999 0:b177004d9e60 151 last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0];
xuweiqian9999 0:b177004d9e60 152 last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1];
xuweiqian9999 0:b177004d9e60 153 last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2];
xuweiqian9999 0:b177004d9e60 154
xuweiqian9999 0:b177004d9e60 155 all_data[0][*num_samples] = moving_avg_buf[3][*start];
xuweiqian9999 0:b177004d9e60 156
xuweiqian9999 0:b177004d9e60 157 float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 158 float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 159 float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 160
xuweiqian9999 0:b177004d9e60 161 if (abs(temp_x) > (float) XMECHANICAL) {
xuweiqian9999 0:b177004d9e60 162 all_data[1][*num_samples] = temp_x;
xuweiqian9999 0:b177004d9e60 163 } else {
xuweiqian9999 0:b177004d9e60 164 all_data[1][*num_samples] = 0.0f;
xuweiqian9999 0:b177004d9e60 165 }
xuweiqian9999 0:b177004d9e60 166
xuweiqian9999 0:b177004d9e60 167 if (abs(temp_y) > (float) YMECHANICAL) {
xuweiqian9999 0:b177004d9e60 168 all_data[2][*num_samples] = temp_y;
xuweiqian9999 0:b177004d9e60 169 } else {
xuweiqian9999 0:b177004d9e60 170 all_data[2][*num_samples] = 0.0f;
xuweiqian9999 0:b177004d9e60 171 }
xuweiqian9999 0:b177004d9e60 172
xuweiqian9999 0:b177004d9e60 173 if (abs(temp_z) > (float) ZMECHANICAL) {
xuweiqian9999 0:b177004d9e60 174 all_data[3][*num_samples] = temp_z;
xuweiqian9999 0:b177004d9e60 175 } else {
xuweiqian9999 0:b177004d9e60 176 all_data[3][*num_samples] = 0.0f;
xuweiqian9999 0:b177004d9e60 177 }
xuweiqian9999 0:b177004d9e60 178
xuweiqian9999 0:b177004d9e60 179 *start = (*start + 1) % (int) MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 180 *end = (*end + 1) % (int) MOVINGAVRBUFSIZE;
xuweiqian9999 0:b177004d9e60 181
xuweiqian9999 0:b177004d9e60 182 moving_avg_buf[0][*end] = accel_data[0];
xuweiqian9999 0:b177004d9e60 183 moving_avg_buf[1][*end] = accel_data[1];
xuweiqian9999 0:b177004d9e60 184 moving_avg_buf[2][*end] = accel_data[2];
xuweiqian9999 0:b177004d9e60 185 moving_avg_buf[3][*end] = time;
xuweiqian9999 0:b177004d9e60 186
xuweiqian9999 0:b177004d9e60 187 (*num_samples) += 1;
xuweiqian9999 0:b177004d9e60 188 }
xuweiqian9999 0:b177004d9e60 189
xuweiqian9999 0:b177004d9e60 190 void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
xuweiqian9999 0:b177004d9e60 191 float *additional_to_vel, float duration) {
xuweiqian9999 0:b177004d9e60 192
xuweiqian9999 0:b177004d9e60 193 if (num_samples > TRENDPROTECTBUFSIZE) {
xuweiqian9999 0:b177004d9e60 194 total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1];
xuweiqian9999 0:b177004d9e60 195 total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1];
xuweiqian9999 0:b177004d9e60 196
xuweiqian9999 0:b177004d9e60 197 total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
xuweiqian9999 0:b177004d9e60 198 total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
xuweiqian9999 0:b177004d9e60 199
xuweiqian9999 0:b177004d9e60 200 if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) {
xuweiqian9999 0:b177004d9e60 201 additional_to_vel[0] = -1000.0f;
xuweiqian9999 0:b177004d9e60 202 } else {
xuweiqian9999 0:b177004d9e60 203 float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
xuweiqian9999 0:b177004d9e60 204 additional_to_vel[0] = avg_accel * duration;
xuweiqian9999 0:b177004d9e60 205 }
xuweiqian9999 0:b177004d9e60 206
xuweiqian9999 0:b177004d9e60 207 if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) {
xuweiqian9999 0:b177004d9e60 208 additional_to_vel[1] = -1000.0f;
xuweiqian9999 0:b177004d9e60 209 } else {
xuweiqian9999 0:b177004d9e60 210 float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
xuweiqian9999 0:b177004d9e60 211 additional_to_vel[1] = avg_accel * duration;
xuweiqian9999 0:b177004d9e60 212 }
xuweiqian9999 0:b177004d9e60 213
xuweiqian9999 0:b177004d9e60 214 } else {
xuweiqian9999 0:b177004d9e60 215 if(num_samples == 1){
xuweiqian9999 0:b177004d9e60 216 } else {
xuweiqian9999 0:b177004d9e60 217 total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
xuweiqian9999 0:b177004d9e60 218 total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
xuweiqian9999 0:b177004d9e60 219 }
xuweiqian9999 0:b177004d9e60 220 }
xuweiqian9999 0:b177004d9e60 221 }
xuweiqian9999 0:b177004d9e60 222
xuweiqian9999 0:b177004d9e60 223
xuweiqian9999 0:b177004d9e60 224
xuweiqian9999 0:b177004d9e60 225 void apply_move_end_check(float **all_data, int num_samples,
xuweiqian9999 0:b177004d9e60 226 int moving_end_buf[][MOVINGENDBUFSIZE],
xuweiqian9999 0:b177004d9e60 227 int *num_unqualified, int *start, int *end,
xuweiqian9999 0:b177004d9e60 228 float *addition_to_vel, float duration,
xuweiqian9999 0:b177004d9e60 229 float *total_diff) {
xuweiqian9999 0:b177004d9e60 230
xuweiqian9999 0:b177004d9e60 231 if (num_samples > MOVINGENDBUFSIZE) {
xuweiqian9999 0:b177004d9e60 232 num_unqualified[0] -= moving_end_buf[0][*start];
xuweiqian9999 0:b177004d9e60 233 num_unqualified[1] -= moving_end_buf[1][*start];
xuweiqian9999 0:b177004d9e60 234 *start = (*start + 1) % MOVINGENDBUFSIZE;
xuweiqian9999 0:b177004d9e60 235 *end = (*end + 1) % MOVINGENDBUFSIZE;
xuweiqian9999 0:b177004d9e60 236
xuweiqian9999 0:b177004d9e60 237 if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){
xuweiqian9999 0:b177004d9e60 238 num_unqualified[0] += 1;
xuweiqian9999 0:b177004d9e60 239 moving_end_buf[0][*end] = 1;
xuweiqian9999 0:b177004d9e60 240 } else {
xuweiqian9999 0:b177004d9e60 241 moving_end_buf[0][*end] = 0;
xuweiqian9999 0:b177004d9e60 242 }
xuweiqian9999 0:b177004d9e60 243
xuweiqian9999 0:b177004d9e60 244 if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){
xuweiqian9999 0:b177004d9e60 245 num_unqualified[1] += 1;
xuweiqian9999 0:b177004d9e60 246 moving_end_buf[1][*end] = 1;
xuweiqian9999 0:b177004d9e60 247 } else {
xuweiqian9999 0:b177004d9e60 248 moving_end_buf[1][*end] = 0;
xuweiqian9999 0:b177004d9e60 249 }
xuweiqian9999 0:b177004d9e60 250
xuweiqian9999 0:b177004d9e60 251 if (num_unqualified[0] >= (int)YMOVENDBIASNUM){
xuweiqian9999 0:b177004d9e60 252 addition_to_vel[0] = -1000.0f;
xuweiqian9999 0:b177004d9e60 253 }
xuweiqian9999 0:b177004d9e60 254
xuweiqian9999 0:b177004d9e60 255 if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){
xuweiqian9999 0:b177004d9e60 256 addition_to_vel[1] = -1000.0f;
xuweiqian9999 0:b177004d9e60 257 }
xuweiqian9999 0:b177004d9e60 258
xuweiqian9999 0:b177004d9e60 259 apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
xuweiqian9999 0:b177004d9e60 260 duration);
xuweiqian9999 0:b177004d9e60 261
xuweiqian9999 0:b177004d9e60 262 if (num_unqualified[0] < (int)YMOVENDBIASNUM){
xuweiqian9999 0:b177004d9e60 263 float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
xuweiqian9999 0:b177004d9e60 264 addition_to_vel[0] = avg_accel * duration;
xuweiqian9999 0:b177004d9e60 265 }
xuweiqian9999 0:b177004d9e60 266
xuweiqian9999 0:b177004d9e60 267 if (num_unqualified[1] < (int)ZMOVENDBIASNUM){
xuweiqian9999 0:b177004d9e60 268 float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
xuweiqian9999 0:b177004d9e60 269 addition_to_vel[1] = avg_accel * duration;
xuweiqian9999 0:b177004d9e60 270 }
xuweiqian9999 0:b177004d9e60 271
xuweiqian9999 0:b177004d9e60 272 } else if (num_samples < MOVINGENDBUFSIZE) {
xuweiqian9999 0:b177004d9e60 273 addition_to_vel[0] = -1000.0f;
xuweiqian9999 0:b177004d9e60 274 addition_to_vel[1] = -1000.0f;
xuweiqian9999 0:b177004d9e60 275 apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
xuweiqian9999 0:b177004d9e60 276 duration);
xuweiqian9999 0:b177004d9e60 277 } else {
xuweiqian9999 0:b177004d9e60 278 int i;
xuweiqian9999 0:b177004d9e60 279 for (i = 0; i < MOVINGENDBUFSIZE; i++) {
xuweiqian9999 0:b177004d9e60 280 if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) {
xuweiqian9999 0:b177004d9e60 281 moving_end_buf[0][i] = 1;
xuweiqian9999 0:b177004d9e60 282 num_unqualified[0] += 1;
xuweiqian9999 0:b177004d9e60 283 } else {
xuweiqian9999 0:b177004d9e60 284 moving_end_buf[0][i] = 0;
xuweiqian9999 0:b177004d9e60 285 }
xuweiqian9999 0:b177004d9e60 286
xuweiqian9999 0:b177004d9e60 287 if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) {
xuweiqian9999 0:b177004d9e60 288 moving_end_buf[1][i] = 1;
xuweiqian9999 0:b177004d9e60 289 num_unqualified[1] += 1;
xuweiqian9999 0:b177004d9e60 290 } else {
xuweiqian9999 0:b177004d9e60 291 moving_end_buf[1][i] = 0;
xuweiqian9999 0:b177004d9e60 292 }
xuweiqian9999 0:b177004d9e60 293
xuweiqian9999 0:b177004d9e60 294 addition_to_vel[0] = -1000.0f;
xuweiqian9999 0:b177004d9e60 295 addition_to_vel[1] = -1000.0f;
xuweiqian9999 0:b177004d9e60 296 }
xuweiqian9999 0:b177004d9e60 297 apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
xuweiqian9999 0:b177004d9e60 298 duration);
xuweiqian9999 0:b177004d9e60 299 }
xuweiqian9999 0:b177004d9e60 300 }
xuweiqian9999 0:b177004d9e60 301
xuweiqian9999 0:b177004d9e60 302 void get_new_velocity (float *original_speed, float *addition_to_vel) {
xuweiqian9999 0:b177004d9e60 303 if (addition_to_vel[0] == -1000.0f) {
xuweiqian9999 0:b177004d9e60 304 original_speed[0] = 0.0f;
xuweiqian9999 0:b177004d9e60 305 } else {
xuweiqian9999 0:b177004d9e60 306 original_speed[0] += addition_to_vel[0];
xuweiqian9999 0:b177004d9e60 307 }
xuweiqian9999 0:b177004d9e60 308
xuweiqian9999 0:b177004d9e60 309 if (addition_to_vel[1] == -1000.0f) {
xuweiqian9999 0:b177004d9e60 310 original_speed[1] = 0.0f;
xuweiqian9999 0:b177004d9e60 311 } else {
xuweiqian9999 0:b177004d9e60 312 original_speed[1] += addition_to_vel[1];
xuweiqian9999 0:b177004d9e60 313 }
xuweiqian9999 0:b177004d9e60 314 }
xuweiqian9999 0:b177004d9e60 315
xuweiqian9999 0:b177004d9e60 316 void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
xuweiqian9999 0:b177004d9e60 317 int num_samples) {
xuweiqian9999 0:b177004d9e60 318
xuweiqian9999 0:b177004d9e60 319 vel_buffer[0][num_samples - 1] = time;
xuweiqian9999 0:b177004d9e60 320 vel_buffer[1][num_samples - 1] = cur_vel[0];
xuweiqian9999 0:b177004d9e60 321 vel_buffer[2][num_samples - 1] = cur_vel[1];
xuweiqian9999 0:b177004d9e60 322 }
xuweiqian9999 0:b177004d9e60 323
xuweiqian9999 0:b177004d9e60 324 void output_all_to_serial(float **all_data, int num_samples) {
xuweiqian9999 0:b177004d9e60 325 int i;
xuweiqian9999 0:b177004d9e60 326 for (i = 0; i < num_samples; i++) {
xuweiqian9999 0:b177004d9e60 327 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]);
xuweiqian9999 0:b177004d9e60 328 wait(0.01);
xuweiqian9999 0:b177004d9e60 329 }
xuweiqian9999 0:b177004d9e60 330 pc.printf("Number of samples = %d\n", num_samples);
xuweiqian9999 0:b177004d9e60 331 pc.printf ("End Transmission!\n");
xuweiqian9999 0:b177004d9e60 332 }
xuweiqian9999 0:b177004d9e60 333
xuweiqian9999 0:b177004d9e60 334 void output_speed_to_serial(float **vel_data, int num_samples) {
xuweiqian9999 0:b177004d9e60 335 int i;
xuweiqian9999 0:b177004d9e60 336 for (i = 0; i < num_samples; i++) {
xuweiqian9999 0:b177004d9e60 337 pc.printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]);
xuweiqian9999 0:b177004d9e60 338 wait(0.01);
xuweiqian9999 0:b177004d9e60 339 }
xuweiqian9999 0:b177004d9e60 340 pc.printf("Number of samples = %d\n", num_samples);
xuweiqian9999 0:b177004d9e60 341 pc.printf ("End Transmission!\n");
xuweiqian9999 0:b177004d9e60 342 }
xuweiqian9999 0:b177004d9e60 343
xuweiqian9999 0:b177004d9e60 344 int main() {
xuweiqian9999 0:b177004d9e60 345 float cur_speed[2] = {0.0, 0.0};
xuweiqian9999 0:b177004d9e60 346 float accel_data[3];
xuweiqian9999 0:b177004d9e60 347 float gyro_data[3];
xuweiqian9999 0:b177004d9e60 348 float caliberate[6];
xuweiqian9999 0:b177004d9e60 349 float moving_avg_buf[4][MOVINGAVRBUFSIZE];
xuweiqian9999 0:b177004d9e60 350 float last_total[3];
xuweiqian9999 0:b177004d9e60 351 float addition_to_vel[2];
xuweiqian9999 0:b177004d9e60 352 float total_difference[2];
xuweiqian9999 0:b177004d9e60 353 int moving_end_buf[2][MOVINGENDBUFSIZE];
xuweiqian9999 0:b177004d9e60 354 int num_unqualified[] = {0,0};
xuweiqian9999 0:b177004d9e60 355 int avg_buf_start = 0;
xuweiqian9999 0:b177004d9e60 356 int avg_buf_end = MOVINGAVRBUFSIZE - 1;
xuweiqian9999 0:b177004d9e60 357 int end_buf_start = 0;
xuweiqian9999 0:b177004d9e60 358 int end_buf_end = MOVINGENDBUFSIZE - 1;
xuweiqian9999 0:b177004d9e60 359 int num_samples = 0;
xuweiqian9999 0:b177004d9e60 360
xuweiqian9999 0:b177004d9e60 361 float **all_data;
xuweiqian9999 0:b177004d9e60 362 int i;
xuweiqian9999 0:b177004d9e60 363 all_data = (float**) malloc(4*sizeof(float*));
xuweiqian9999 0:b177004d9e60 364 if(all_data == NULL) {
xuweiqian9999 0:b177004d9e60 365 pc.printf("Error allocating memory\n");
xuweiqian9999 0:b177004d9e60 366 exit(1);
xuweiqian9999 0:b177004d9e60 367 }
xuweiqian9999 0:b177004d9e60 368
xuweiqian9999 0:b177004d9e60 369 for(i = 0; i < 4; i++) {
xuweiqian9999 0:b177004d9e60 370 all_data[i] = (float*) malloc(MAXBUFFERSIZE * sizeof(float));
xuweiqian9999 0:b177004d9e60 371 }
xuweiqian9999 0:b177004d9e60 372 if(all_data[3] == NULL) {
xuweiqian9999 0:b177004d9e60 373 pc.printf("Error allocating memory\n");
xuweiqian9999 0:b177004d9e60 374 exit(1);
xuweiqian9999 0:b177004d9e60 375 }
xuweiqian9999 0:b177004d9e60 376
xuweiqian9999 0:b177004d9e60 377 float **vel_data;
xuweiqian9999 0:b177004d9e60 378 vel_data = (float**) malloc(3*sizeof(float*));
xuweiqian9999 0:b177004d9e60 379 if(vel_data == NULL) {
xuweiqian9999 0:b177004d9e60 380 pc.printf("Error allocating memory\n");
xuweiqian9999 0:b177004d9e60 381 exit(1);
xuweiqian9999 0:b177004d9e60 382 }
xuweiqian9999 0:b177004d9e60 383
xuweiqian9999 0:b177004d9e60 384 for(i = 0; i < 3; i++) {
xuweiqian9999 0:b177004d9e60 385 vel_data[i] = (float*) malloc(MAXBUFFERSIZE * sizeof(float));
xuweiqian9999 0:b177004d9e60 386 }
xuweiqian9999 0:b177004d9e60 387 if(vel_data[2] == NULL) {
xuweiqian9999 0:b177004d9e60 388 pc.printf("Error allocating memory\n");
xuweiqian9999 0:b177004d9e60 389 exit(1);
xuweiqian9999 0:b177004d9e60 390 }
xuweiqian9999 0:b177004d9e60 391
xuweiqian9999 0:b177004d9e60 392
xuweiqian9999 0:b177004d9e60 393 Timer t;
xuweiqian9999 0:b177004d9e60 394
xuweiqian9999 0:b177004d9e60 395 // Configure Accelerometer FXOS8700, Magnetometer FXOS8700
xuweiqian9999 0:b177004d9e60 396 accel.accel_config();
xuweiqian9999 0:b177004d9e60 397 mag.mag_config();
xuweiqian9999 0:b177004d9e60 398 gyro.gyro_config();
xuweiqian9999 0:b177004d9e60 399 wait(0.5);
xuweiqian9999 0:b177004d9e60 400 get_caliberate_data(caliberate);
xuweiqian9999 0:b177004d9e60 401 calibFlag = 1;
xuweiqian9999 0:b177004d9e60 402
xuweiqian9999 0:b177004d9e60 403 wait(2);
xuweiqian9999 0:b177004d9e60 404 pc.printf("Caliberation finished\n");
xuweiqian9999 0:b177004d9e60 405 wait(3);
xuweiqian9999 0:b177004d9e60 406 pc.printf("Start Recording!\n");
xuweiqian9999 0:b177004d9e60 407
xuweiqian9999 0:b177004d9e60 408 t.start();
xuweiqian9999 0:b177004d9e60 409 init_moving_avg_buf(&moving_avg_buf[0], &num_samples, last_total,
xuweiqian9999 0:b177004d9e60 410 all_data, caliberate, &t);
xuweiqian9999 0:b177004d9e60 411
xuweiqian9999 0:b177004d9e60 412 float cur_time = t.read();
xuweiqian9999 0:b177004d9e60 413 float last_time = cur_time;
xuweiqian9999 0:b177004d9e60 414 float duration = 0;
xuweiqian9999 0:b177004d9e60 415
xuweiqian9999 0:b177004d9e60 416 while (cur_time < (float)SAMPLEPERIOD_S) {
xuweiqian9999 0:b177004d9e60 417 //t.reset();
xuweiqian9999 0:b177004d9e60 418
xuweiqian9999 0:b177004d9e60 419 last_time = cur_time;
xuweiqian9999 0:b177004d9e60 420 acquire_sensor_data(accel_data, gyro_data, caliberate);
xuweiqian9999 0:b177004d9e60 421 cur_time = t.read();
xuweiqian9999 0:b177004d9e60 422 duration = cur_time - last_time;
xuweiqian9999 0:b177004d9e60 423
xuweiqian9999 0:b177004d9e60 424 get_new_moving_average_point(all_data, last_total, accel_data, &moving_avg_buf[0],
xuweiqian9999 0:b177004d9e60 425 cur_time, &num_samples, &avg_buf_start, &avg_buf_end);
xuweiqian9999 0:b177004d9e60 426
xuweiqian9999 0:b177004d9e60 427 apply_move_end_check(all_data, num_samples, &moving_end_buf[0], num_unqualified,
xuweiqian9999 0:b177004d9e60 428 &end_buf_start, &end_buf_end, addition_to_vel, duration,
xuweiqian9999 0:b177004d9e60 429 total_difference);
xuweiqian9999 0:b177004d9e60 430
xuweiqian9999 0:b177004d9e60 431 get_new_velocity(cur_speed, addition_to_vel);
xuweiqian9999 0:b177004d9e60 432
xuweiqian9999 0:b177004d9e60 433 insert_new_vel_to_buffer(vel_data, cur_time, cur_speed, num_samples);
xuweiqian9999 0:b177004d9e60 434
xuweiqian9999 0:b177004d9e60 435 //acquire_new_speed(cur_speed, last_period, accel_data);
xuweiqian9999 0:b177004d9e60 436 //load_buffer(all_data, accel_data, last_period, num_samples);
xuweiqian9999 0:b177004d9e60 437 Thread::wait(MAINWAIT);
xuweiqian9999 0:b177004d9e60 438 }
xuweiqian9999 0:b177004d9e60 439 pc.printf ("Stop Recording!\n");
xuweiqian9999 0:b177004d9e60 440 wait(3);
xuweiqian9999 0:b177004d9e60 441
xuweiqian9999 0:b177004d9e60 442 output_all_to_serial(all_data, num_samples);
xuweiqian9999 0:b177004d9e60 443 output_speed_to_serial(vel_data, num_samples);
xuweiqian9999 0:b177004d9e60 444
xuweiqian9999 0:b177004d9e60 445 return 0;
xuweiqian9999 0:b177004d9e60 446 }