Fall Detection Algorithm based on ST LSM303
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);
}
}
}