File content as of revision 13:da8097acb518:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  AST / EST
 * @version V0.0.1
 * @date    14-August-2015
 * @brief   Simple Example application for using the X_NUCLEO_IKS01A1
 *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
*/

/**************************************************************
*************** THIS HANDLES HOPPER WINDOW ********************
***************************************************************/

/* Includes */
#include "mbed.h"
#include "x_nucleo_iks01a1.h"

// define sample frequency
#define SAMPLES_DELAY 5000.f
#define ARR_LEN 100
#define MG2MM 100.f
#define GYRO_FACTOR 1000.f

// thresholds
#define EVENT_THRESHOLD 1500.f
#define MOVEMENT_THRESHOLD 5.f
#define VEL_THRESHOLD 15.f
#define STILL_THRESHOLD 2.f
#define OPEN_THRESHOLD 5.f
#define CLOSE_THRESHOLD 5.f

#define PI 3.14159265f

/* Instantiate the expansion board */
static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);

/* Retrieve the composing elements of the expansion board */
static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();

// sensor sample rate: x_nucleo_iks01a1.cpp ; lsm6ds0_class.cpp


// define uart
Serial pc(USBTX, USBRX);

// timer object
Timer timer;

// send data
bool flagData=0;
float sum_v = 0, sum_s = 0;
float gyro_bias[3], acc_bias = 0;
int axis = 0;

// time stamp
int timeStamp_us=0;
int sampleStamp=0;


DigitalOut led1(LED1);

/*
float pitch(float x, float y, float z) {
    float calc, angle;
    calc = pow(x, 2) / sqrt(pow(y, 2) + pow(z, 2));
    angle = atan(calc) * 180 / PI;
    return angle;
}


float roll(float x, float y, float z) {
    float calc, angle;
    calc = pow(y, 2) / sqrt(pow(x, 2) + pow(z, 2));
    angle = atan(calc) * 180 / PI;
    return angle;
}
*/

float lowpass(float sample, float prev_lp) {
    float lp_factor = 0.005;
    float new_lp;
    new_lp = sample * lp_factor + prev_lp * (1 - lp_factor);
    return new_lp;
}


float highpass(float sample, float prev_sample, float prev_hp) {
    float hp_factor = 0.9;
    float new_hp;
    new_hp = prev_hp * hp_factor + (sample - prev_sample) * hp_factor;
    return new_hp;
}


float integrate(float sample, float prev_sum) {
    float ret;
    ret = prev_sum + sample;
    return ret;
}

/*
bool check_event_start(float* samples, int i) {
    sum_v = 0;
    sum_s = 0;
    for (int j=1; j <= ARR_LEN; j++){
        sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], sum_v);
        float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
        sum_s = integrate(effective_vel, sum_s);
    }
    sum_s = sum_s / 1000;
    if (abs(sum_s) > MOVEMENT_THRESHOLD) {
        return true;
    }
    return false;
}*/


/*bool check_event_end(float* samples, int i) {
    float temp_sum_v = 0, temp_sum_s = 0;
    for (uint8_t j=1; j <= ARR_LEN; j++){
        temp_sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], temp_sum_v);
        temp_sum_s = integrate(temp_sum_v, temp_sum_s);
    }
    
    if (temp_sum_s < STILL_THRESHOLD) {
        return false;
    }
    return true;
}*/

/* Simple main function */
int main()
{
    pc.baud(921600);
    timer.start();
    timeStamp_us = timer.read_us();
    int i = 0, j;
    uint8_t id;
    int32_t Acc_axes[3];
    int32_t Gyro_axes[3];
    //float diff, prev_lp = 0, prev_hp = 0;
    float acc_samp[3][ARR_LEN] = {}, acc_lp[3][ARR_LEN] = {};
    float gyro_samp[3][ARR_LEN] = {}, gyro_rad[3][ARR_LEN] = {}, gyro_hp[3][ARR_LEN] = {};
    float complementary[3][ARR_LEN] = {};
    //float pitch_calc[ARR_LEN], roll_calc[ARR_LEN];
    bool opened = false;
    
    // get init data and bias
    for(j=0; j < ARR_LEN; j++){
        accelerometer->get_x_axes(Acc_axes);
        gyroscope->get_g_axes(Gyro_axes);
        //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]);
        
        // get accelerometer starting data
        acc_samp[0][j % ARR_LEN] = Acc_axes[0];
        acc_samp[1][j % ARR_LEN] = Acc_axes[1];
        acc_samp[2][j % ARR_LEN] = Acc_axes[2];
        acc_bias += (acc_samp[1][j % ARR_LEN] / ARR_LEN);
        
        // get gyroscope starting data
        gyro_samp[0][j % ARR_LEN] = Gyro_axes[0] / GYRO_FACTOR;
        gyro_samp[1][j % ARR_LEN] = Gyro_axes[1] / GYRO_FACTOR;
        gyro_samp[2][j % ARR_LEN] = Gyro_axes[2] / GYRO_FACTOR;
        gyro_bias[0] += (gyro_samp[0][j % ARR_LEN] / ARR_LEN);
        gyro_bias[1] += (gyro_samp[1][j % ARR_LEN] / ARR_LEN);
        gyro_bias[2] += (gyro_samp[2][j % ARR_LEN] / ARR_LEN);
    }
    
    // get init lowpass on acc, integral + highpass on gyro and complementary
    for(j=0; j < ARR_LEN; j++){ 
        // get acc lowpass data
        acc_lp[0][j % ARR_LEN] = lowpass(acc_samp[0][j % ARR_LEN], acc_lp[0][(j-1) % ARR_LEN]);
        acc_lp[1][j % ARR_LEN] = lowpass(acc_samp[1][j % ARR_LEN], acc_lp[1][(j-1) % ARR_LEN]);
        acc_lp[2][j % ARR_LEN] = lowpass(acc_samp[2][j % ARR_LEN], acc_lp[2][(j-1) % ARR_LEN]);
        
        // get gyro highpass data
        gyro_rad[0][j % ARR_LEN] = integrate(gyro_samp[0][j % ARR_LEN] - gyro_bias[0], gyro_rad[0][(j-1) % ARR_LEN]);
        gyro_rad[1][j % ARR_LEN] = integrate(gyro_samp[1][j % ARR_LEN] - gyro_bias[1], gyro_rad[1][(j-1) % ARR_LEN]);
        gyro_rad[2][j % ARR_LEN] = integrate(gyro_samp[2][j % ARR_LEN] - gyro_bias[2], gyro_rad[2][(j-1) % ARR_LEN]);
        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]);
        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]);
        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]);
        
        // combine to complementary
        complementary[0][j % ARR_LEN] = acc_lp[0][j % ARR_LEN] + gyro_hp[0][j % ARR_LEN];
        complementary[1][j % ARR_LEN] = acc_lp[1][j % ARR_LEN] + gyro_hp[1][j % ARR_LEN];
        complementary[2][j % ARR_LEN] = acc_lp[2][j % ARR_LEN] + gyro_hp[2][j % ARR_LEN];
        
        // get pitch and roll
        //pitch_calc[j % ARR_LEN] = pitch(complementary[0][j % ARR_LEN], complementary[1][j % ARR_LEN], complementary[2][j % ARR_LEN]);
        //roll_calc[j % ARR_LEN] = roll(complementary[0][j % ARR_LEN], complementary[1][j % ARR_LEN], complementary[2][j % ARR_LEN]);
        //pc.printf("pitch: %f\t roll: %f\r\n", pitch_calc[j % ARR_LEN], roll_calc[j % ARR_LEN]);
    }
    
    pc.printf("\r\n--- Starting new run ---\r\n");

    gyroscope->read_id(&id);
    pc.printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
    pc.printf("bias for gyro: %f, %f, %f\r\n", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
    
    wait(0.5);

    while(1) {
        // get time stamp
        timeStamp_us = timer.read_us();
        // sample delay
        if ((timeStamp_us-sampleStamp)>SAMPLES_DELAY) {
            sampleStamp=timeStamp_us;
            // get raw data
            accelerometer->get_x_axes(Acc_axes);
            gyroscope->get_g_axes(Gyro_axes);
            //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]);
            
            // update acc data
            acc_samp[0][i % ARR_LEN] = Acc_axes[0];
            acc_samp[1][i % ARR_LEN] = Acc_axes[1];
            acc_samp[2][i % ARR_LEN] = Acc_axes[2];
            // update gyro data
            gyro_samp[0][i % ARR_LEN] = Gyro_axes[0] / GYRO_FACTOR;
            gyro_samp[1][i % ARR_LEN] = Gyro_axes[1] / GYRO_FACTOR;
            gyro_samp[2][i % ARR_LEN] = Gyro_axes[2] / GYRO_FACTOR;
            
            // get acc lowpass data
            acc_lp[0][i % ARR_LEN] = lowpass(acc_samp[0][i % ARR_LEN], acc_lp[0][(i-1) % ARR_LEN]);
            acc_lp[1][i % ARR_LEN] = lowpass(acc_samp[1][i % ARR_LEN], acc_lp[1][(i-1) % ARR_LEN]);
            acc_lp[2][i % ARR_LEN] = lowpass(acc_samp[2][i % ARR_LEN], acc_lp[2][(i-1) % ARR_LEN]);
            
            // get gyro highpass data
            gyro_rad[0][i % ARR_LEN] = integrate(gyro_samp[0][i % ARR_LEN] - gyro_bias[0], gyro_rad[0][(i-1) % ARR_LEN]);
            gyro_rad[1][i % ARR_LEN] = integrate(gyro_samp[1][i % ARR_LEN] - gyro_bias[1], gyro_rad[1][(i-1) % ARR_LEN]);
            gyro_rad[2][i % ARR_LEN] = integrate(gyro_samp[2][i % ARR_LEN] - gyro_bias[2], gyro_rad[2][(i-1) % ARR_LEN]);
            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]);
            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]);
            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]);
            
            //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]);
            
            
            // combine to complementary
            complementary[0][i % ARR_LEN] = acc_lp[0][i % ARR_LEN] + gyro_hp[0][i % ARR_LEN];
            complementary[1][i % ARR_LEN] = acc_lp[1][i % ARR_LEN] + gyro_hp[1][i % ARR_LEN];
            complementary[2][i % ARR_LEN] = acc_lp[2][i % ARR_LEN] + gyro_hp[2][i % ARR_LEN];
            
            //pc.printf("%f acc0, %f compl0\r\n" ,acc_lp[0][i % ARR_LEN], complementary[0][i % ARR_LEN]); // gryo_rad[0]
            
            // get pitch and roll
            //pitch_calc[i % ARR_LEN] = pitch(complementary[0][i % ARR_LEN], complementary[1][i % ARR_LEN], complementary[2][i % ARR_LEN]);
            //roll_calc[i % ARR_LEN] = roll(complementary[0][i % ARR_LEN], complementary[1][i % ARR_LEN], complementary[2][i % ARR_LEN]);
            
            //pc.printf("pitch: %f\t roll: %f\r\n", pitch_calc[i % ARR_LEN], roll_calc[i % ARR_LEN]);
            
            //diff = abs(samples[axis][i % ARR_LEN] - samples[axis][(i - (ARR_LEN / 2)) % ARR_LEN]);
            // 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);
            if (abs(complementary[0][i % ARR_LEN]) > EVENT_THRESHOLD) {
                if (abs(acc_samp[1][i % ARR_LEN] - acc_bias) > 35 ){
                    opened = true;
                    led1 =true;
                }
                else if (abs(acc_samp[1][i % ARR_LEN] - acc_bias) < 15){
                    opened = false;
                    led1 = false;
                }
            }
            /*if(opened){
                pc.printf("1, %f\r\n", abs(acc_samp[1][i % ARR_LEN] - acc_bias));
            }
            else {
                pc.printf("0, %f\r\n", abs(acc_samp[1][i % ARR_LEN] - acc_bias));
            }*/
            i++;
                /*//pc.printf("suspected event: ");
                event_flag = check_event_start(samples[axis], i);
                if(event_flag) {
                    pc.printf("OPENED: %f\r\n", sum_s);
                }
                else {
                    pc.printf("False sum_s: %f\r\n", sum_s);
                }
                //event_flag = false 
            }
            else if (event_flag){
                sum_v = integrate(samples[axis][i % ARR_LEN] - bias[axis], sum_v);
                //float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
                sum_v = abs(sum_v) < VEL_THRESHOLD ? 0 : sum_v;
                sum_s = integrate(sum_v, sum_s);
                pc.printf("sum_v: %f, sum_s: %f\r\n", sum_v, sum_s);
                if (abs(sum_s) <= CLOSE_THRESHOLD){
                    pc.printf("CLOSED: %f\r\n", sum_s);
                    event_flag = false;
                    sum_s = 0;
                    sum_v = 0;
                }
            }*/
        }// end get & send samples
    }// end loop
}// end main



//TODO list:
// compute event movement
// find event ending
// display 0,1