From 327c431a8e4b444ea1b7b67654f4dfabf1ab0945 Mon Sep 17 00:00:00 2001 From: denes Date: Thu, 18 Apr 2019 12:56:21 +0200 Subject: [PATCH] Merged in the Blinky app's functionality Publishes data when the timer of 1s awakes, not when samplesize > samplerate, as empty measurements are skipped, which can lead to a change in publishing time. --- MovesenseExerciseService.cpp | 96 ++++++++++++++++++++---------- MovesenseExerciseService.h | 18 +++++- wbresources/MovesenseExercise.yaml | 5 +- 3 files changed, 81 insertions(+), 38 deletions(-) diff --git a/MovesenseExerciseService.cpp b/MovesenseExerciseService.cpp index c3f9a2a..9138ce7 100644 --- a/MovesenseExerciseService.cpp +++ b/MovesenseExerciseService.cpp @@ -1,14 +1,18 @@ #include "MovesenseExerciseService.h" + #include "app-resources/resources.h" #include "common/core/debug.h" #include "meas_acc/resources.h" #include "whiteboard/builtinTypes/UnknownStructure.h" +#include "ui_ind/resources.h" #include #include -const char* const MovesenseExerciseService::LAUNCHABLE_NAME = "SampleA"; +const char* const MovesenseExerciseService::LAUNCHABLE_NAME = "MExercise"; #define SAMPLE_RATE 13 +const size_t BLINK_PERIOD_MS = 1000; + static const whiteboard::ExecutionContextId sExecutionContextId = WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::EXECUTION_CONTEXT; @@ -16,11 +20,27 @@ static const whiteboard::LocalResourceId sProviderResources[] = { WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::LID, }; +namespace { + whiteboard::FloatVector3D SumVectorAverage(const std::vector& v) + { + whiteboard::FloatVector3D sum; + for (size_t i = 0; i < v.size(); i++) + { + whiteboard::FloatVector3D fv = v[i]; + sum += fv; + } + return sum / (float)v.size(); + } +} + MovesenseExerciseService::MovesenseExerciseService() : ResourceClient(WBDEBUG_NAME(__FUNCTION__), sExecutionContextId), ResourceProvider(WBDEBUG_NAME(__FUNCTION__), sExecutionContextId), LaunchableModule(LAUNCHABLE_NAME, sExecutionContextId), - isRunning(false) + isRunning(false), + mLastTimeStamp(0), + mTimer(whiteboard::ID_INVALID_TIMER), + mMaxAccelerationData(SAMPLE_RATE) { } @@ -50,9 +70,21 @@ bool MovesenseExerciseService::startModule() { mModuleState = WB_RES::ModuleStateValues::STARTED; + // Start LED timer. true = trigger repeatedly + mTimer = ResourceClient::startTimer(BLINK_PERIOD_MS, true); + return true; } +void MovesenseExerciseService::stopModule() +{ + mModuleState = WB_RES::ModuleStateValues::STOPPED; + + // Stop LED timer + ResourceClient::stopTimer(mTimer); + mTimer = whiteboard::ID_INVALID_TIMER; +} + void MovesenseExerciseService::onUnsubscribeResult(whiteboard::RequestId requestId, whiteboard::ResourceId resourceId, whiteboard::Result resultCode, @@ -86,10 +118,6 @@ whiteboard::Result MovesenseExerciseService::startRunning(whiteboard::RequestId& DEBUGLOG("MovesenseExerciseService::startRunning()"); - // Reset max acceleration members - mMaxAccelerationSq = FLT_MIN; - mSamplesIncluded = 0; - wb::Result result = getResource("Meas/Acc/13", mMeasAccResourceId); if (!wb::RETURN_OKC(result)) { @@ -157,36 +185,11 @@ void MovesenseExerciseService::onNotify(whiteboard::ResourceId resourceId, const const whiteboard::Array& arrayData = linearAccelerationValue.arrayAcc; - uint32_t relativeTime = linearAccelerationValue.timestamp; + mLastTimeStamp = linearAccelerationValue.timestamp; for (size_t i = 0; i < arrayData.size(); i++) { - mSamplesIncluded++; - - whiteboard::FloatVector3D accValue = arrayData[i]; - float accelerationSq = accValue.mX * accValue.mX + - accValue.mY * accValue.mY + - accValue.mZ * accValue.mZ; - - if (mMaxAccelerationSq < accelerationSq) - mMaxAccelerationSq = accelerationSq; - } - - // 13Hz 5sec - if (mSamplesIncluded > 5 * 13) - { - // Reset counter and notify our subscribers - WB_RES::SampleDataValue sampleDataValue; - sampleDataValue.relativeTime = relativeTime; - sampleDataValue.value = sqrtf(mMaxAccelerationSq); - - // Reset members - mSamplesIncluded = 0; - mMaxAccelerationSq = FLT_MIN; - - // and update our WB resource. This causes notification to be fired to our subscribers - updateResource(WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES(), - ResponseOptions::Empty, sampleDataValue); + mMaxAccelerationData.push_back(arrayData[i]); } } break; @@ -250,3 +253,30 @@ void MovesenseExerciseService::onClientUnavailable(whiteboard::ClientId clientId DEBUGLOG("MovesenseExerciseService::onClientUnavailable()"); stopRunning(); } + +void MovesenseExerciseService::onTimer(whiteboard::TimerId timerId) +{ + if (timerId != mTimer) + { + return; + } + + uint16_t indicationType = 2; // SHORT_VISUAL_INDICATION, defined in ui/ind.yaml + + // Make PUT request to trigger led blink + asyncPut(WB_RES::LOCAL::UI_IND_VISUAL::ID, AsyncRequestOptions::Empty, indicationType); + + // Notify our subscribers + /// @todo The timestamp is the last measurement's timestamp and not the current time. + WB_RES::SampleDataValue sampleDataValue; + sampleDataValue.relativeTime = mLastTimeStamp; + sampleDataValue.value = SumVectorAverage(mMaxAccelerationData); + + // Reset members + mLastTimeStamp = 0; + mMaxAccelerationData.clear(); + + // and update our WB resource. This causes notification to be fired to our subscribers + updateResource(WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES(), + ResponseOptions::Empty, sampleDataValue); +} diff --git a/MovesenseExerciseService.h b/MovesenseExerciseService.h index 5fb75ed..c56fbca 100644 --- a/MovesenseExerciseService.h +++ b/MovesenseExerciseService.h @@ -7,6 +7,8 @@ #include "wb-resources/resources.h" +#include + class MovesenseExerciseService FINAL : private whiteboard::ResourceClient, private whiteboard::ResourceProvider, public whiteboard::LaunchableModule @@ -18,6 +20,14 @@ public: MovesenseExerciseService(); ~MovesenseExerciseService(); +protected: + /** + * Timer callback. + * + * @param timerId Id of timer that triggered + */ + virtual void onTimer(whiteboard::TimerId timerId) OVERRIDE; + private: /** @see whiteboard::ILaunchableModule::initModule */ virtual bool initModule() OVERRIDE; @@ -29,7 +39,7 @@ private: virtual bool startModule() OVERRIDE; /** @see whiteboard::ILaunchableModule::stopModule */ - virtual void stopModule() OVERRIDE { mModuleState = WB_RES::ModuleStateValues::STOPPED; } + virtual void stopModule() OVERRIDE; /** * Subscribe notification callback. @@ -118,6 +128,10 @@ private: whiteboard::RequestMap<2, void> mOngoingRequests; // For storing relations of incoming & outgoing requests bool isRunning; + + whiteboard::TimerId mTimer; + size_t mSamplesIncluded; - float mMaxAccelerationSq; + std::vector mMaxAccelerationData; + uint32_t mLastTimeStamp; }; diff --git a/wbresources/MovesenseExercise.yaml b/wbresources/MovesenseExercise.yaml index 48d8d6b..f5a88eb 100644 --- a/wbresources/MovesenseExercise.yaml +++ b/wbresources/MovesenseExercise.yaml @@ -55,7 +55,6 @@ definitions: format: uint32 x-unit: millisecond Value: - description: Sample Data Value (in practice max absolute acceleration, over about 5 seconds when subscribed) - type: number - format: float + description: Average acceleration, over 1 second + $ref: 'http://localhost:9000/builtinTypes.yaml#/definitions/FloatVector3D' x-unit: m/s^2