Tasos Ch / Mbed OS Fall_Detect

Fall Detection Algorithm based on ST LSM303

Dependencies:   LSM303DLHC

main.cpp@2:83d3f3f9a6fb, 2018-08-23 (annotated)

Committer:
tash1000
Date:
Thu Aug 23 18:50:41 2018 +0000
Revision:
2:83d3f3f9a6fb
Parent:
1:474dc40b688a 
V 1.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tash1000 0:c355c42bd6de 1 #include "mbed.h"
tash1000 0:c355c42bd6de 2 #include "LSM303DLHC.h"
tash1000 0:c355c42bd6de 3
tash1000 0:c355c42bd6de 4
tash1000 0:c355c42bd6de 5 DigitalOut led1(LED1);
tash1000 0:c355c42bd6de 6 DigitalOut led2 (LED2);
tash1000 0:c355c42bd6de 7
tash1000 0:c355c42bd6de 8
tash1000 0:c355c42bd6de 9 float aax, aay, aaz, mmx, mmy, mmz, aaz_old;
tash1000 0:c355c42bd6de 10 bool retval;
tash1000 0:c355c42bd6de 11 EventQueue queue;
tash1000 0:c355c42bd6de 12 InterruptIn free_fall(P5_1); // Free Fall Interupt asserted on pin 5_1
tash1000 0:c355c42bd6de 13
tash1000 0:c355c42bd6de 14 LSM303DLHC sensor(P3_4, P3_5); //Initialise LMS303
tash1000 0:c355c42bd6de 15
tash1000 0:c355c42bd6de 16 /** Function to calculate Impact Acceleration after a Free Fall Interrupt has been Asserted **/
tash1000 0:c355c42bd6de 17 void ff_proc()
tash1000 0:c355c42bd6de 18 {
tash1000 0:c355c42bd6de 19 double atotal;
tash1000 0:c355c42bd6de 20 double TH=4;
tash1000 0:c355c42bd6de 21 printf("Free Fall Interupt Asserted ..... \n");
tash1000 0:c355c42bd6de 22 retval=sensor.read(&aax, &aay, &aaz, &mmx, &mmy, &mmz);
tash1000 0:c355c42bd6de 23 atotal=sqrt(pow(aax,2)+pow(aay,2)+pow(aaz,2));
tash1000 0:c355c42bd6de 24 printf("Total Acceleration %f\n", atotal);
tash1000 0:c355c42bd6de 25 if (atotal<TH) //Fall NOT verified
tash1000 0:c355c42bd6de 26 {
tash1000 0:c355c42bd6de 27 led2=0;
tash1000 0:c355c42bd6de 28 led1=1;
tash1000 0:c355c42bd6de 29 }
tash1000 0:c355c42bd6de 30
tash1000 0:c355c42bd6de 31 }
tash1000 0:c355c42bd6de 32
tash1000 0:c355c42bd6de 33 /** Free Fall Interrupt Service **/
tash1000 0:c355c42bd6de 34 void ff()
tash1000 0:c355c42bd6de 35 {
tash1000 0:c355c42bd6de 36 // Potential Fall detected
tash1000 0:c355c42bd6de 37 led2=1;
tash1000 0:c355c42bd6de 38 led1=0;
tash1000 0:c355c42bd6de 39
tash1000 0:c355c42bd6de 40 queue.call(&ff_proc); //Calculate Impact Acceleration and any other thresholds to verify Free Fall Event
tash1000 0:c355c42bd6de 41 }
tash1000 0:c355c42bd6de 42
tash1000 0:c355c42bd6de 43
tash1000 0:c355c42bd6de 44
tash1000 0:c355c42bd6de 45 //*******************************************************************************//
tash1000 0:c355c42bd6de 46
tash1000 0:c355c42bd6de 47 // main()
tash1000 0:c355c42bd6de 48 int main() {
tash1000 0:c355c42bd6de 49
tash1000 0:c355c42bd6de 50 int flag=1;
tash1000 0:c355c42bd6de 51 Thread eventThread;
tash1000 0:c355c42bd6de 52
tash1000 0:c355c42bd6de 53 led1=1; // LED1 OFF
tash1000 0:c355c42bd6de 54 led2=1; // LED2 OFF
tash1000 0:c355c42bd6de 55
tash1000 0:c355c42bd6de 56 eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
tash1000 0:c355c42bd6de 57
tash1000 0:c355c42bd6de 58 led2=0;
tash1000 0:c355c42bd6de 59 printf("Initialising LSM303DLHC ..... \n");
tash1000 0:c355c42bd6de 60
tash1000 0:c355c42bd6de 61 retval=sensor.read(&aax, &aay, &aaz, &mmx, &mmy, &mmz);
tash1000 0:c355c42bd6de 62 if (retval==1)
tash1000 0:c355c42bd6de 63 {
tash1000 1:474dc40b688a 64 printf("Success: LSM303DLHC Connected, Accel Values: %3.3f %3.3f %3.3f\n", aax, aay, aaz);
tash1000 1:474dc40b688a 65
tash1000 0:c355c42bd6de 66 }
tash1000 0:c355c42bd6de 67 else {
tash1000 0:c355c42bd6de 68 printf("Failure : LSM303DLHC NOT RESPONDING................... \n");
tash1000 0:c355c42bd6de 69 flag=0;
tash1000 0:c355c42bd6de 70 }
tash1000 0:c355c42bd6de 71
tash1000 0:c355c42bd6de 72 if (flag) {
tash1000 0:c355c42bd6de 73
tash1000 0:c355c42bd6de 74 free_fall.fall(&ff);
tash1000 0:c355c42bd6de 75
tash1000 0:c355c42bd6de 76 while(1)
tash1000 1:474dc40b688a 77 {
tash1000 1:474dc40b688a 78 retval=sensor.read(&aax, &aay, &aaz, &mmx, &mmy, &mmz);
tash1000 1:474dc40b688a 79 printf("Accel Values: %3.3f %3.3f %3.3f\n", aax, aay, aaz);
tash1000 1:474dc40b688a 80 wait(5);
tash1000 0:c355c42bd6de 81 }
tash1000 0:c355c42bd6de 82
tash1000 0:c355c42bd6de 83 }
tash1000 0:c355c42bd6de 84 }