Fall Detection Algorithm based on ST LSM303

Dependencies:   LSM303DLHC

main.cpp

Committer:
tash1000
Date:
2018-08-23
Revision:
2:83d3f3f9a6fb
Parent:
1:474dc40b688a

File content as of revision 2:83d3f3f9a6fb:

#include "mbed.h"
#include "LSM303DLHC.h"


DigitalOut led1(LED1);
DigitalOut led2 (LED2);


float aax, aay, aaz, mmx, mmy, mmz, aaz_old;
bool retval;
EventQueue queue;
InterruptIn free_fall(P5_1); // Free Fall Interupt asserted on pin 5_1

LSM303DLHC sensor(P3_4, P3_5);  //Initialise LMS303

/** Function to calculate Impact Acceleration after a Free Fall Interrupt has been Asserted **/
void ff_proc()
{
double atotal;
double TH=4;
printf("Free Fall Interupt Asserted ..... \n");
retval=sensor.read(&aax, &aay, &aaz, &mmx, &mmy, &mmz);
atotal=sqrt(pow(aax,2)+pow(aay,2)+pow(aaz,2));
printf("Total Acceleration  %f\n", atotal);
if (atotal<TH) //Fall NOT verified
{
    led2=0;
    led1=1;
    }
    
}

/** Free Fall Interrupt Service **/
void ff()
{
// Potential Fall detected    
    led2=1;
    led1=0;
    
    queue.call(&ff_proc); //Calculate Impact Acceleration and any other thresholds to verify Free Fall Event
}    



//*******************************************************************************//

// main() 
    int main() {

int flag=1;
Thread eventThread;

   led1=1;  // LED1 OFF
   led2=1;  // LED2 OFF
   
 eventThread.start(callback(&queue, &EventQueue::dispatch_forever));  
    
    led2=0;
        printf("Initialising LSM303DLHC ..... \n");
        
        retval=sensor.read(&aax, &aay, &aaz, &mmx, &mmy, &mmz);
        if (retval==1)
        {
            printf("Success: LSM303DLHC Connected, Accel Values:  %3.3f %3.3f %3.3f\n", aax, aay, aaz);
                        
         }
         else {
           printf("Failure : LSM303DLHC NOT RESPONDING................... \n");
           flag=0;
           }     
        
if (flag) {
             
free_fall.fall(&ff);              

while(1)
{    
retval=sensor.read(&aax, &aay, &aaz, &mmx, &mmy, &mmz);
printf("Accel Values:  %3.3f %3.3f %3.3f\n", aax, aay, aaz);   
wait(5);
}

}
}