diff options
author | dragonprevost <dragon@dkp.io> | 2018-10-22 12:00:41 -0700 |
---|---|---|
committer | dragonprevost <dragon@dkp.io> | 2018-10-22 12:00:41 -0700 |
commit | 5292177fb0a7a2855d57b57a372df62202421348 (patch) | |
tree | 23eb7d2afa75e071590e17c8fac4620450905daa /motionMonitor | |
parent | 37ca52c840cf46d11a22da5ecbef435c23c63dfd (diff) |
Impacts are now stored in 3 arrays that log the acceleration at time of impact on each dimension.
Diffstat (limited to 'motionMonitor')
-rw-r--r-- | motionMonitor/motionMonitor.c | 107 |
1 files changed, 68 insertions, 39 deletions
diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index bcc77ac..2d2c24b 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -2,9 +2,26 @@ #include "legato.h" #include "util.h" #include <pthread.h> +#include <semaphore.h> + +#define MAX_ARRAY_SIZE 200 void *impactMonitor(void *); +typedef struct{ + double x; + double y; + double z; +} Acceleration; + + + +double xAccImpact [MAX_ARRAY_SIZE]; +double yAccImpact [MAX_ARRAY_SIZE]; +double zAccImpact [MAX_ARRAY_SIZE]; + + + static const char FormatStr[] = "/sys/devices/i2c-0/0-0068/iio:device0/in_%s_%s"; static const char AccType[] = "accel"; static const char GyroType[] = "anglvel"; @@ -12,22 +29,18 @@ static const char CompX[] = "x_raw"; static const char CompY[] = "y_raw"; static const char CompZ[] = "z_raw"; static const char CompScale[] = "scale"; -bool hasSuddenImpact = false; -static const int impactThreshold = 20; +int totalImpacts = 0; +static const double impactThreshold = 20.0; -typedef struct { - double x; - double y; - double z; -} Acceleration; +Acceleration suddenImpact = {0, 0, 0}; pthread_t impact_thread; +sem_t impact_mutex; -Acceleration suddenImpact = {0, 0, 0}; -/** +/* * Reports the x, y and z accelerometer readings in meters per second squared. - */ +*/ le_result_t brnkl_motion_getCurrentAcceleration( double *xAcc, @@ -37,10 +50,6 @@ le_result_t brnkl_motion_getCurrentAcceleration( { le_result_t r; char path[256]; - if(hasSuddenImpact){ - LE_INFO("hasSuddenImpact"); - hasSuddenImpact = false; - } double scaling = 0.0; int pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompScale); @@ -80,26 +89,40 @@ done: } /* -*return 1 if hardware has been in a sudden impact. +* Return 1 if hardware has been in a sudden impact. */ int8_t brnkl_motion_hasSuddenImpact() { - if(hasSuddenImpact){ - hasSuddenImpact = false; - return 1; - } return 0; } +//implement case for full array, or solution to not getting a full array +void recordImpact(double* xAcc, + double* yAcc, + double* zAcc){ + + //time = (unsigned long)time(NULL) + + xAccImpact[totalImpacts] = *xAcc; + yAccImpact[totalImpacts] = *yAcc; + zAccImpact[totalImpacts] = *zAcc; + + totalImpacts++; + LE_INFO("New Impact, totalImpacts: %d", totalImpacts); + + for(int i = 0; i < totalImpacts; i++){ + LE_INFO("ImpactOf X: %f Y: %f Z: %f ", xAccImpact[i], yAccImpact[i], zAccImpact[i]); + } +} + +/* +* Return array of sudden impacts(Acceleration) +*/ + le_result_t brnkl_motion_getSuddenImpact(double* xAcc, - double* yAcc, - double* zAcc) { - *xAcc = suddenImpact.x; - *yAcc = suddenImpact.y; - *zAcc = suddenImpact.z; - suddenImpact.x = 0; - suddenImpact.y = 0; - suddenImpact.z = 0; - hasSuddenImpact = false; + double* yAcc, + double* zAcc) { + //Return Array of Sudden Impacts + //*impactArray = impactArr; return LE_OK; } @@ -115,13 +138,18 @@ void *impactMonitor(void * ptr){ brnkl_motion_getCurrentAcceleration(&x, &y, &z); - if( - abs(x) + - abs(y) + - abs(z) > - impactThreshold - ) - hasSuddenImpact = true; + double euclidian = sqrt(x*x + y*y + z*z); + + if(euclidian > impactThreshold){ + LE_INFO("euclidian : %f", euclidian); + //3. add x, y, z to impact array + sem_wait(&impact_mutex); + LE_INFO("addingImpact"); + + recordImpact(&x, &y, &z); + + sem_post(&impact_mutex); + } usleep(100*1000); @@ -134,17 +162,18 @@ void *impactMonitor(void * ptr){ *Create thread to monitor accelerometer iio */ void initThread(){ - int thread; + int thread, mutx; thread = pthread_create( &impact_thread, NULL, impactMonitor, NULL); + mutx = sem_init(&impact_mutex, 0, 1); - if(thread){ + if(thread && mutx){ LE_INFO("Reader Thread Created"); }else{ - LE_ERROR("Reader Thread Creation Failed"); + LE_ERROR("Reader Thread or Mutex Creation Failed"); } } COMPONENT_INIT { initThread(); -} +}
\ No newline at end of file |