for led strip
motion_tracking.cpp@1:b90027de2bdb, 2018-06-13 (annotated)
- Committer:
- xuweiqian9999
- Date:
- Wed Jun 13 01:47:37 2018 +0000
- Revision:
- 1:b90027de2bdb
- Parent:
- 0:da91c8ed4a98
working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
xuweiqian9999 | 0:da91c8ed4a98 | 1 | #include "motion_tracking.h" |
xuweiqian9999 | 0:da91c8ed4a98 | 2 | |
xuweiqian9999 | 0:da91c8ed4a98 | 3 | void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) { |
xuweiqian9999 | 0:da91c8ed4a98 | 4 | cur_speed[0] = cur_speed[0] + duration * instant_accel[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 5 | cur_speed[1] = cur_speed[1] + duration * instant_accel[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 6 | cur_speed[2] = cur_speed[2] + duration * instant_accel[2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 7 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 8 | |
xuweiqian9999 | 0:da91c8ed4a98 | 9 | void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data, |
xuweiqian9999 | 0:da91c8ed4a98 | 10 | int calibFlag, FXOS8700 *accel, FXAS21002 *gyro) { |
xuweiqian9999 | 0:da91c8ed4a98 | 11 | accel->acquire_accel_data_g(accel_data); |
xuweiqian9999 | 0:da91c8ed4a98 | 12 | gyro->acquire_gyro_data_dps(gyro_data); |
xuweiqian9999 | 0:da91c8ed4a98 | 13 | int precision = 1000; |
xuweiqian9999 | 0:da91c8ed4a98 | 14 | |
xuweiqian9999 | 0:da91c8ed4a98 | 15 | float temp_y = accel_data[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 16 | accel_data[1] = accel_data[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 17 | accel_data[0] = temp_y; |
xuweiqian9999 | 0:da91c8ed4a98 | 18 | |
xuweiqian9999 | 0:da91c8ed4a98 | 19 | if (calibFlag && (calib_data != NULL)) { |
xuweiqian9999 | 0:da91c8ed4a98 | 20 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 21 | for (i = 0; i < 3; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 22 | accel_data[i] -= calib_data[i]; |
xuweiqian9999 | 0:da91c8ed4a98 | 23 | gyro_data[i] -= calib_data[i + 3]; |
xuweiqian9999 | 0:da91c8ed4a98 | 24 | |
xuweiqian9999 | 0:da91c8ed4a98 | 25 | if (accel_data[i] < 0) { |
xuweiqian9999 | 0:da91c8ed4a98 | 26 | accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision); |
xuweiqian9999 | 0:da91c8ed4a98 | 27 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 28 | accel_data[i] = floor(precision*accel_data[i])/precision; |
xuweiqian9999 | 0:da91c8ed4a98 | 29 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 30 | if (gyro_data[i] < 0) { |
xuweiqian9999 | 0:da91c8ed4a98 | 31 | gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision); |
xuweiqian9999 | 0:da91c8ed4a98 | 32 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 33 | gyro_data[i] = floor(precision*gyro_data[i])/precision; |
xuweiqian9999 | 0:da91c8ed4a98 | 34 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 35 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 36 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 37 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 38 | |
xuweiqian9999 | 0:da91c8ed4a98 | 39 | void get_caliberate_data(float *caliberate, FXOS8700 *accel, FXAS21002 *gyro) { |
xuweiqian9999 | 0:da91c8ed4a98 | 40 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 41 | int j; |
xuweiqian9999 | 0:da91c8ed4a98 | 42 | float accel_data[3]; |
xuweiqian9999 | 0:da91c8ed4a98 | 43 | float gyro_data[3]; |
xuweiqian9999 | 0:da91c8ed4a98 | 44 | |
xuweiqian9999 | 0:da91c8ed4a98 | 45 | double temp_calib[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; |
xuweiqian9999 | 0:da91c8ed4a98 | 46 | |
xuweiqian9999 | 0:da91c8ed4a98 | 47 | for (i = 0; i < CALIBTIMES; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 48 | acquire_sensor_data(accel_data, gyro_data, NULL, 0, accel, gyro); |
xuweiqian9999 | 0:da91c8ed4a98 | 49 | for (j = 0; j < 3; j++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 50 | temp_calib[j] += accel_data[j]; |
xuweiqian9999 | 0:da91c8ed4a98 | 51 | temp_calib[j + 3] += gyro_data[j]; |
xuweiqian9999 | 0:da91c8ed4a98 | 52 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 53 | wait_ms(MAINWAIT); |
xuweiqian9999 | 0:da91c8ed4a98 | 54 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 55 | |
xuweiqian9999 | 0:da91c8ed4a98 | 56 | for (i = 0; i < 6; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 57 | temp_calib[i] /= (double)CALIBTIMES; |
xuweiqian9999 | 0:da91c8ed4a98 | 58 | caliberate[i] = (float)temp_calib[i]; |
xuweiqian9999 | 0:da91c8ed4a98 | 59 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 60 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 61 | |
xuweiqian9999 | 0:da91c8ed4a98 | 62 | void load_buffer(float **all_data, float *accel_data, float time, int i) { |
xuweiqian9999 | 0:da91c8ed4a98 | 63 | if (i == MAXBUFFERSIZE) { |
xuweiqian9999 | 0:da91c8ed4a98 | 64 | printf("Buffer full!\n\r"); |
xuweiqian9999 | 0:da91c8ed4a98 | 65 | exit(1); |
xuweiqian9999 | 0:da91c8ed4a98 | 66 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 67 | |
xuweiqian9999 | 0:da91c8ed4a98 | 68 | all_data[0][i] = time; |
xuweiqian9999 | 0:da91c8ed4a98 | 69 | all_data[1][i] = accel_data[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 70 | all_data[2][i] = accel_data[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 71 | all_data[3][i] = accel_data[2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 72 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 73 | |
xuweiqian9999 | 0:da91c8ed4a98 | 74 | void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples, |
xuweiqian9999 | 0:da91c8ed4a98 | 75 | float *last_total, float **all_data, float *caliberate, |
xuweiqian9999 | 0:da91c8ed4a98 | 76 | Timer *t, FXOS8700 *accel, FXAS21002 *gyro) { |
xuweiqian9999 | 0:da91c8ed4a98 | 77 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 78 | float accel_data[3]; |
xuweiqian9999 | 0:da91c8ed4a98 | 79 | float gyro_data[3]; |
xuweiqian9999 | 0:da91c8ed4a98 | 80 | float time; |
xuweiqian9999 | 0:da91c8ed4a98 | 81 | float total_accelx = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 82 | float total_accely = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 83 | float total_accelz = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 84 | |
xuweiqian9999 | 0:da91c8ed4a98 | 85 | for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 86 | acquire_sensor_data(accel_data, gyro_data, caliberate, 1, accel, gyro); |
xuweiqian9999 | 0:da91c8ed4a98 | 87 | time = t->read(); |
xuweiqian9999 | 0:da91c8ed4a98 | 88 | moving_avg_buf[0][i] = accel_data[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 89 | moving_avg_buf[1][i] = accel_data[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 90 | moving_avg_buf[2][i] = accel_data[2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 91 | moving_avg_buf[3][i] = time; |
xuweiqian9999 | 0:da91c8ed4a98 | 92 | wait_ms(MAINWAIT); |
xuweiqian9999 | 0:da91c8ed4a98 | 93 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 94 | |
xuweiqian9999 | 0:da91c8ed4a98 | 95 | for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 96 | total_accelx += moving_avg_buf[0][i]; |
xuweiqian9999 | 0:da91c8ed4a98 | 97 | total_accely += moving_avg_buf[1][i]; |
xuweiqian9999 | 0:da91c8ed4a98 | 98 | total_accelz += moving_avg_buf[2][i]; |
xuweiqian9999 | 0:da91c8ed4a98 | 99 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 100 | |
xuweiqian9999 | 0:da91c8ed4a98 | 101 | last_total[0] = total_accelx; |
xuweiqian9999 | 0:da91c8ed4a98 | 102 | last_total[1] = total_accely; |
xuweiqian9999 | 0:da91c8ed4a98 | 103 | last_total[2] = total_accelz; |
xuweiqian9999 | 0:da91c8ed4a98 | 104 | |
xuweiqian9999 | 0:da91c8ed4a98 | 105 | all_data[0][0] = moving_avg_buf[3][0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 106 | all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 107 | all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 108 | all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 109 | |
xuweiqian9999 | 0:da91c8ed4a98 | 110 | (*num_samples)++; |
xuweiqian9999 | 0:da91c8ed4a98 | 111 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 112 | |
xuweiqian9999 | 0:da91c8ed4a98 | 113 | void get_new_moving_average_point(float **all_data, float *last_total, |
xuweiqian9999 | 0:da91c8ed4a98 | 114 | float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE], |
xuweiqian9999 | 0:da91c8ed4a98 | 115 | float time, int *num_samples, int *start, |
xuweiqian9999 | 0:da91c8ed4a98 | 116 | int *end) { |
xuweiqian9999 | 0:da91c8ed4a98 | 117 | |
xuweiqian9999 | 0:da91c8ed4a98 | 118 | last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 119 | last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 120 | last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 121 | |
xuweiqian9999 | 0:da91c8ed4a98 | 122 | all_data[0][*num_samples] = moving_avg_buf[3][*start]; |
xuweiqian9999 | 0:da91c8ed4a98 | 123 | |
xuweiqian9999 | 0:da91c8ed4a98 | 124 | float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 125 | float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 126 | float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 127 | |
xuweiqian9999 | 0:da91c8ed4a98 | 128 | if (abs(temp_x) > (float) XMECHANICAL) { |
xuweiqian9999 | 0:da91c8ed4a98 | 129 | all_data[1][*num_samples] = temp_x; |
xuweiqian9999 | 0:da91c8ed4a98 | 130 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 131 | all_data[1][*num_samples] = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 132 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 133 | |
xuweiqian9999 | 0:da91c8ed4a98 | 134 | if (abs(temp_y) > (float) YMECHANICAL) { |
xuweiqian9999 | 0:da91c8ed4a98 | 135 | all_data[2][*num_samples] = temp_y; |
xuweiqian9999 | 0:da91c8ed4a98 | 136 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 137 | all_data[2][*num_samples] = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 138 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 139 | |
xuweiqian9999 | 0:da91c8ed4a98 | 140 | if (abs(temp_z) > (float) ZMECHANICAL) { |
xuweiqian9999 | 0:da91c8ed4a98 | 141 | all_data[3][*num_samples] = temp_z; |
xuweiqian9999 | 0:da91c8ed4a98 | 142 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 143 | all_data[3][*num_samples] = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 144 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 145 | |
xuweiqian9999 | 0:da91c8ed4a98 | 146 | *start = (*start + 1) % (int) MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 147 | *end = (*end + 1) % (int) MOVINGAVRBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 148 | |
xuweiqian9999 | 0:da91c8ed4a98 | 149 | moving_avg_buf[0][*end] = accel_data[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 150 | moving_avg_buf[1][*end] = accel_data[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 151 | moving_avg_buf[2][*end] = accel_data[2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 152 | moving_avg_buf[3][*end] = time; |
xuweiqian9999 | 0:da91c8ed4a98 | 153 | |
xuweiqian9999 | 0:da91c8ed4a98 | 154 | (*num_samples) += 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 155 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 156 | |
xuweiqian9999 | 0:da91c8ed4a98 | 157 | void apply_trend_protect(float **all_data, int num_samples, float *total_diff, |
xuweiqian9999 | 0:da91c8ed4a98 | 158 | float *additional_to_vel, float duration) { |
xuweiqian9999 | 0:da91c8ed4a98 | 159 | |
xuweiqian9999 | 0:da91c8ed4a98 | 160 | if (num_samples > TRENDPROTECTBUFSIZE) { |
xuweiqian9999 | 0:da91c8ed4a98 | 161 | total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 162 | total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 163 | |
xuweiqian9999 | 0:da91c8ed4a98 | 164 | total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 165 | total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 166 | |
xuweiqian9999 | 0:da91c8ed4a98 | 167 | if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) { |
xuweiqian9999 | 0:da91c8ed4a98 | 168 | additional_to_vel[0] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 169 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 170 | 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:da91c8ed4a98 | 171 | additional_to_vel[0] = avg_accel * duration; |
xuweiqian9999 | 0:da91c8ed4a98 | 172 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 173 | |
xuweiqian9999 | 0:da91c8ed4a98 | 174 | if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) { |
xuweiqian9999 | 0:da91c8ed4a98 | 175 | additional_to_vel[1] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 176 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 177 | 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:da91c8ed4a98 | 178 | additional_to_vel[1] = avg_accel * duration; |
xuweiqian9999 | 0:da91c8ed4a98 | 179 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 180 | |
xuweiqian9999 | 0:da91c8ed4a98 | 181 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 182 | if(num_samples == 1){ |
xuweiqian9999 | 0:da91c8ed4a98 | 183 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 184 | total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 185 | total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2]; |
xuweiqian9999 | 0:da91c8ed4a98 | 186 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 187 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 188 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 189 | |
xuweiqian9999 | 0:da91c8ed4a98 | 190 | |
xuweiqian9999 | 0:da91c8ed4a98 | 191 | |
xuweiqian9999 | 0:da91c8ed4a98 | 192 | void apply_move_end_check(float **all_data, int num_samples, |
xuweiqian9999 | 0:da91c8ed4a98 | 193 | int moving_end_buf[][MOVINGENDBUFSIZE], |
xuweiqian9999 | 0:da91c8ed4a98 | 194 | int *num_unqualified, int *start, int *end, |
xuweiqian9999 | 0:da91c8ed4a98 | 195 | float *addition_to_vel, float duration, |
xuweiqian9999 | 0:da91c8ed4a98 | 196 | float *total_diff) { |
xuweiqian9999 | 0:da91c8ed4a98 | 197 | |
xuweiqian9999 | 0:da91c8ed4a98 | 198 | if (num_samples > MOVINGENDBUFSIZE) { |
xuweiqian9999 | 0:da91c8ed4a98 | 199 | num_unqualified[0] -= moving_end_buf[0][*start]; |
xuweiqian9999 | 0:da91c8ed4a98 | 200 | num_unqualified[1] -= moving_end_buf[1][*start]; |
xuweiqian9999 | 0:da91c8ed4a98 | 201 | *start = (*start + 1) % MOVINGENDBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 202 | *end = (*end + 1) % MOVINGENDBUFSIZE; |
xuweiqian9999 | 0:da91c8ed4a98 | 203 | |
xuweiqian9999 | 0:da91c8ed4a98 | 204 | if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){ |
xuweiqian9999 | 0:da91c8ed4a98 | 205 | num_unqualified[0] += 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 206 | moving_end_buf[0][*end] = 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 207 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 208 | moving_end_buf[0][*end] = 0; |
xuweiqian9999 | 0:da91c8ed4a98 | 209 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 210 | |
xuweiqian9999 | 0:da91c8ed4a98 | 211 | if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){ |
xuweiqian9999 | 0:da91c8ed4a98 | 212 | num_unqualified[1] += 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 213 | moving_end_buf[1][*end] = 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 214 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 215 | moving_end_buf[1][*end] = 0; |
xuweiqian9999 | 0:da91c8ed4a98 | 216 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 217 | |
xuweiqian9999 | 0:da91c8ed4a98 | 218 | if (num_unqualified[0] >= (int)YMOVENDBIASNUM){ |
xuweiqian9999 | 0:da91c8ed4a98 | 219 | addition_to_vel[0] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 220 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 221 | |
xuweiqian9999 | 0:da91c8ed4a98 | 222 | if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){ |
xuweiqian9999 | 0:da91c8ed4a98 | 223 | addition_to_vel[1] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 224 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 225 | |
xuweiqian9999 | 0:da91c8ed4a98 | 226 | apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel, |
xuweiqian9999 | 0:da91c8ed4a98 | 227 | duration); |
xuweiqian9999 | 0:da91c8ed4a98 | 228 | |
xuweiqian9999 | 0:da91c8ed4a98 | 229 | if (num_unqualified[0] < (int)YMOVENDBIASNUM){ |
xuweiqian9999 | 0:da91c8ed4a98 | 230 | 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:da91c8ed4a98 | 231 | addition_to_vel[0] = avg_accel * duration; |
xuweiqian9999 | 0:da91c8ed4a98 | 232 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 233 | |
xuweiqian9999 | 0:da91c8ed4a98 | 234 | if (num_unqualified[1] < (int)ZMOVENDBIASNUM){ |
xuweiqian9999 | 0:da91c8ed4a98 | 235 | 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:da91c8ed4a98 | 236 | addition_to_vel[1] = avg_accel * duration; |
xuweiqian9999 | 0:da91c8ed4a98 | 237 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 238 | |
xuweiqian9999 | 0:da91c8ed4a98 | 239 | } else if (num_samples < MOVINGENDBUFSIZE) { |
xuweiqian9999 | 0:da91c8ed4a98 | 240 | addition_to_vel[0] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 241 | addition_to_vel[1] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 242 | apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel, |
xuweiqian9999 | 0:da91c8ed4a98 | 243 | duration); |
xuweiqian9999 | 0:da91c8ed4a98 | 244 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 245 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 246 | for (i = 0; i < MOVINGENDBUFSIZE; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 247 | if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) { |
xuweiqian9999 | 0:da91c8ed4a98 | 248 | moving_end_buf[0][i] = 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 249 | num_unqualified[0] += 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 250 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 251 | moving_end_buf[0][i] = 0; |
xuweiqian9999 | 0:da91c8ed4a98 | 252 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 253 | |
xuweiqian9999 | 0:da91c8ed4a98 | 254 | if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) { |
xuweiqian9999 | 0:da91c8ed4a98 | 255 | moving_end_buf[1][i] = 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 256 | num_unqualified[1] += 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 257 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 258 | moving_end_buf[1][i] = 0; |
xuweiqian9999 | 0:da91c8ed4a98 | 259 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 260 | |
xuweiqian9999 | 0:da91c8ed4a98 | 261 | addition_to_vel[0] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 262 | addition_to_vel[1] = -1000.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 263 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 264 | apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel, |
xuweiqian9999 | 0:da91c8ed4a98 | 265 | duration); |
xuweiqian9999 | 0:da91c8ed4a98 | 266 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 267 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 268 | |
xuweiqian9999 | 0:da91c8ed4a98 | 269 | void get_new_velocity (float *original_speed, float *addition_to_vel) { |
xuweiqian9999 | 0:da91c8ed4a98 | 270 | if (addition_to_vel[0] == -1000.0f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 271 | original_speed[0] = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 272 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 273 | original_speed[0] += addition_to_vel[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 274 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 275 | |
xuweiqian9999 | 0:da91c8ed4a98 | 276 | if (addition_to_vel[1] == -1000.0f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 277 | original_speed[1] = 0.0f; |
xuweiqian9999 | 0:da91c8ed4a98 | 278 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 279 | original_speed[1] += addition_to_vel[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 280 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 281 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 282 | |
xuweiqian9999 | 0:da91c8ed4a98 | 283 | void get_new_position (float *original_position, float *cur_speed, float duration) { |
xuweiqian9999 | 0:da91c8ed4a98 | 284 | original_position[0] += cur_speed[0] * duration; |
xuweiqian9999 | 0:da91c8ed4a98 | 285 | original_position[1] -= cur_speed[1] * duration; |
xuweiqian9999 | 0:da91c8ed4a98 | 286 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 287 | |
xuweiqian9999 | 0:da91c8ed4a98 | 288 | |
xuweiqian9999 | 0:da91c8ed4a98 | 289 | void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel, |
xuweiqian9999 | 0:da91c8ed4a98 | 290 | int num_samples) { |
xuweiqian9999 | 0:da91c8ed4a98 | 291 | |
xuweiqian9999 | 0:da91c8ed4a98 | 292 | vel_buffer[0][num_samples - 1] = time; |
xuweiqian9999 | 0:da91c8ed4a98 | 293 | vel_buffer[1][num_samples - 1] = cur_vel[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 294 | vel_buffer[2][num_samples - 1] = cur_vel[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 295 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 296 | |
xuweiqian9999 | 0:da91c8ed4a98 | 297 | void insert_new_pos_to_buffer(float** pos_buffer, float time, float* cur_pos, |
xuweiqian9999 | 0:da91c8ed4a98 | 298 | int num_samples) { |
xuweiqian9999 | 0:da91c8ed4a98 | 299 | pos_buffer[0][num_samples - 1] = time; |
xuweiqian9999 | 0:da91c8ed4a98 | 300 | pos_buffer[1][num_samples - 1] = cur_pos[0]; |
xuweiqian9999 | 0:da91c8ed4a98 | 301 | pos_buffer[2][num_samples - 1] = cur_pos[1]; |
xuweiqian9999 | 0:da91c8ed4a98 | 302 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 303 | |
xuweiqian9999 | 0:da91c8ed4a98 | 304 | void insert_new_color_to_buffer(float **color_buffer, int r, int g, int b, |
xuweiqian9999 | 0:da91c8ed4a98 | 305 | float time, int num_samples) { |
xuweiqian9999 | 0:da91c8ed4a98 | 306 | color_buffer[0][num_samples - 1] = time; |
xuweiqian9999 | 0:da91c8ed4a98 | 307 | color_buffer[1][num_samples - 1] = (float) r; |
xuweiqian9999 | 0:da91c8ed4a98 | 308 | color_buffer[2][num_samples - 1] = (float) g; |
xuweiqian9999 | 0:da91c8ed4a98 | 309 | color_buffer[3][num_samples - 1] = (float) b; |
xuweiqian9999 | 0:da91c8ed4a98 | 310 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 311 | |
xuweiqian9999 | 0:da91c8ed4a98 | 312 | void initialize_color_table(RGB rgb_table[][HORIZONTALNUMCOLOR]){ |
xuweiqian9999 | 1:b90027de2bdb | 313 | rgb_table[0][0] = (RGB){4, 77, 210}; |
xuweiqian9999 | 1:b90027de2bdb | 314 | rgb_table[0][1] = (RGB){5, 77, 77}; |
xuweiqian9999 | 1:b90027de2bdb | 315 | rgb_table[0][2] = (RGB){6, 179, 102}; |
xuweiqian9999 | 1:b90027de2bdb | 316 | rgb_table[0][3] = (RGB){7, 224, 133}; |
xuweiqian9999 | 1:b90027de2bdb | 317 | rgb_table[0][4] = (RGB){8, 121, 255}; |
xuweiqian9999 | 0:da91c8ed4a98 | 318 | |
xuweiqian9999 | 1:b90027de2bdb | 319 | rgb_table[1][0] = (RGB){9, 0, 191}; |
xuweiqian9999 | 1:b90027de2bdb | 320 | rgb_table[1][1] = (RGB){10, 0, 0}; |
xuweiqian9999 | 1:b90027de2bdb | 321 | rgb_table[1][2] = (RGB){11, 128, 0}; |
xuweiqian9999 | 1:b90027de2bdb | 322 | rgb_table[1][3] = (RGB){12, 204, 51}; |
xuweiqian9999 | 1:b90027de2bdb | 323 | rgb_table[1][4] = (RGB){13, 64, 255}; |
xuweiqian9999 | 0:da91c8ed4a98 | 324 | |
xuweiqian9999 | 1:b90027de2bdb | 325 | rgb_table[2][0] = (RGB){14, 0, 134}; |
xuweiqian9999 | 1:b90027de2bdb | 326 | rgb_table[2][1] = (RGB){15, 0, 0}; |
xuweiqian9999 | 1:b90027de2bdb | 327 | rgb_table[2][2] = (RGB){16, 102, 0}; |
xuweiqian9999 | 1:b90027de2bdb | 328 | rgb_table[2][3] = (RGB){17, 122, 31}; |
xuweiqian9999 | 1:b90027de2bdb | 329 | rgb_table[2][4] = (RGB){18, 45, 179}; |
xuweiqian9999 | 0:da91c8ed4a98 | 330 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 331 | |
xuweiqian9999 | 0:da91c8ed4a98 | 332 | RGB get_new_color(float *cur_location, RGB rgb_table[][HORIZONTALNUMCOLOR]) { |
xuweiqian9999 | 0:da91c8ed4a98 | 333 | int x; |
xuweiqian9999 | 0:da91c8ed4a98 | 334 | int y; |
xuweiqian9999 | 0:da91c8ed4a98 | 335 | |
xuweiqian9999 | 0:da91c8ed4a98 | 336 | if (cur_location[0] < -0.03f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 337 | x = 0; |
xuweiqian9999 | 0:da91c8ed4a98 | 338 | } else if (cur_location[0] >= -0.03f && cur_location[0] < -0.01f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 339 | x = 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 340 | } else if (cur_location[0] >= -0.01f && cur_location[0] < 0.01f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 341 | x = 2; |
xuweiqian9999 | 0:da91c8ed4a98 | 342 | } else if (cur_location[0] >= 0.01f && cur_location[0] < 0.03f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 343 | x = 3; |
xuweiqian9999 | 0:da91c8ed4a98 | 344 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 345 | x = 4; |
xuweiqian9999 | 0:da91c8ed4a98 | 346 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 347 | |
xuweiqian9999 | 0:da91c8ed4a98 | 348 | if (cur_location[1] > 0.0165f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 349 | y = 0; |
xuweiqian9999 | 0:da91c8ed4a98 | 350 | } else if (cur_location[1] > -0.0165f && cur_location[1] <= 0.0165f) { |
xuweiqian9999 | 0:da91c8ed4a98 | 351 | y = 1; |
xuweiqian9999 | 0:da91c8ed4a98 | 352 | } else { |
xuweiqian9999 | 0:da91c8ed4a98 | 353 | y = 2; |
xuweiqian9999 | 0:da91c8ed4a98 | 354 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 355 | |
xuweiqian9999 | 0:da91c8ed4a98 | 356 | return rgb_table[y][x]; |
xuweiqian9999 | 0:da91c8ed4a98 | 357 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 358 | |
xuweiqian9999 | 0:da91c8ed4a98 | 359 | void output_all_to_serial(float **all_data, int num_samples) { |
xuweiqian9999 | 0:da91c8ed4a98 | 360 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 361 | for (i = 0; i < num_samples; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 362 | 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:da91c8ed4a98 | 363 | wait(0.01); |
xuweiqian9999 | 0:da91c8ed4a98 | 364 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 365 | printf("Number of samples = %d\n", num_samples); |
xuweiqian9999 | 0:da91c8ed4a98 | 366 | printf ("End Transmission!\n"); |
xuweiqian9999 | 0:da91c8ed4a98 | 367 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 368 | |
xuweiqian9999 | 0:da91c8ed4a98 | 369 | |
xuweiqian9999 | 0:da91c8ed4a98 | 370 | void output_color_to_serial(float **color_data, int num_samples) { |
xuweiqian9999 | 0:da91c8ed4a98 | 371 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 372 | for (i = 0; i < num_samples; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 373 | printf("%6.3f,%d,%d,%d\n", color_data[0][i], (int)color_data[1][i], |
xuweiqian9999 | 0:da91c8ed4a98 | 374 | (int)color_data[2][i],(int)color_data[3][i]); |
xuweiqian9999 | 0:da91c8ed4a98 | 375 | wait(0.01); |
xuweiqian9999 | 0:da91c8ed4a98 | 376 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 377 | printf("Number of samples = %d\n", num_samples); |
xuweiqian9999 | 0:da91c8ed4a98 | 378 | printf ("End Transmission!\n"); |
xuweiqian9999 | 0:da91c8ed4a98 | 379 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 380 | |
xuweiqian9999 | 0:da91c8ed4a98 | 381 | void output_to_serial(float **vel_data, int num_samples) { |
xuweiqian9999 | 0:da91c8ed4a98 | 382 | int i; |
xuweiqian9999 | 0:da91c8ed4a98 | 383 | for (i = 0; i < num_samples; i++) { |
xuweiqian9999 | 0:da91c8ed4a98 | 384 | printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]); |
xuweiqian9999 | 0:da91c8ed4a98 | 385 | wait(0.01); |
xuweiqian9999 | 0:da91c8ed4a98 | 386 | } |
xuweiqian9999 | 0:da91c8ed4a98 | 387 | printf("Number of samples = %d\n", num_samples); |
xuweiqian9999 | 0:da91c8ed4a98 | 388 | printf ("End Transmission!\n"); |
xuweiqian9999 | 0:da91c8ed4a98 | 389 | } |