From 37ca52c840cf46d11a22da5ecbef435c23c63dfd Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Mon, 15 Oct 2018 16:10:53 -0700 Subject: Implemented thread to monitor accelerometer on short intervals in order to detect impacts. Note: interrupts are not being used --- motionMonitor/motionMonitor.c | 172 +++++++++++++++++++++++++----------------- 1 file changed, 103 insertions(+), 69 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 9028653..bcc77ac 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -1,15 +1,19 @@ #include "interfaces.h" #include "legato.h" #include "util.h" +#include -static const char FormatStr[] = - "/sys/bus/i2c/devices/3-0068/iio:device0/in_%s_%s"; -static const char AccType[] = "accel"; -static const char GyroType[] = "anglvel"; -static const char CompX[] = "x_raw"; -static const char CompY[] = "y_raw"; -static const char CompZ[] = "z_raw"; +void *impactMonitor(void *); + +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"; +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; typedef struct { double x; @@ -17,50 +21,73 @@ typedef struct { double z; } Acceleration; -bool hasSuddenImpact = false; +pthread_t impact_thread; + 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, - double* yAcc, - double* zAcc) { - le_result_t r; - char path[256]; - - double scaling = 0.0; - int pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompScale); - LE_ASSERT(pathLen < sizeof(path)); - r = ioutil_readDoubleFromFile(path, &scaling); - if (r != LE_OK) { - goto done; - } - pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompX); - LE_ASSERT(pathLen < sizeof(path)); - r = ioutil_readDoubleFromFile(path, xAcc); - if (r != LE_OK) { - goto done; - } - *xAcc *= scaling; +le_result_t brnkl_motion_getCurrentAcceleration( + double *xAcc, + double *yAcc, + double *zAcc +) +{ + le_result_t r; + char path[256]; + if(hasSuddenImpact){ + LE_INFO("hasSuddenImpact"); + hasSuddenImpact = false; + } - pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompY); - LE_ASSERT(pathLen < sizeof(path)); - r = ioutil_readDoubleFromFile(path, yAcc); - if (r != LE_OK) { - goto done; - } - *yAcc *= scaling; + double scaling = 0.0; + int pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompScale); + LE_ASSERT(pathLen < sizeof(path)); + r = ioutil_readDoubleFromFile(path, &scaling); + if (r != LE_OK) + { + goto done; + } - pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompZ); - LE_ASSERT(pathLen < sizeof(path)); - r = ioutil_readDoubleFromFile(path, zAcc); - *zAcc *= scaling; + pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompX); + LE_ASSERT(pathLen < sizeof(path)); + r = ioutil_readDoubleFromFile(path, xAcc); + if (r != LE_OK) + { + goto done; + } + *xAcc *= scaling; + + pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompY); + LE_ASSERT(pathLen < sizeof(path)); + r = ioutil_readDoubleFromFile(path, yAcc); + if (r != LE_OK) + { + goto done; + } + *yAcc *= scaling; + + pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompZ); + LE_ASSERT(pathLen < sizeof(path)); + r = ioutil_readDoubleFromFile(path, zAcc); + *zAcc *= scaling; done: - LE_INFO("Showing accel: x: %f, y: %f, z: %f", *xAcc, *yAcc, *zAcc); - return r; + LE_INFO("Showing accel X: %f Y: %f Z: %f ", *xAcc, *yAcc, *zAcc); + return r; +} + +/* +*return 1 if hardware has been in a sudden impact. +*/ +int8_t brnkl_motion_hasSuddenImpact() { + if(hasSuddenImpact){ + hasSuddenImpact = false; + return 1; + } + return 0; } le_result_t brnkl_motion_getSuddenImpact(double* xAcc, @@ -76,41 +103,48 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, return LE_OK; } -int8_t brnkl_motion_hasSuddenImpact() { - return hasSuddenImpact; -} +/* +*Monitors accelerometer from iio on 100ms intervals +*Sets hasSuddenImpact flag when accelerometer surpasses threshold +*/ +void *impactMonitor(void * ptr){ -// take a reading but make sure its "larger" than -// whatever is already in the buffer -void interruptChangeHandler(bool state, void* ctx) { double x, y, z; - le_result_t r = brnkl_motion_getCurrentAcceleration(&x, &y, &z); - LE_INFO("TRIGGERED x:%f y:%f z:%f", x, y, z); - if (r == LE_OK) { - bool updateX = fabs(x) > fabs(suddenImpact.x); - bool updateY = fabs(y) > fabs(suddenImpact.y); - bool updateZ = fabs(z) > fabs(suddenImpact.z); - if (updateX) { - suddenImpact.x = x; - } - if (updateY) { - suddenImpact.y = y; - } - if (updateZ) { - suddenImpact.z = z; - } - if (updateX || updateY || updateZ) { + + for(;;){ + + brnkl_motion_getCurrentAcceleration(&x, &y, &z); + + if( + abs(x) + + abs(y) + + abs(z) > + impactThreshold + ) hasSuddenImpact = true; - } + + + usleep(100*1000); + } + return ptr; } -void initGpio() { - interrupt_SetInput(INTERRUPT_ACTIVE_HIGH); - interrupt_AddChangeEventHandler(INTERRUPT_EDGE_RISING, interruptChangeHandler, - NULL, 0); +/* +*Create thread to monitor accelerometer iio +*/ +void initThread(){ + int thread; + + thread = pthread_create( &impact_thread, NULL, impactMonitor, NULL); + + if(thread){ + LE_INFO("Reader Thread Created"); + }else{ + LE_ERROR("Reader Thread Creation Failed"); + } } COMPONENT_INIT { - initGpio(); + initThread(); } -- cgit v1.2.3 From 5292177fb0a7a2855d57b57a372df62202421348 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Mon, 22 Oct 2018 12:00:41 -0700 Subject: Impacts are now stored in 3 arrays that log the acceleration at time of impact on each dimension. --- motionMonitor/motionMonitor.c | 107 +++++++++++++++++++++++++++--------------- 1 file changed, 68 insertions(+), 39 deletions(-) (limited to 'motionMonitor') 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 +#include + +#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 -- cgit v1.2.3 From c5d0a802c38dbc31a784afbd8f10b919c78c28b4 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Mon, 22 Oct 2018 14:22:02 -0700 Subject: getSuddenImpact is designed to return 3 arrays that hold impact data --- motionMonitor/motionMonitor.c | 37 +++++++++++++++++++++---------------- 1 file changed, 21 insertions(+), 16 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 2d2c24b..cc05a0c 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -4,7 +4,7 @@ #include #include -#define MAX_ARRAY_SIZE 200 +#define N_CHANGE_BLOCKS 200 void *impactMonitor(void *); @@ -16,9 +16,9 @@ typedef struct{ -double xAccImpact [MAX_ARRAY_SIZE]; -double yAccImpact [MAX_ARRAY_SIZE]; -double zAccImpact [MAX_ARRAY_SIZE]; +double xAccImpact [N_CHANGE_BLOCKS]; +double yAccImpact [N_CHANGE_BLOCKS]; +double zAccImpact [N_CHANGE_BLOCKS]; @@ -88,13 +88,6 @@ done: return r; } -/* -* Return 1 if hardware has been in a sudden impact. -*/ -int8_t brnkl_motion_hasSuddenImpact() { - return 0; -} - //implement case for full array, or solution to not getting a full array void recordImpact(double* xAcc, double* yAcc, @@ -118,11 +111,23 @@ void recordImpact(double* xAcc, * Return array of sudden impacts(Acceleration) */ -le_result_t brnkl_motion_getSuddenImpact(double* xAcc, - double* yAcc, - double* zAcc) { - //Return Array of Sudden Impacts - //*impactArray = impactArr; +le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, + double* yAcc, size_t *ySize, + double* zAcc, size_t *zSize) { + + if(!totalImpacts) + LE_INFO("No Sudden Impacts to Report"); + + *xSize = *ySize = *zSize = totalImpacts; + + xAcc = xAccImpact; + yAcc = yAccImpact; + zAcc = zAccImpact; + + //Flush Arrays + + totalImpacts = 0; + return LE_OK; } -- cgit v1.2.3 From fe5acb3679efa12cce6bea126cc642037d005ef3 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Mon, 22 Oct 2018 17:35:18 -0700 Subject: Implented array that holds timestamp of all sudden impacts that are recorded. --- motionMonitor/motionMonitor.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index cc05a0c..0f2771a 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -19,6 +19,7 @@ typedef struct{ double xAccImpact [N_CHANGE_BLOCKS]; double yAccImpact [N_CHANGE_BLOCKS]; double zAccImpact [N_CHANGE_BLOCKS]; +uint64_t timestamps [N_CHANGE_BLOCKS]; @@ -94,17 +95,13 @@ void recordImpact(double* xAcc, double* zAcc){ //time = (unsigned long)time(NULL) - + timestamps[totalImpacts] = (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]); - } } /* @@ -124,8 +121,6 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, yAcc = yAccImpact; zAcc = zAccImpact; - //Flush Arrays - totalImpacts = 0; return LE_OK; -- cgit v1.2.3 From ee4498a0e5278526fef99776e06ad77cccbbbeb4 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Mon, 22 Oct 2018 17:58:18 -0700 Subject: slight clean up to increase readability. --- motionMonitor/motionMonitor.c | 25 ++----------------------- 1 file changed, 2 insertions(+), 23 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 0f2771a..0e2fbe5 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -14,15 +14,11 @@ typedef struct{ double z; } Acceleration; - - double xAccImpact [N_CHANGE_BLOCKS]; double yAccImpact [N_CHANGE_BLOCKS]; double zAccImpact [N_CHANGE_BLOCKS]; uint64_t timestamps [N_CHANGE_BLOCKS]; - - 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"; @@ -89,25 +85,16 @@ done: return r; } -//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) +void recordImpact(double* xAcc, double* yAcc, double* zAcc){ timestamps[totalImpacts] = (unsigned long)time(NULL); xAccImpact[totalImpacts] = *xAcc; yAccImpact[totalImpacts] = *yAcc; zAccImpact[totalImpacts] = *zAcc; - totalImpacts++; + LE_INFO("New Impact, totalImpacts: %d", totalImpacts); } -/* -* Return array of sudden impacts(Acceleration) -*/ - le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, double* yAcc, size_t *ySize, double* zAcc, size_t *zSize) { @@ -131,11 +118,8 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, *Sets hasSuddenImpact flag when accelerometer surpasses threshold */ void *impactMonitor(void * ptr){ - double x, y, z; - for(;;){ - brnkl_motion_getCurrentAcceleration(&x, &y, &z); double euclidian = sqrt(x*x + y*y + z*z); @@ -144,14 +128,9 @@ void *impactMonitor(void * ptr){ 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); } -- cgit v1.2.3 From 69f496ba08ff63567a5707fb9ffa2a42b6e3b289 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Mon, 22 Oct 2018 18:14:10 -0700 Subject: fixed camelCase variable names --- motionMonitor/motionMonitor.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 0e2fbe5..d71d91b 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -31,8 +31,8 @@ static const double impactThreshold = 20.0; Acceleration suddenImpact = {0, 0, 0}; -pthread_t impact_thread; -sem_t impact_mutex; +pthread_t impactThread; +sem_t impactMutex; /* @@ -127,9 +127,9 @@ void *impactMonitor(void * ptr){ if(euclidian > impactThreshold){ LE_INFO("euclidian : %f", euclidian); //3. add x, y, z to impact array - sem_wait(&impact_mutex); + sem_wait(&impactMutex); recordImpact(&x, &y, &z); - sem_post(&impact_mutex); + sem_post(&impactMutex); } usleep(100*1000); @@ -143,8 +143,8 @@ void *impactMonitor(void * ptr){ void initThread(){ int thread, mutx; - thread = pthread_create( &impact_thread, NULL, impactMonitor, NULL); - mutx = sem_init(&impact_mutex, 0, 1); + thread = pthread_create( &impactThread, NULL, impactMonitor, NULL); + mutx = sem_init(&impactMutex, 0, 1); if(thread && mutx){ LE_INFO("Reader Thread Created"); -- cgit v1.2.3 From 7e731316021119dd708cd45f158df90007542636 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Tue, 23 Oct 2018 12:31:18 -0700 Subject: BROKEN: Swtiching from semaphores to pthread_mutex_t --- motionMonitor/motionMonitor.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index d71d91b..04ae52d 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -2,7 +2,6 @@ #include "legato.h" #include "util.h" #include -#include #define N_CHANGE_BLOCKS 200 @@ -32,7 +31,7 @@ static const double impactThreshold = 20.0; Acceleration suddenImpact = {0, 0, 0}; pthread_t impactThread; -sem_t impactMutex; +pthread_mutex_t impactMutex; /* @@ -127,9 +126,9 @@ void *impactMonitor(void * ptr){ if(euclidian > impactThreshold){ LE_INFO("euclidian : %f", euclidian); //3. add x, y, z to impact array - sem_wait(&impactMutex); + pthread_mutex_lock(&impactMutex); recordImpact(&x, &y, &z); - sem_post(&impactMutex); + pthread_mutex_unlock(&impactMutex); } usleep(100*1000); @@ -142,10 +141,10 @@ void *impactMonitor(void * ptr){ */ void initThread(){ int thread, mutx; - + LE_INFO("initThread called"); + mutx = pthread_mutex_init(&impactMutex, NULL); thread = pthread_create( &impactThread, NULL, impactMonitor, NULL); - mutx = sem_init(&impactMutex, 0, 1); - + LE_INFO("mutexResult: %d", mutx); if(thread && mutx){ LE_INFO("Reader Thread Created"); }else{ -- cgit v1.2.3 From b6a15bc57c5f2586f747de646dd9deaa4a6e5bf2 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Tue, 23 Oct 2018 13:19:12 -0700 Subject: Fixed changes stated in previous pull request, semaphore -> mutex, LE_OK check in getSuddenImpact, variable rename, removal of LE_ASSERT, thread && mutex init check --- motionMonitor/motionMonitor.c | 39 ++++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 19 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 04ae52d..092d4a3 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -48,16 +48,12 @@ le_result_t brnkl_motion_getCurrentAcceleration( char path[256]; double scaling = 0.0; - int pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompScale); - LE_ASSERT(pathLen < sizeof(path)); r = ioutil_readDoubleFromFile(path, &scaling); if (r != LE_OK) { goto done; } - pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompX); - LE_ASSERT(pathLen < sizeof(path)); r = ioutil_readDoubleFromFile(path, xAcc); if (r != LE_OK) { @@ -65,8 +61,6 @@ le_result_t brnkl_motion_getCurrentAcceleration( } *xAcc *= scaling; - pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompY); - LE_ASSERT(pathLen < sizeof(path)); r = ioutil_readDoubleFromFile(path, yAcc); if (r != LE_OK) { @@ -74,8 +68,6 @@ le_result_t brnkl_motion_getCurrentAcceleration( } *yAcc *= scaling; - pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompZ); - LE_ASSERT(pathLen < sizeof(path)); r = ioutil_readDoubleFromFile(path, zAcc); *zAcc *= scaling; @@ -85,7 +77,7 @@ done: } void recordImpact(double* xAcc, double* yAcc, double* zAcc){ - timestamps[totalImpacts] = (unsigned long)time(NULL); + timestamps[totalImpacts] = GetCurrentTimestamp(); xAccImpact[totalImpacts] = *xAcc; yAccImpact[totalImpacts] = *yAcc; zAccImpact[totalImpacts] = *zAcc; @@ -101,12 +93,22 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, if(!totalImpacts) LE_INFO("No Sudden Impacts to Report"); + //check + if( + totalImpacts > sizeof(xAccImpact) || + totalImpacts > sizeof(yAccImpact) || + totalImpacts > sizeof(zAccImpact) + ) + return LE_FORMAT_ERROR; + *xSize = *ySize = *zSize = totalImpacts; - xAcc = xAccImpact; - yAcc = yAccImpact; - zAcc = zAccImpact; - + for(int i = 0; i < totalImpacts; i++){ + xAcc[i] = xAccImpact[i]; + yAcc[i] = yAccImpact[i]; + zAcc[i] = zAccImpact[i]; + } + totalImpacts = 0; return LE_OK; @@ -121,10 +123,9 @@ void *impactMonitor(void * ptr){ for(;;){ brnkl_motion_getCurrentAcceleration(&x, &y, &z); - double euclidian = sqrt(x*x + y*y + z*z); + double impactMagnitude = sqrt(x*x + y*y + z*z); - if(euclidian > impactThreshold){ - LE_INFO("euclidian : %f", euclidian); + if(impactMagnitude > impactThreshold){ //3. add x, y, z to impact array pthread_mutex_lock(&impactMutex); recordImpact(&x, &y, &z); @@ -145,10 +146,10 @@ void initThread(){ mutx = pthread_mutex_init(&impactMutex, NULL); thread = pthread_create( &impactThread, NULL, impactMonitor, NULL); LE_INFO("mutexResult: %d", mutx); - if(thread && mutx){ - LE_INFO("Reader Thread Created"); - }else{ + if(thread || mutx){ LE_ERROR("Reader Thread or Mutex Creation Failed"); + }else{ + LE_INFO("Reader Thread Created"); } } -- cgit v1.2.3 From 2266c66716429ce6012577609ae601f8a8101308 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Tue, 23 Oct 2018 13:26:58 -0700 Subject: Changed LE Return value in get sudden impact LE_FORMAT_ERROR -> LE_OUT_OF_RANGE --- motionMonitor/motionMonitor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 092d4a3..9344c35 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -99,7 +99,7 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, totalImpacts > sizeof(yAccImpact) || totalImpacts > sizeof(zAccImpact) ) - return LE_FORMAT_ERROR; + return LE_OUT_OF_RANGE; *xSize = *ySize = *zSize = totalImpacts; -- cgit v1.2.3 From 7dbefb9120799d9530be69072175ad4a4ac16ed3 Mon Sep 17 00:00:00 2001 From: dragonprevost Date: Tue, 23 Oct 2018 13:38:54 -0700 Subject: Added check for array length when returning impact arrays. --- motionMonitor/motionMonitor.c | 36 +++++++++++++++++++++++++----------- 1 file changed, 25 insertions(+), 11 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 9344c35..59f26e6 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -76,14 +76,22 @@ done: return r; } -void recordImpact(double* xAcc, double* yAcc, double* zAcc){ +le_result_t recordImpact(double* xAcc, double* yAcc, double* zAcc){ + if( + totalImpacts > sizeof(xAccImpact) || + totalImpacts > sizeof(yAccImpact) || + totalImpacts > sizeof(zAccImpact) + ) + return LE_OUT_OF_RANGE; + timestamps[totalImpacts] = GetCurrentTimestamp(); xAccImpact[totalImpacts] = *xAcc; yAccImpact[totalImpacts] = *yAcc; zAccImpact[totalImpacts] = *zAcc; totalImpacts++; - LE_INFO("New Impact, totalImpacts: %d", totalImpacts); + + return LE_OK; } le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, @@ -94,14 +102,13 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, LE_INFO("No Sudden Impacts to Report"); //check - if( - totalImpacts > sizeof(xAccImpact) || - totalImpacts > sizeof(yAccImpact) || - totalImpacts > sizeof(zAccImpact) - ) - return LE_OUT_OF_RANGE; - - *xSize = *ySize = *zSize = totalImpacts; + + if( + totalImpacts < *xSize || + totalImpacts < *ySize || + totalImpacts < *zSize + ) + return LE_OUT_OF_RANGE; for(int i = 0; i < totalImpacts; i++){ xAcc[i] = xAccImpact[i]; @@ -109,6 +116,9 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, zAcc[i] = zAccImpact[i]; } + *xSize = *ySize = *zSize = totalImpacts; + + totalImpacts = 0; return LE_OK; @@ -120,6 +130,7 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, */ void *impactMonitor(void * ptr){ double x, y, z; + le_result_t r = LE_OK; for(;;){ brnkl_motion_getCurrentAcceleration(&x, &y, &z); @@ -128,9 +139,12 @@ void *impactMonitor(void * ptr){ if(impactMagnitude > impactThreshold){ //3. add x, y, z to impact array pthread_mutex_lock(&impactMutex); - recordImpact(&x, &y, &z); + r = recordImpact(&x, &y, &z); pthread_mutex_unlock(&impactMutex); } + if(r != LE_OK) + LE_ERROR("Impact Not Recorded"); + usleep(100*1000); } -- cgit v1.2.3 From 1c287552b5a30999068d4aeb5ed83f1deed91210 Mon Sep 17 00:00:00 2001 From: Nick Van Doorn Date: Sun, 21 Oct 2018 16:14:09 -0700 Subject: Factor out sample period --- motionMonitor/motionMonitor.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 59f26e6..f2027f9 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -4,6 +4,7 @@ #include #define N_CHANGE_BLOCKS 200 +#define SAMPLE_PERIOD_MS 100 void *impactMonitor(void *); @@ -145,7 +146,7 @@ void *impactMonitor(void * ptr){ if(r != LE_OK) LE_ERROR("Impact Not Recorded"); - usleep(100*1000); + usleep(SAMPLE_PERIOD_MS*1000); } return ptr; -- cgit v1.2.3 From 4ed74f2d45acf41416e69a2cc7df7cfb1e87c67d Mon Sep 17 00:00:00 2001 From: Nick Van Doorn Date: Sun, 21 Oct 2018 16:14:30 -0700 Subject: Fix size comparison --- motionMonitor/motionMonitor.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index f2027f9..b7844ea 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -79,9 +79,9 @@ done: le_result_t recordImpact(double* xAcc, double* yAcc, double* zAcc){ if( - totalImpacts > sizeof(xAccImpact) || - totalImpacts > sizeof(yAccImpact) || - totalImpacts > sizeof(zAccImpact) + totalImpacts > N_CHANGE_BLOCKS || + totalImpacts > N_CHANGE_BLOCKS || + totalImpacts > N_CHANGE_BLOCKS ) return LE_OUT_OF_RANGE; -- cgit v1.2.3 From 8e8dbfd58038cc7c83cf163f03a6e6b69eb34ad1 Mon Sep 17 00:00:00 2001 From: Nick Van Doorn Date: Sun, 21 Oct 2018 16:15:26 -0700 Subject: Lock shared resources whewn reading --- motionMonitor/motionMonitor.c | 38 +++++++++++++++++++++----------------- 1 file changed, 21 insertions(+), 17 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index b7844ea..a4736e2 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -101,26 +101,30 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, if(!totalImpacts) LE_INFO("No Sudden Impacts to Report"); + else { + pthread_mutex_lock(&impactMutex); + //check + + if( + totalImpacts < *xSize || + totalImpacts < *ySize || + totalImpacts < *zSize + ) + return LE_OUT_OF_RANGE; + + for(int i = 0; i < totalImpacts; i++){ + xAcc[i] = xAccImpact[i]; + yAcc[i] = yAccImpact[i]; + zAcc[i] = zAccImpact[i]; + } + + *xSize = *ySize = *zSize = totalImpacts; - //check - if( - totalImpacts < *xSize || - totalImpacts < *ySize || - totalImpacts < *zSize - ) - return LE_OUT_OF_RANGE; + totalImpacts = 0; - for(int i = 0; i < totalImpacts; i++){ - xAcc[i] = xAccImpact[i]; - yAcc[i] = yAccImpact[i]; - zAcc[i] = zAccImpact[i]; + pthread_mutex_unlock(&impactMutex); } - - *xSize = *ySize = *zSize = totalImpacts; - - - totalImpacts = 0; return LE_OK; } @@ -170,4 +174,4 @@ void initThread(){ COMPONENT_INIT { initThread(); -} \ No newline at end of file +} -- cgit v1.2.3 From b960ed187644a9d4bac17a2b66bc5179002064fa Mon Sep 17 00:00:00 2001 From: Nick Van Doorn Date: Sun, 21 Oct 2018 16:17:15 -0700 Subject: Formatter --- motionMonitor/motionMonitor.c | 149 +++++++++++++++++++----------------------- 1 file changed, 68 insertions(+), 81 deletions(-) (limited to 'motionMonitor') diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index a4736e2..0af8728 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -6,27 +6,28 @@ #define N_CHANGE_BLOCKS 200 #define SAMPLE_PERIOD_MS 100 -void *impactMonitor(void *); +void* impactMonitor(void*); -typedef struct{ +typedef struct { double x; double y; double z; } Acceleration; -double xAccImpact [N_CHANGE_BLOCKS]; -double yAccImpact [N_CHANGE_BLOCKS]; -double zAccImpact [N_CHANGE_BLOCKS]; -uint64_t timestamps [N_CHANGE_BLOCKS]; - -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"; -static const char CompX[] = "x_raw"; -static const char CompY[] = "y_raw"; -static const char CompZ[] = "z_raw"; +double xAccImpact[N_CHANGE_BLOCKS]; +double yAccImpact[N_CHANGE_BLOCKS]; +double zAccImpact[N_CHANGE_BLOCKS]; +uint64_t timestamps[N_CHANGE_BLOCKS]; + +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"; +static const char CompX[] = "x_raw"; +static const char CompY[] = "y_raw"; +static const char CompZ[] = "z_raw"; static const char CompScale[] = "scale"; -int totalImpacts = 0; +int totalImpacts = 0; static const double impactThreshold = 20.0; Acceleration suddenImpact = {0, 0, 0}; @@ -34,55 +35,45 @@ Acceleration suddenImpact = {0, 0, 0}; pthread_t impactThread; pthread_mutex_t impactMutex; - /* * Reports the x, y and z accelerometer readings in meters per second squared. */ -le_result_t brnkl_motion_getCurrentAcceleration( - double *xAcc, - double *yAcc, - double *zAcc -) -{ - le_result_t r; - char path[256]; - - double scaling = 0.0; - r = ioutil_readDoubleFromFile(path, &scaling); - if (r != LE_OK) - { - goto done; - } +le_result_t brnkl_motion_getCurrentAcceleration(double* xAcc, + double* yAcc, + double* zAcc) { + le_result_t r; + char path[256]; - r = ioutil_readDoubleFromFile(path, xAcc); - if (r != LE_OK) - { - goto done; - } - *xAcc *= scaling; + double scaling = 0.0; + r = ioutil_readDoubleFromFile(path, &scaling); + if (r != LE_OK) { + goto done; + } - r = ioutil_readDoubleFromFile(path, yAcc); - if (r != LE_OK) - { - goto done; - } - *yAcc *= scaling; + r = ioutil_readDoubleFromFile(path, xAcc); + if (r != LE_OK) { + goto done; + } + *xAcc *= scaling; - r = ioutil_readDoubleFromFile(path, zAcc); - *zAcc *= scaling; + r = ioutil_readDoubleFromFile(path, yAcc); + if (r != LE_OK) { + goto done; + } + *yAcc *= scaling; + + r = ioutil_readDoubleFromFile(path, zAcc); + *zAcc *= scaling; done: - LE_INFO("Showing accel X: %f Y: %f Z: %f ", *xAcc, *yAcc, *zAcc); - return r; + LE_INFO("Showing accel X: %f Y: %f Z: %f ", *xAcc, *yAcc, *zAcc); + return r; } -le_result_t recordImpact(double* xAcc, double* yAcc, double* zAcc){ - if( - totalImpacts > N_CHANGE_BLOCKS || - totalImpacts > N_CHANGE_BLOCKS || - totalImpacts > N_CHANGE_BLOCKS - ) +le_result_t recordImpact(double* xAcc, double* yAcc, double* zAcc) { + if (totalImpacts > N_CHANGE_BLOCKS || totalImpacts > N_CHANGE_BLOCKS || + totalImpacts > N_CHANGE_BLOCKS) return LE_OUT_OF_RANGE; timestamps[totalImpacts] = GetCurrentTimestamp(); @@ -95,31 +86,28 @@ le_result_t recordImpact(double* xAcc, double* yAcc, double* zAcc){ return LE_OK; } -le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, - double* yAcc, size_t *ySize, - double* zAcc, size_t *zSize) { - - if(!totalImpacts) +le_result_t brnkl_motion_getSuddenImpact(double* xAcc, + size_t* xSize, + double* yAcc, + size_t* ySize, + double* zAcc, + size_t* zSize) { + if (!totalImpacts) LE_INFO("No Sudden Impacts to Report"); else { pthread_mutex_lock(&impactMutex); - //check + // check - if( - totalImpacts < *xSize || - totalImpacts < *ySize || - totalImpacts < *zSize - ) - return LE_OUT_OF_RANGE; + if (totalImpacts < *xSize || totalImpacts < *ySize || totalImpacts < *zSize) + return LE_OUT_OF_RANGE; - for(int i = 0; i < totalImpacts; i++){ + for (int i = 0; i < totalImpacts; i++) { xAcc[i] = xAccImpact[i]; yAcc[i] = yAccImpact[i]; zAcc[i] = zAccImpact[i]; } - - *xSize = *ySize = *zSize = totalImpacts; + *xSize = *ySize = *zSize = totalImpacts; totalImpacts = 0; @@ -133,25 +121,24 @@ le_result_t brnkl_motion_getSuddenImpact(double* xAcc, size_t *xSize, *Monitors accelerometer from iio on 100ms intervals *Sets hasSuddenImpact flag when accelerometer surpasses threshold */ -void *impactMonitor(void * ptr){ +void* impactMonitor(void* ptr) { double x, y, z; le_result_t r = LE_OK; - for(;;){ + for (;;) { brnkl_motion_getCurrentAcceleration(&x, &y, &z); - double impactMagnitude = sqrt(x*x + y*y + z*z); + double impactMagnitude = sqrt(x * x + y * y + z * z); - if(impactMagnitude > impactThreshold){ - //3. add x, y, z to impact array + if (impactMagnitude > impactThreshold) { + // 3. add x, y, z to impact array pthread_mutex_lock(&impactMutex); r = recordImpact(&x, &y, &z); pthread_mutex_unlock(&impactMutex); - } - if(r != LE_OK) + } + if (r != LE_OK) LE_ERROR("Impact Not Recorded"); - usleep(SAMPLE_PERIOD_MS*1000); - + usleep(SAMPLE_PERIOD_MS * 1000); } return ptr; } @@ -159,15 +146,15 @@ void *impactMonitor(void * ptr){ /* *Create thread to monitor accelerometer iio */ -void initThread(){ +void initThread() { int thread, mutx; LE_INFO("initThread called"); - mutx = pthread_mutex_init(&impactMutex, NULL); - thread = pthread_create( &impactThread, NULL, impactMonitor, NULL); + mutx = pthread_mutex_init(&impactMutex, NULL); + thread = pthread_create(&impactThread, NULL, impactMonitor, NULL); LE_INFO("mutexResult: %d", mutx); - if(thread || mutx){ + if (thread || mutx) { LE_ERROR("Reader Thread or Mutex Creation Failed"); - }else{ + } else { LE_INFO("Reader Thread Created"); } } -- cgit v1.2.3