Tasos Ch
/
Fall_Detect
Fall Detection Algorithm based on ST LSM303
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?
User | Revision | Line number | New 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 | } |