diff --git a/examples/c++/CMakeLists.txt b/examples/c++/CMakeLists.txt index b2277035..e427f933 100644 --- a/examples/c++/CMakeLists.txt +++ b/examples/c++/CMakeLists.txt @@ -240,6 +240,7 @@ add_example (rhusb) add_example (apds9930) add_example (kxcjk1013) add_example (ssd1351) +add_example (curieimu) # These are special cases where you specify example binary, source file and module(s) include_directories (${PROJECT_SOURCE_DIR}/src) diff --git a/examples/c++/curieimu.cxx b/examples/c++/curieimu.cxx new file mode 100644 index 00000000..150a2b46 --- /dev/null +++ b/examples/c++/curieimu.cxx @@ -0,0 +1,70 @@ +/* + * Author: Brendan Le Foll + * Author: Ron Evans (@deadprogram) + * Author: Justin Zemlyansky (@JustInDevelopment) + * Copyright (c) 2016 Intel Corporation. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include +#include +#include "curieimu.hpp" +#include "mraa.h" +#include "mraa/firmata.h" +#include + +int +main(int argc, char **argv) +{ + //! [Interesting] + mraa_init(); + mraa_add_subplatform(MRAA_GENERIC_FIRMATA, "/dev/ttyACM0"); + + upm::CurieImu* sensor = new upm::CurieImu(); + + std::cout << "temperature is: " << (sensor->getTemperature() * pow(0.5, 9) + 23) << std::endl; + + int x, y, z; + sensor->readAccelerometer(&x, &y, &z); + printf("accelerometer is: %d, %d, %d\n", x, y, z); + + int a, b, c; + sensor->readGyro(&a, &b, &c); + printf("gyroscope is: %d, %d, %d\n", a, b, c); + + int axis, direction; + sensor->enableShockDetection(true); + for(int i=0; i<300; i++) { + if (sensor->isShockDetected()) { + sensor->getShockDetectData(&axis, &direction); + printf("shock data is: %d, %d\n", axis, direction); + } + usleep(10000); + } + + int m, n, o, p, q, r; + sensor->readMotion(&m, &n, &o, &p, &q, &r); + printf("motion is: %d, %d, %d, %d, %d, %d\n", m, n, o, p, q, r); + + delete sensor; + + return 0; +} diff --git a/src/curieimu/CMakeLists.txt b/src/curieimu/CMakeLists.txt new file mode 100644 index 00000000..4955178e --- /dev/null +++ b/src/curieimu/CMakeLists.txt @@ -0,0 +1,5 @@ +set (libname "curieimu") +set (libdescription "upm Curie IMU via Firmata") +set (module_src ${libname}.cpp) +set (module_h ${libname}.hpp) +upm_module_init() diff --git a/src/curieimu/curieimu.cpp b/src/curieimu/curieimu.cpp new file mode 100644 index 00000000..7033976c --- /dev/null +++ b/src/curieimu/curieimu.cpp @@ -0,0 +1,384 @@ +/* + * Author: Brendan Le Foll + * Author: Ron Evans (@deadprogram) + * Author: Justin Zemlyansky (@JustInDevelopment) + * Copyright (c) 2016 Intel Corporation. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "curieimu.hpp" + +using namespace upm; + +static CurieImu* awaitingReponse; + +CurieImu::CurieImu (int subplatformoffset) +{ + m_firmata = mraa_firmata_init(FIRMATA_CURIE_IMU); + if (m_firmata == NULL) { + throw std::invalid_argument(std::string(__FUNCTION__) + + ": mraa_firmata_init() failed"); + return; + } + + if (pthread_mutex_init(&m_responseLock, NULL)) { + throw std::runtime_error(std::string(__FUNCTION__) + + ": pthread_mutex_init(m_responseLock) failed"); + return; + } + + if (pthread_cond_init(&m_responseCond, NULL)) { + throw std::runtime_error(std::string(__FUNCTION__) + + ": pthread_cond_init(m_responseCond) failed"); + return; + } +} + +CurieImu::~CurieImu() +{ + pthread_mutex_destroy(&m_responseLock); + pthread_cond_destroy(&m_responseCond); +} + +void +CurieImu::lock() +{ + pthread_mutex_lock(&m_responseLock); +} + +void +CurieImu::unlock() +{ + pthread_mutex_unlock(&m_responseLock); +} + +void +CurieImu::waitForResponse() +{ + awaitingReponse = this; + pthread_cond_wait(&m_responseCond, &m_responseLock); +} + +void +CurieImu::proceed() +{ + pthread_cond_broadcast(&m_responseCond); +} + +void +CurieImu::setResults(uint8_t* buf, int length) +{ + m_results = new char(length); + memcpy((void*)m_results, (void*)buf, length); +} + +/* +* Handles a single syncronous response being returned from Firmata +* +* @param buffer the data beinig returned from Firmata +* @param length length of results buffer +*/ +static void +handleSyncResponse(uint8_t* buf, int length) +{ + awaitingReponse->setResults(buf, length); + awaitingReponse->proceed(); +} + +/* +* Handles asyncronous responses being returned from Firmata +* +* @param buffer the data beinig returned from Firmata +* @param length length of results buffer +*/ +static void +handleAsyncResponses(uint8_t* buf, int length) +{ + awaitingReponse->setResults(buf, length); + awaitingReponse->processResponse(); +} + +void +CurieImu::processResponse() +{ + switch(m_results[2]) { + case FIRMATA_CURIE_IMU_SHOCK_DETECT: + { + IMUDataItem* item = new IMUDataItem(); + item->axis = m_results[3]; + item->direction = m_results[4]; + m_shockData.push(item); + break; + } + case FIRMATA_CURIE_IMU_STEP_COUNTER: + { + int count = ((m_results[3] & 0x7f) | ((m_results[4] & 0x7f) << 7)); + m_stepData.push(count); + break; + } + case FIRMATA_CURIE_IMU_TAP_DETECT: + { + IMUDataItem* item = new IMUDataItem(); + item->axis = m_results[3]; + item->direction = m_results[4]; + m_tapData.push(item); + break; + } + } + + return; +} + +void +CurieImu::readAccelerometer(int *xVal, int *yVal, int *zVal) +{ + char message[4]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_READ_ACCEL; + message[3] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleSyncResponse); + mraa_firmata_write_sysex(m_firmata, &message[0], 4); + + waitForResponse(); + + *xVal = ((m_results[3] & 0x7f) | ((m_results[4] & 0x7f) << 7)); + *yVal = ((m_results[5] & 0x7f) | ((m_results[6] & 0x7f) << 7)); + *zVal = ((m_results[7] & 0x7f) | ((m_results[8] & 0x7f) << 7)); + + delete m_results; + unlock(); + + return; +} + +void +CurieImu::readGyro(int *xVal, int *yVal, int *zVal) +{ + char message[4]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_READ_GYRO; + message[3] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleSyncResponse); + mraa_firmata_write_sysex(m_firmata, &message[0], 4); + + waitForResponse(); + + *xVal = ((m_results[3] & 0x7f) | ((m_results[4] & 0x7f) << 7)); + *yVal = ((m_results[5] & 0x7f) | ((m_results[6] & 0x7f) << 7)); + *zVal = ((m_results[7] & 0x7f) | ((m_results[8] & 0x7f) << 7)); + + delete m_results; + unlock(); + + return; +} + +int16_t +CurieImu::getTemperature() +{ + char message[4]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_READ_TEMP; + message[3] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleSyncResponse); + mraa_firmata_write_sysex(m_firmata, &message[0], 4); + + waitForResponse(); + + int16_t result; + result = ((m_results[3] & 0x7f) | ((m_results[4] & 0x7f) << 7)); + result += ((m_results[5] & 0x7f) | ((m_results[6] & 0x7f) << 7)) << 8; + + delete m_results; + unlock(); + + return result; +} + +void +CurieImu::readMotion(int *xA, int *yA, int *zA, int *xG, int *yG, int *zG) +{ + char message[4]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_READ_MOTION; + message[3] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleSyncResponse); + mraa_firmata_write_sysex(m_firmata, &message[0], 4); + + waitForResponse(); + + *xA = ((m_results[3] & 0x7f) | ((m_results[4] & 0x7f) << 7)); + *yA = ((m_results[5] & 0x7f) | ((m_results[6] & 0x7f) << 7)); + *zA = ((m_results[7] & 0x7f) | ((m_results[8] & 0x7f) << 7)); + *xG = ((m_results[9] & 0x7f) | ((m_results[10] & 0x7f) << 7)); + *yG = ((m_results[11] & 0x7f) | ((m_results[12] & 0x7f) << 7)); + *zG = ((m_results[13] & 0x7f) | ((m_results[13] & 0x7f) << 7)); + + delete m_results; + unlock(); + + return; +} + +void +CurieImu::enableShockDetection(bool enable) +{ + char message[5]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_SHOCK_DETECT; + message[3] = enable; + message[4] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleAsyncResponses); + mraa_firmata_write_sysex(m_firmata, &message[0], 5); + + awaitingReponse = this; + unlock(); + + return; +} + +bool +CurieImu::isShockDetected() +{ + return (m_shockData.size() > 0); +} + +void +CurieImu::getShockDetectData(int *axis, int *direction) +{ + if (m_shockData.size() > 0) { + IMUDataItem* item = m_shockData.front(); + *axis = item->axis; + *direction = item->direction; + m_shockData.pop(); + delete item; + } +} + +void +CurieImu::enableStepCounter(bool enable) +{ + char message[5]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_STEP_COUNTER; + message[3] = enable; + message[4] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleAsyncResponses); + mraa_firmata_write_sysex(m_firmata, &message[0], 5); + + awaitingReponse = this; + unlock(); + + return; +} + +bool +CurieImu::isStepDetected() +{ + return (m_stepData.size() > 0); +} + +void +CurieImu::getStepCount(int *count) +{ + if (m_stepData.size() > 0) { + *count = m_stepData.front(); + m_stepData.pop(); + } +} + +void +CurieImu::enableTapDetection(bool enable) +{ + char message[5]; + message[0] = FIRMATA_START_SYSEX; + message[1] = FIRMATA_CURIE_IMU; + message[2] = FIRMATA_CURIE_IMU_TAP_DETECT; + message[3] = enable; + message[4] = FIRMATA_END_SYSEX; + + lock(); + + mraa_firmata_response_stop(m_firmata); + mraa_firmata_response(m_firmata, handleAsyncResponses); + mraa_firmata_write_sysex(m_firmata, &message[0], 5); + + awaitingReponse = this; + unlock(); + + return; +} + +bool +CurieImu::isTapDetected() +{ + return (m_tapData.size() > 0); +} + +void +CurieImu::getTapDetectData(int *axis, int *direction) +{ + if (m_tapData.size() > 0) { + IMUDataItem* item = m_tapData.front(); + *axis = item->axis; + *direction = item->direction; + m_tapData.pop(); + delete item; + } +} diff --git a/src/curieimu/curieimu.hpp b/src/curieimu/curieimu.hpp new file mode 100644 index 00000000..bf6ecb0f --- /dev/null +++ b/src/curieimu/curieimu.hpp @@ -0,0 +1,235 @@ +/* + * Author: Brendan Le Foll + * Author: Ron Evans (@deadprogram) + * Author: Justin Zemlyansky (@JustInDevelopment) + * Copyright (c) 2016 Intel Corporation. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ +#pragma once + +#include +#include + +namespace upm { + +/** + * @brief CurieIMU sensor for Genuino 101 running Firmata + * @defgroup curieimu libupm-curieimu + * @ingroup firmata + */ + +/** + * @library curieimu + * @sensor curieimu + * @comname Curie IMU sensor over Genuino 101 running Firmata + * @altname Curie Firmata IMU + * @type firmata + * @man firmata imu genuino + * @con firmata + * + * @brief API for the Curie IMU via Firmata + * + * Curie IMU is a 6-axxis acclerometer + * + * This module has been tested on an Genuino 101 running ConfigurableFirmata with CurieIMU + * + * @snippet curieimu.cxx Interesting + */ + +#define FIRMATA_START_SYSEX 0xF0 +#define FIRMATA_END_SYSEX 0xF7 +#define FIRMATA_CURIE_IMU 0x11 +#define FIRMATA_CURIE_IMU_READ_ACCEL 0x00 +#define FIRMATA_CURIE_IMU_READ_GYRO 0x01 +#define FIRMATA_CURIE_IMU_READ_TEMP 0x02 +#define FIRMATA_CURIE_IMU_SHOCK_DETECT 0x03 +#define FIRMATA_CURIE_IMU_STEP_COUNTER 0x04 +#define FIRMATA_CURIE_IMU_TAP_DETECT 0x05 +#define FIRMATA_CURIE_IMU_READ_MOTION 0x06 + +struct IMUDataItem { + int axis; + int direction; +}; + +class CurieImu { + public: + + /** + * Instantiates a CurieImu object + * + * @param subplatformoffset Subplatform offset + */ + CurieImu (int subplatform_offset=512); + + /** + * Destructor for CurieImu object + */ + ~CurieImu(); + + /** + * Read accelerometer X, Y, and Z axis + * + * @param xVal Pointer to returned X-axis value + * @param yVal Pointer to returned Y-axis value + * @param zVal Pointer to returned Z-axis value + */ + void readAccelerometer(int *xVal, int *yVal, int *zVal); + + /** + * Read gyroscope X, Y, and Z axis + * + * @param xVal Pointer to returned X-axis value + * @param yVal Pointer to returned Y-axis value + * @param zVal Pointer to returned Z-axis value + */ + void readGyro(int *xVal, int *yVal, int *zVal); + + /** + * Reads the internal temperature + * + * @return 16-bit integer containing the scaled temperature reading + */ + int16_t getTemperature(); + + /** + * Reads the X, Y, and Z axis of both gyroscope and accelerometer + * + * @param xA Pointer to returned X-axis value of accelerometer + * @param yA Pointer to returned Y-axis value of accelerometer + * @param zA Pointer to returned Z-axis value of accelerometer + * @param xG Pointer to returned X-axis value of Gyroscope + * @param yG Pointer to returned Y-axis value of Gyroscope + * @param zG Pointer to returned Z-axis value of Gyroscope + */ + void readMotion(int *xA, int *yA, int *zA, int *xG, int *yG, int *zG); + + /** + * Turns shock detection notifications on/off + * + * @param enable enables/disables notifications + */ + void enableShockDetection(bool enable); + + /** + * Has there been a shock detected? + * + * @return true if any unprocessed shock notifications are in the queue + */ + bool isShockDetected(); + + /** + * Gets shock detect data from queue + * + * @param axis gets axis data + * @param direction gets direction data + */ + void getShockDetectData(int *axis, int *direction); + + /** + * Turns step counter notifications on/off + * + * @param enable enables/disables notifications + */ + void enableStepCounter(bool enable); + + /** + * Has there been a step detected? + * + * @return true if any unprocessed step notifications are in the queue + */ + bool isStepDetected(); + + /** + * Gets step count data from queue + * + * @param count the total number of steps taken + */ + void getStepCount(int *count); + + /** + * Turns tap detection notifications on/off + * + * @param enable enables/disables notifications + */ + void enableTapDetection(bool enable); + + /** + * Has there been a tap detected? + * + * @return true if any unprocessed tap notifications are in the queue + */ + bool isTapDetected(); + + /** + * Gets tap detect data from queue + * + * @param axis gets axis data + * @param direction gets direction data + */ + void getTapDetectData(int *axis, int *direction); + + /** + * Locks responses from Firmata + */ + void lock(); + + /** + * Unlocks responses from Firmata + */ + void unlock(); + + /** + * Wait for a response from Firmata before proceeding + */ + void waitForResponse(); + + /** + * Proceed with original function call now that response + * from Firmata has been received + */ + void proceed(); + + /** + * Set results being returned from Firmata for processing + * + * @param buf is the buffer + * @param length is the length of results buffer + */ + void setResults(uint8_t* buf, int length); + + /** + * Processes asyncronous responses returned from Firmata + */ + void processResponse(); + + private: + mraa_firmata_context m_firmata; + pthread_mutex_t m_responseLock; + pthread_cond_t m_responseCond; + char* m_results; + + std::queue m_shockData; + std::queue m_stepData; + std::queue m_tapData; +}; + +} diff --git a/src/curieimu/javaupm_curieimu.i b/src/curieimu/javaupm_curieimu.i new file mode 100644 index 00000000..725e10ec --- /dev/null +++ b/src/curieimu/javaupm_curieimu.i @@ -0,0 +1,19 @@ +%module javaupm_curieimu +%include "../upm.i" + +%{ + #include "curieimu.hpp" +%} + +%include "curieimu.hpp" + +%pragma(java) jniclasscode=%{ + static { + try { + System.loadLibrary("javaupm_curieimu"); + } catch (UnsatisfiedLinkError e) { + System.err.println("Native code library failed to load. \n" + e); + System.exit(1); + } + } +%} diff --git a/src/curieimu/jsupm_curieimu.i b/src/curieimu/jsupm_curieimu.i new file mode 100644 index 00000000..2ecb1deb --- /dev/null +++ b/src/curieimu/jsupm_curieimu.i @@ -0,0 +1,8 @@ +%module jsupm_curieimu +%include "../upm.i" + +%{ + #include "curieimu.hpp" +%} + +%include "curieimu.hpp" diff --git a/src/curieimu/pyupm_curieimu.i b/src/curieimu/pyupm_curieimu.i new file mode 100644 index 00000000..53054074 --- /dev/null +++ b/src/curieimu/pyupm_curieimu.i @@ -0,0 +1,11 @@ +// Include doxygen-generated documentation +%include "pyupm_doxy2swig.i" +%module pyupm_curieimu +%include "../upm.i" + +%include "stdint.i" + +%include "curieimu.hpp" +%{ + #include "curieimu.hpp" +%}