Mari Mishel / Mbed 2 deprecated TAU_Studets_Motion_IKS01A1_2nd

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@13:da8097acb518, 2017-08-08 (annotated)

Committer:
HolyMari
Date:
Tue Aug 08 16:32:14 2017 +0000
Revision:
13:da8097acb518
Parent:
11:4a99e73b88cc 
//

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wolfgang Betz 0:c71c9af137dd 1 /**
Wolfgang Betz 0:c71c9af137dd 2 ******************************************************************************
Wolfgang Betz 0:c71c9af137dd 3 * @file main.cpp
Wolfgang Betz 0:c71c9af137dd 4 * @author AST / EST
Wolfgang Betz 0:c71c9af137dd 5 * @version V0.0.1
Wolfgang Betz 0:c71c9af137dd 6 * @date 14-August-2015
Arkadi 11:4a99e73b88cc 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A1
Wolfgang Betz 0:c71c9af137dd 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
Wolfgang Betz 0:c71c9af137dd 9 ******************************************************************************
Wolfgang Betz 0:c71c9af137dd 10 * @attention
Wolfgang Betz 0:c71c9af137dd 11 *
Wolfgang Betz 0:c71c9af137dd 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Wolfgang Betz 0:c71c9af137dd 13 *
Wolfgang Betz 0:c71c9af137dd 14 * Redistribution and use in source and binary forms, with or without modification,
Wolfgang Betz 0:c71c9af137dd 15 * are permitted provided that the following conditions are met:
Wolfgang Betz 0:c71c9af137dd 16 * 1. Redistributions of source code must retain the above copyright notice,
Wolfgang Betz 0:c71c9af137dd 17 * this list of conditions and the following disclaimer.
Wolfgang Betz 0:c71c9af137dd 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Wolfgang Betz 0:c71c9af137dd 19 * this list of conditions and the following disclaimer in the documentation
Wolfgang Betz 0:c71c9af137dd 20 * and/or other materials provided with the distribution.
Wolfgang Betz 0:c71c9af137dd 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Wolfgang Betz 0:c71c9af137dd 22 * may be used to endorse or promote products derived from this software
Wolfgang Betz 0:c71c9af137dd 23 * without specific prior written permission.
Wolfgang Betz 0:c71c9af137dd 24 *
Wolfgang Betz 0:c71c9af137dd 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Wolfgang Betz 0:c71c9af137dd 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Wolfgang Betz 0:c71c9af137dd 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Wolfgang Betz 0:c71c9af137dd 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Wolfgang Betz 0:c71c9af137dd 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Wolfgang Betz 0:c71c9af137dd 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Wolfgang Betz 0:c71c9af137dd 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Wolfgang Betz 0:c71c9af137dd 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Wolfgang Betz 0:c71c9af137dd 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Wolfgang Betz 0:c71c9af137dd 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Wolfgang Betz 0:c71c9af137dd 35 *
Wolfgang Betz 0:c71c9af137dd 36 ******************************************************************************
Arkadi 11:4a99e73b88cc 37 */
Wolfgang Betz 0:c71c9af137dd 38
HolyMari 13:da8097acb518 39 /**************************************************************
HolyMari 13:da8097acb518 40 *************** THIS HANDLES HOPPER WINDOW ********************
HolyMari 13:da8097acb518 41 ***************************************************************/
HolyMari 13:da8097acb518 42
Wolfgang Betz 0:c71c9af137dd 43 /* Includes */
Wolfgang Betz 0:c71c9af137dd 44 #include "mbed.h"
Wolfgang Betz 0:c71c9af137dd 45 #include "x_nucleo_iks01a1.h"
Wolfgang Betz 0:c71c9af137dd 46
Arkadi 11:4a99e73b88cc 47 // define sample frequency
HolyMari 13:da8097acb518 48 #define SAMPLES_DELAY 5000.f
HolyMari 13:da8097acb518 49 #define ARR_LEN 100
HolyMari 13:da8097acb518 50 #define MG2MM 100.f
HolyMari 13:da8097acb518 51 #define GYRO_FACTOR 1000.f
HolyMari 13:da8097acb518 52
HolyMari 13:da8097acb518 53 // thresholds
HolyMari 13:da8097acb518 54 #define EVENT_THRESHOLD 1500.f
HolyMari 13:da8097acb518 55 #define MOVEMENT_THRESHOLD 5.f
HolyMari 13:da8097acb518 56 #define VEL_THRESHOLD 15.f
HolyMari 13:da8097acb518 57 #define STILL_THRESHOLD 2.f
HolyMari 13:da8097acb518 58 #define OPEN_THRESHOLD 5.f
HolyMari 13:da8097acb518 59 #define CLOSE_THRESHOLD 5.f
HolyMari 13:da8097acb518 60
HolyMari 13:da8097acb518 61 #define PI 3.14159265f
HolyMari 13:da8097acb518 62
Wolfgang Betz 0:c71c9af137dd 63 /* Instantiate the expansion board */
Wolfgang Betz 4:b1526d074d83 64 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
Wolfgang Betz 0:c71c9af137dd 65
Wolfgang Betz 0:c71c9af137dd 66 /* Retrieve the composing elements of the expansion board */
Wolfgang Betz 0:c71c9af137dd 67 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
Wolfgang Betz 0:c71c9af137dd 68 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
Arkadi 11:4a99e73b88cc 69
Arkadi 11:4a99e73b88cc 70 // sensor sample rate: x_nucleo_iks01a1.cpp ; lsm6ds0_class.cpp
Wolfgang Betz 0:c71c9af137dd 71
Wolfgang Betz 0:c71c9af137dd 72
Arkadi 11:4a99e73b88cc 73 // define uart
Arkadi 11:4a99e73b88cc 74 Serial pc(USBTX, USBRX);
Wolfgang Betz 0:c71c9af137dd 75
Arkadi 11:4a99e73b88cc 76 // timer object
Arkadi 11:4a99e73b88cc 77 Timer timer;
Wolfgang Betz 0:c71c9af137dd 78
Arkadi 11:4a99e73b88cc 79 // send data
Arkadi 11:4a99e73b88cc 80 bool flagData=0;
HolyMari 13:da8097acb518 81 float sum_v = 0, sum_s = 0;
HolyMari 13:da8097acb518 82 float gyro_bias[3], acc_bias = 0;
HolyMari 13:da8097acb518 83 int axis = 0;
Wolfgang Betz 0:c71c9af137dd 84
Arkadi 11:4a99e73b88cc 85 // time stamp
Arkadi 11:4a99e73b88cc 86 int timeStamp_us=0;
Arkadi 11:4a99e73b88cc 87 int sampleStamp=0;
Wolfgang Betz 0:c71c9af137dd 88
HolyMari 13:da8097acb518 89
HolyMari 13:da8097acb518 90 DigitalOut led1(LED1);
HolyMari 13:da8097acb518 91
HolyMari 13:da8097acb518 92 /*
HolyMari 13:da8097acb518 93 float pitch(float x, float y, float z) {
HolyMari 13:da8097acb518 94 float calc, angle;
HolyMari 13:da8097acb518 95 calc = pow(x, 2) / sqrt(pow(y, 2) + pow(z, 2));
HolyMari 13:da8097acb518 96 angle = atan(calc) * 180 / PI;
HolyMari 13:da8097acb518 97 return angle;
HolyMari 13:da8097acb518 98 }
HolyMari 13:da8097acb518 99
HolyMari 13:da8097acb518 100
HolyMari 13:da8097acb518 101 float roll(float x, float y, float z) {
HolyMari 13:da8097acb518 102 float calc, angle;
HolyMari 13:da8097acb518 103 calc = pow(y, 2) / sqrt(pow(x, 2) + pow(z, 2));
HolyMari 13:da8097acb518 104 angle = atan(calc) * 180 / PI;
HolyMari 13:da8097acb518 105 return angle;
HolyMari 13:da8097acb518 106 }
HolyMari 13:da8097acb518 107 */
HolyMari 13:da8097acb518 108
HolyMari 13:da8097acb518 109 float lowpass(float sample, float prev_lp) {
HolyMari 13:da8097acb518 110 float lp_factor = 0.005;
HolyMari 13:da8097acb518 111 float new_lp;
HolyMari 13:da8097acb518 112 new_lp = sample * lp_factor + prev_lp * (1 - lp_factor);
HolyMari 13:da8097acb518 113 return new_lp;
HolyMari 13:da8097acb518 114 }
HolyMari 13:da8097acb518 115
HolyMari 13:da8097acb518 116
HolyMari 13:da8097acb518 117 float highpass(float sample, float prev_sample, float prev_hp) {
HolyMari 13:da8097acb518 118 float hp_factor = 0.9;
HolyMari 13:da8097acb518 119 float new_hp;
HolyMari 13:da8097acb518 120 new_hp = prev_hp * hp_factor + (sample - prev_sample) * hp_factor;
HolyMari 13:da8097acb518 121 return new_hp;
HolyMari 13:da8097acb518 122 }
HolyMari 13:da8097acb518 123
HolyMari 13:da8097acb518 124
HolyMari 13:da8097acb518 125 float integrate(float sample, float prev_sum) {
HolyMari 13:da8097acb518 126 float ret;
HolyMari 13:da8097acb518 127 ret = prev_sum + sample;
HolyMari 13:da8097acb518 128 return ret;
HolyMari 13:da8097acb518 129 }
HolyMari 13:da8097acb518 130
HolyMari 13:da8097acb518 131 /*
HolyMari 13:da8097acb518 132 bool check_event_start(float* samples, int i) {
HolyMari 13:da8097acb518 133 sum_v = 0;
HolyMari 13:da8097acb518 134 sum_s = 0;
HolyMari 13:da8097acb518 135 for (int j=1; j <= ARR_LEN; j++){
HolyMari 13:da8097acb518 136 sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], sum_v);
HolyMari 13:da8097acb518 137 float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
HolyMari 13:da8097acb518 138 sum_s = integrate(effective_vel, sum_s);
HolyMari 13:da8097acb518 139 }
HolyMari 13:da8097acb518 140 sum_s = sum_s / 1000;
HolyMari 13:da8097acb518 141 if (abs(sum_s) > MOVEMENT_THRESHOLD) {
HolyMari 13:da8097acb518 142 return true;
HolyMari 13:da8097acb518 143 }
HolyMari 13:da8097acb518 144 return false;
HolyMari 13:da8097acb518 145 }*/
HolyMari 13:da8097acb518 146
HolyMari 13:da8097acb518 147
HolyMari 13:da8097acb518 148 /*bool check_event_end(float* samples, int i) {
HolyMari 13:da8097acb518 149 float temp_sum_v = 0, temp_sum_s = 0;
HolyMari 13:da8097acb518 150 for (uint8_t j=1; j <= ARR_LEN; j++){
HolyMari 13:da8097acb518 151 temp_sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], temp_sum_v);
HolyMari 13:da8097acb518 152 temp_sum_s = integrate(temp_sum_v, temp_sum_s);
HolyMari 13:da8097acb518 153 }
HolyMari 13:da8097acb518 154
HolyMari 13:da8097acb518 155 if (temp_sum_s < STILL_THRESHOLD) {
HolyMari 13:da8097acb518 156 return false;
HolyMari 13:da8097acb518 157 }
HolyMari 13:da8097acb518 158 return true;
HolyMari 13:da8097acb518 159 }*/
HolyMari 13:da8097acb518 160
Wolfgang Betz 0:c71c9af137dd 161 /* Simple main function */
Arkadi 11:4a99e73b88cc 162 int main()
Arkadi 11:4a99e73b88cc 163 {
Arkadi 11:4a99e73b88cc 164 pc.baud(921600);
Arkadi 11:4a99e73b88cc 165 timer.start();
Arkadi 11:4a99e73b88cc 166 timeStamp_us = timer.read_us();
HolyMari 13:da8097acb518 167 int i = 0, j;
Arkadi 11:4a99e73b88cc 168 uint8_t id;
Arkadi 11:4a99e73b88cc 169 int32_t Acc_axes[3];
Arkadi 11:4a99e73b88cc 170 int32_t Gyro_axes[3];
HolyMari 13:da8097acb518 171 //float diff, prev_lp = 0, prev_hp = 0;
HolyMari 13:da8097acb518 172 float acc_samp[3][ARR_LEN] = {}, acc_lp[3][ARR_LEN] = {};
HolyMari 13:da8097acb518 173 float gyro_samp[3][ARR_LEN] = {}, gyro_rad[3][ARR_LEN] = {}, gyro_hp[3][ARR_LEN] = {};
HolyMari 13:da8097acb518 174 float complementary[3][ARR_LEN] = {};
HolyMari 13:da8097acb518 175 //float pitch_calc[ARR_LEN], roll_calc[ARR_LEN];
HolyMari 13:da8097acb518 176 bool opened = false;
HolyMari 13:da8097acb518 177
HolyMari 13:da8097acb518 178 // get init data and bias
HolyMari 13:da8097acb518 179 for(j=0; j < ARR_LEN; j++){
HolyMari 13:da8097acb518 180 accelerometer->get_x_axes(Acc_axes);
HolyMari 13:da8097acb518 181 gyroscope->get_g_axes(Gyro_axes);
HolyMari 13:da8097acb518 182 //pc.printf("%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
HolyMari 13:da8097acb518 183
HolyMari 13:da8097acb518 184 // get accelerometer starting data
HolyMari 13:da8097acb518 185 acc_samp[0][j % ARR_LEN] = Acc_axes[0];
HolyMari 13:da8097acb518 186 acc_samp[1][j % ARR_LEN] = Acc_axes[1];
HolyMari 13:da8097acb518 187 acc_samp[2][j % ARR_LEN] = Acc_axes[2];
HolyMari 13:da8097acb518 188 acc_bias += (acc_samp[1][j % ARR_LEN] / ARR_LEN);
HolyMari 13:da8097acb518 189
HolyMari 13:da8097acb518 190 // get gyroscope starting data
HolyMari 13:da8097acb518 191 gyro_samp[0][j % ARR_LEN] = Gyro_axes[0] / GYRO_FACTOR;
HolyMari 13:da8097acb518 192 gyro_samp[1][j % ARR_LEN] = Gyro_axes[1] / GYRO_FACTOR;
HolyMari 13:da8097acb518 193 gyro_samp[2][j % ARR_LEN] = Gyro_axes[2] / GYRO_FACTOR;
HolyMari 13:da8097acb518 194 gyro_bias[0] += (gyro_samp[0][j % ARR_LEN] / ARR_LEN);
HolyMari 13:da8097acb518 195 gyro_bias[1] += (gyro_samp[1][j % ARR_LEN] / ARR_LEN);
HolyMari 13:da8097acb518 196 gyro_bias[2] += (gyro_samp[2][j % ARR_LEN] / ARR_LEN);
HolyMari 13:da8097acb518 197 }
HolyMari 13:da8097acb518 198
HolyMari 13:da8097acb518 199 // get init lowpass on acc, integral + highpass on gyro and complementary
HolyMari 13:da8097acb518 200 for(j=0; j < ARR_LEN; j++){
HolyMari 13:da8097acb518 201 // get acc lowpass data
HolyMari 13:da8097acb518 202 acc_lp[0][j % ARR_LEN] = lowpass(acc_samp[0][j % ARR_LEN], acc_lp[0][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 203 acc_lp[1][j % ARR_LEN] = lowpass(acc_samp[1][j % ARR_LEN], acc_lp[1][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 204 acc_lp[2][j % ARR_LEN] = lowpass(acc_samp[2][j % ARR_LEN], acc_lp[2][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 205
HolyMari 13:da8097acb518 206 // get gyro highpass data
HolyMari 13:da8097acb518 207 gyro_rad[0][j % ARR_LEN] = integrate(gyro_samp[0][j % ARR_LEN] - gyro_bias[0], gyro_rad[0][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 208 gyro_rad[1][j % ARR_LEN] = integrate(gyro_samp[1][j % ARR_LEN] - gyro_bias[1], gyro_rad[1][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 209 gyro_rad[2][j % ARR_LEN] = integrate(gyro_samp[2][j % ARR_LEN] - gyro_bias[2], gyro_rad[2][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 210 gyro_hp[0][j % ARR_LEN] = highpass(gyro_rad[0][j % ARR_LEN], gyro_rad[0][(j-1) % ARR_LEN], gyro_hp[0][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 211 gyro_hp[1][j % ARR_LEN] = highpass(gyro_rad[1][j % ARR_LEN], gyro_rad[1][(j-1) % ARR_LEN], gyro_hp[1][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 212 gyro_hp[2][j % ARR_LEN] = highpass(gyro_rad[2][j % ARR_LEN], gyro_rad[2][(j-1) % ARR_LEN], gyro_hp[2][(j-1) % ARR_LEN]);
HolyMari 13:da8097acb518 213
HolyMari 13:da8097acb518 214 // combine to complementary
HolyMari 13:da8097acb518 215 complementary[0][j % ARR_LEN] = acc_lp[0][j % ARR_LEN] + gyro_hp[0][j % ARR_LEN];
HolyMari 13:da8097acb518 216 complementary[1][j % ARR_LEN] = acc_lp[1][j % ARR_LEN] + gyro_hp[1][j % ARR_LEN];
HolyMari 13:da8097acb518 217 complementary[2][j % ARR_LEN] = acc_lp[2][j % ARR_LEN] + gyro_hp[2][j % ARR_LEN];
HolyMari 13:da8097acb518 218
HolyMari 13:da8097acb518 219 // get pitch and roll
HolyMari 13:da8097acb518 220 //pitch_calc[j % ARR_LEN] = pitch(complementary[0][j % ARR_LEN], complementary[1][j % ARR_LEN], complementary[2][j % ARR_LEN]);
HolyMari 13:da8097acb518 221 //roll_calc[j % ARR_LEN] = roll(complementary[0][j % ARR_LEN], complementary[1][j % ARR_LEN], complementary[2][j % ARR_LEN]);
HolyMari 13:da8097acb518 222 //pc.printf("pitch: %f\t roll: %f\r\n", pitch_calc[j % ARR_LEN], roll_calc[j % ARR_LEN]);
HolyMari 13:da8097acb518 223 }
HolyMari 13:da8097acb518 224
Arkadi 11:4a99e73b88cc 225 pc.printf("\r\n--- Starting new run ---\r\n");
Arkadi 11:4a99e73b88cc 226
Arkadi 11:4a99e73b88cc 227 gyroscope->read_id(&id);
Arkadi 11:4a99e73b88cc 228 pc.printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
HolyMari 13:da8097acb518 229 pc.printf("bias for gyro: %f, %f, %f\r\n", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
HolyMari 13:da8097acb518 230
Arkadi 11:4a99e73b88cc 231 wait(0.5);
Wolfgang Betz 0:c71c9af137dd 232
Arkadi 11:4a99e73b88cc 233 while(1) {
Arkadi 11:4a99e73b88cc 234 // get time stamp
Arkadi 11:4a99e73b88cc 235 timeStamp_us = timer.read_us();
Arkadi 11:4a99e73b88cc 236 // sample delay
Arkadi 11:4a99e73b88cc 237 if ((timeStamp_us-sampleStamp)>SAMPLES_DELAY) {
Arkadi 11:4a99e73b88cc 238 sampleStamp=timeStamp_us;
Arkadi 11:4a99e73b88cc 239 // get raw data
Arkadi 11:4a99e73b88cc 240 accelerometer->get_x_axes(Acc_axes);
Arkadi 11:4a99e73b88cc 241 gyroscope->get_g_axes(Gyro_axes);
HolyMari 13:da8097acb518 242 //pc.printf("%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
HolyMari 13:da8097acb518 243
HolyMari 13:da8097acb518 244 // update acc data
HolyMari 13:da8097acb518 245 acc_samp[0][i % ARR_LEN] = Acc_axes[0];
HolyMari 13:da8097acb518 246 acc_samp[1][i % ARR_LEN] = Acc_axes[1];
HolyMari 13:da8097acb518 247 acc_samp[2][i % ARR_LEN] = Acc_axes[2];
HolyMari 13:da8097acb518 248 // update gyro data
HolyMari 13:da8097acb518 249 gyro_samp[0][i % ARR_LEN] = Gyro_axes[0] / GYRO_FACTOR;
HolyMari 13:da8097acb518 250 gyro_samp[1][i % ARR_LEN] = Gyro_axes[1] / GYRO_FACTOR;
HolyMari 13:da8097acb518 251 gyro_samp[2][i % ARR_LEN] = Gyro_axes[2] / GYRO_FACTOR;
HolyMari 13:da8097acb518 252
HolyMari 13:da8097acb518 253 // get acc lowpass data
HolyMari 13:da8097acb518 254 acc_lp[0][i % ARR_LEN] = lowpass(acc_samp[0][i % ARR_LEN], acc_lp[0][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 255 acc_lp[1][i % ARR_LEN] = lowpass(acc_samp[1][i % ARR_LEN], acc_lp[1][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 256 acc_lp[2][i % ARR_LEN] = lowpass(acc_samp[2][i % ARR_LEN], acc_lp[2][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 257
HolyMari 13:da8097acb518 258 // get gyro highpass data
HolyMari 13:da8097acb518 259 gyro_rad[0][i % ARR_LEN] = integrate(gyro_samp[0][i % ARR_LEN] - gyro_bias[0], gyro_rad[0][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 260 gyro_rad[1][i % ARR_LEN] = integrate(gyro_samp[1][i % ARR_LEN] - gyro_bias[1], gyro_rad[1][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 261 gyro_rad[2][i % ARR_LEN] = integrate(gyro_samp[2][i % ARR_LEN] - gyro_bias[2], gyro_rad[2][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 262 gyro_hp[0][i % ARR_LEN] = highpass(gyro_rad[0][i % ARR_LEN], gyro_rad[0][(j-1) % ARR_LEN], gyro_hp[0][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 263 gyro_hp[1][i % ARR_LEN] = highpass(gyro_rad[1][i % ARR_LEN], gyro_rad[1][(j-1) % ARR_LEN], gyro_hp[1][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 264 gyro_hp[2][i % ARR_LEN] = highpass(gyro_rad[2][i % ARR_LEN], gyro_rad[2][(j-1) % ARR_LEN], gyro_hp[2][(i-1) % ARR_LEN]);
HolyMari 13:da8097acb518 265
HolyMari 13:da8097acb518 266 //pc.printf("%f acc0, %f acc1, %f acc2\r\n", acc_lp[0][i % ARR_LEN], acc_lp[1][i % ARR_LEN], acc_lp[2][i % ARR_LEN]);
HolyMari 13:da8097acb518 267
HolyMari 13:da8097acb518 268
HolyMari 13:da8097acb518 269 // combine to complementary
HolyMari 13:da8097acb518 270 complementary[0][i % ARR_LEN] = acc_lp[0][i % ARR_LEN] + gyro_hp[0][i % ARR_LEN];
HolyMari 13:da8097acb518 271 complementary[1][i % ARR_LEN] = acc_lp[1][i % ARR_LEN] + gyro_hp[1][i % ARR_LEN];
HolyMari 13:da8097acb518 272 complementary[2][i % ARR_LEN] = acc_lp[2][i % ARR_LEN] + gyro_hp[2][i % ARR_LEN];
HolyMari 13:da8097acb518 273
HolyMari 13:da8097acb518 274 //pc.printf("%f acc0, %f compl0\r\n" ,acc_lp[0][i % ARR_LEN], complementary[0][i % ARR_LEN]); // gryo_rad[0]
HolyMari 13:da8097acb518 275
HolyMari 13:da8097acb518 276 // get pitch and roll
HolyMari 13:da8097acb518 277 //pitch_calc[i % ARR_LEN] = pitch(complementary[0][i % ARR_LEN], complementary[1][i % ARR_LEN], complementary[2][i % ARR_LEN]);
HolyMari 13:da8097acb518 278 //roll_calc[i % ARR_LEN] = roll(complementary[0][i % ARR_LEN], complementary[1][i % ARR_LEN], complementary[2][i % ARR_LEN]);
HolyMari 13:da8097acb518 279
HolyMari 13:da8097acb518 280 //pc.printf("pitch: %f\t roll: %f\r\n", pitch_calc[i % ARR_LEN], roll_calc[i % ARR_LEN]);
HolyMari 13:da8097acb518 281
HolyMari 13:da8097acb518 282 //diff = abs(samples[axis][i % ARR_LEN] - samples[axis][(i - (ARR_LEN / 2)) % ARR_LEN]);
HolyMari 13:da8097acb518 283 // pc.printf("%f,%f,%f\t%f\r\n", samples[0][i % ARR_LEN], samples[1][i % ARR_LEN], samples[2][i % ARR_LEN], diff);
HolyMari 13:da8097acb518 284 if (abs(complementary[0][i % ARR_LEN]) > EVENT_THRESHOLD) {
HolyMari 13:da8097acb518 285 if (abs(acc_samp[1][i % ARR_LEN] - acc_bias) > 35 ){
HolyMari 13:da8097acb518 286 opened = true;
HolyMari 13:da8097acb518 287 led1 =true;
HolyMari 13:da8097acb518 288 }
HolyMari 13:da8097acb518 289 else if (abs(acc_samp[1][i % ARR_LEN] - acc_bias) < 15){
HolyMari 13:da8097acb518 290 opened = false;
HolyMari 13:da8097acb518 291 led1 = false;
HolyMari 13:da8097acb518 292 }
HolyMari 13:da8097acb518 293 }
HolyMari 13:da8097acb518 294 /*if(opened){
HolyMari 13:da8097acb518 295 pc.printf("1, %f\r\n", abs(acc_samp[1][i % ARR_LEN] - acc_bias));
HolyMari 13:da8097acb518 296 }
HolyMari 13:da8097acb518 297 else {
HolyMari 13:da8097acb518 298 pc.printf("0, %f\r\n", abs(acc_samp[1][i % ARR_LEN] - acc_bias));
HolyMari 13:da8097acb518 299 }*/
HolyMari 13:da8097acb518 300 i++;
HolyMari 13:da8097acb518 301 /*//pc.printf("suspected event: ");
HolyMari 13:da8097acb518 302 event_flag = check_event_start(samples[axis], i);
HolyMari 13:da8097acb518 303 if(event_flag) {
HolyMari 13:da8097acb518 304 pc.printf("OPENED: %f\r\n", sum_s);
HolyMari 13:da8097acb518 305 }
HolyMari 13:da8097acb518 306 else {
HolyMari 13:da8097acb518 307 pc.printf("False sum_s: %f\r\n", sum_s);
HolyMari 13:da8097acb518 308 }
HolyMari 13:da8097acb518 309 //event_flag = false
HolyMari 13:da8097acb518 310 }
HolyMari 13:da8097acb518 311 else if (event_flag){
HolyMari 13:da8097acb518 312 sum_v = integrate(samples[axis][i % ARR_LEN] - bias[axis], sum_v);
HolyMari 13:da8097acb518 313 //float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
HolyMari 13:da8097acb518 314 sum_v = abs(sum_v) < VEL_THRESHOLD ? 0 : sum_v;
HolyMari 13:da8097acb518 315 sum_s = integrate(sum_v, sum_s);
HolyMari 13:da8097acb518 316 pc.printf("sum_v: %f, sum_s: %f\r\n", sum_v, sum_s);
HolyMari 13:da8097acb518 317 if (abs(sum_s) <= CLOSE_THRESHOLD){
HolyMari 13:da8097acb518 318 pc.printf("CLOSED: %f\r\n", sum_s);
HolyMari 13:da8097acb518 319 event_flag = false;
HolyMari 13:da8097acb518 320 sum_s = 0;
HolyMari 13:da8097acb518 321 sum_v = 0;
HolyMari 13:da8097acb518 322 }
HolyMari 13:da8097acb518 323 }*/
Arkadi 11:4a99e73b88cc 324 }// end get & send samples
Arkadi 11:4a99e73b88cc 325 }// end loop
Arkadi 11:4a99e73b88cc 326 }// end main
HolyMari 13:da8097acb518 327
HolyMari 13:da8097acb518 328
HolyMari 13:da8097acb518 329
HolyMari 13:da8097acb518 330 //TODO list:
HolyMari 13:da8097acb518 331 // compute event movement
HolyMari 13:da8097acb518 332 // find event ending
HolyMari 13:da8097acb518 333 // display 0,1