File content as of revision 13:f4bb537c9440:

/**
 ******************************************************************************
 * @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 SLIDING WINDOW *******************
***************************************************************/
/* Includes */
#include "mbed.h"
#include "x_nucleo_iks01a1.h"

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

// thresholds
#define EVENT_THRESHOLD 5.f //5
#define MOVEMENT_THRESHOLD 5.f
#define VEL_THRESHOLD 10.f
#define LOC_THRESHOLD 1.f
#define STILL_THRESHOLD 2.f
#define OPEN_THRESHOLD 5.f
#define CLOSE_THRESHOLD 3.f

/* 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 bias[3];
int axis = 0;

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


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;
    float effective_s;
    for (int j=1; j <= ARR_LEN; j++){
        sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], sum_v);
        pc.printf("sum_vvvvv: %f, \r\n", sum_v);
        float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
        pc.printf("effective_vvvvv: %f, \r\n", effective_vel);
        sum_s = integrate(effective_vel, sum_s);
        effective_s = abs(sum_s) >= LOC_THRESHOLD ? sum_s : 0; 
        //effective_s = sum_s;
        
    }
    effective_s = effective_s / 1000;
    pc.printf("effective_LOC: %f, \r\n", effective_s);
    if (abs(effective_s) > MOVEMENT_THRESHOLD) {
       // pc.printf("effective_LOC: %f, \r\n", effective_s);
        // (1){}
        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;
    float samples[3][ARR_LEN];
    bool event_flag = false;
    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]);
        samples[0][j % ARR_LEN] = Acc_axes[0] / MG2MM;
        samples[1][j % ARR_LEN] = Acc_axes[1] / MG2MM;
        samples[2][j % ARR_LEN] = Acc_axes[2] / MG2MM;
        bias[0] += (samples[0][j % ARR_LEN] / ARR_LEN);
        bias[1] += (samples[1][j % ARR_LEN] / ARR_LEN);
        bias[2] += (samples[2][j % ARR_LEN] / 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: %f, %f, %f\r\n", bias[0], bias[1], 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]);
            samples[0][i % ARR_LEN] = Acc_axes[0] / MG2MM;
            samples[1][i % ARR_LEN] = Acc_axes[1] / MG2MM;
            samples[2][i % ARR_LEN] = Acc_axes[2] / MG2MM;
            
            diff = abs(samples[axis][i % ARR_LEN] - samples[axis][(i - (ARR_LEN / 2)) % ARR_LEN]);
            //pc.printf("diff: %f\r\n", diff);
            if (diff > EVENT_THRESHOLD && !event_flag) {
                //pc.printf("suspected event: ");
                event_flag = check_event_start(samples[axis], i);
                if(event_flag) {
                    pc.printf("OPENED: sumv= %f   sums=%f\r\n", sum_v,sum_s/1000);
                    //while (1){}
                }
                /*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);
               // pc.printf("sum_v: %f, \r\n", sum_v);
                float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
                //sum_v = abs(sum_v) < VEL_THRESHOLD ? 0 : sum_v;
                pc.printf("effective_v: %f, \r\n", effective_vel);
                sum_s = integrate(effective_vel, sum_s);
                float effective_s = abs(sum_s) >= LOC_THRESHOLD ? sum_s : 0; 
                //effective_s = sum_s;
                pc.printf("sum_v: %f, sum_s: %f\r\n", sum_v, sum_s);
                effective_s = effective_s /1000;
                if (abs(effective_s) <= CLOSE_THRESHOLD){
                    pc.printf("CLOSED: sumv = %f     sums= %f   \r\n", sum_v,effective_s);
                    event_flag = false;
                    sum_s = 0;
                    sum_v = 0;
                }
            }
            i++;
        }// end get & send samples
    }// end loop
}// end main



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