diff options
-rw-r--r-- | brnkl_motion.api | 6 | ||||
-rw-r--r-- | motionMonitor/motionMonitor.c | 160 |
2 files changed, 108 insertions, 58 deletions
diff --git a/brnkl_motion.api b/brnkl_motion.api index f5d3154..fd7c9b5 100644 --- a/brnkl_motion.api +++ b/brnkl_motion.api @@ -1,3 +1,5 @@ -FUNCTION int8 hasSuddenImpact (); -FUNCTION le_result_t getSuddenImpact (double x OUT, double y OUT, double z OUT); +DEFINE N_CHANGE_BLOCKS = 200; + +FUNCTION le_result_t getSuddenImpact (double x [N_CHANGE_BLOCKS] OUT, double y [N_CHANGE_BLOCKS] OUT, double z [N_CHANGE_BLOCKS] OUT); FUNCTION le_result_t getCurrentAcceleration (double x OUT, double y OUT, double z OUT); +
\ No newline at end of file diff --git a/motionMonitor/motionMonitor.c b/motionMonitor/motionMonitor.c index 9028653..0af8728 100644 --- a/motionMonitor/motionMonitor.c +++ b/motionMonitor/motionMonitor.c @@ -1,28 +1,44 @@ #include "interfaces.h" #include "legato.h" #include "util.h" +#include <pthread.h> + +#define N_CHANGE_BLOCKS 200 +#define SAMPLE_PERIOD_MS 100 + +void* impactMonitor(void*); + +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/bus/i2c/devices/3-0068/iio:device0/in_%s_%s"; + "/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; +static const double impactThreshold = 20.0; -typedef struct { - double x; - double y; - double z; -} Acceleration; - -bool hasSuddenImpact = false; 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) { @@ -30,87 +46,119 @@ le_result_t brnkl_motion_getCurrentAcceleration(double* xAcc, 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; - 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); + LE_INFO("Showing accel X: %f Y: %f Z: %f ", *xAcc, *yAcc, *zAcc); return r; } -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; +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(); + xAccImpact[totalImpacts] = *xAcc; + yAccImpact[totalImpacts] = *yAcc; + zAccImpact[totalImpacts] = *zAcc; + totalImpacts++; + LE_INFO("New Impact, totalImpacts: %d", totalImpacts); + return LE_OK; } -int8_t brnkl_motion_hasSuddenImpact() { - return hasSuddenImpact; +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 + + 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; + + totalImpacts = 0; + + pthread_mutex_unlock(&impactMutex); + } + + return LE_OK; } -// take a reading but make sure its "larger" than -// whatever is already in the buffer -void interruptChangeHandler(bool state, void* ctx) { +/* +*Monitors accelerometer from iio on 100ms intervals +*Sets hasSuddenImpact flag when accelerometer surpasses threshold +*/ +void* impactMonitor(void* ptr) { 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) { - hasSuddenImpact = true; + le_result_t r = LE_OK; + for (;;) { + brnkl_motion_getCurrentAcceleration(&x, &y, &z); + + double impactMagnitude = sqrt(x * x + y * y + z * z); + + 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) + LE_ERROR("Impact Not Recorded"); + + usleep(SAMPLE_PERIOD_MS * 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, mutx; + LE_INFO("initThread called"); + mutx = pthread_mutex_init(&impactMutex, NULL); + thread = pthread_create(&impactThread, NULL, impactMonitor, NULL); + LE_INFO("mutexResult: %d", mutx); + if (thread || mutx) { + LE_ERROR("Reader Thread or Mutex Creation Failed"); + } else { + LE_INFO("Reader Thread Created"); + } } COMPONENT_INIT { - initGpio(); + initThread(); } |