curieimu: Plugin to interface with the arduino101's IMU

Internal sensor onboard the Curie/Arduino101 can be accessed via Firmata using
this plugin. You will need mraa compiled with -DFIRMATA=ON for this to work and
be using Firmata with the CurieIMU firmata extension for ExtensibleFirmata

Signed-off-by: Ron Evans <ron@hybridgroup.com>
Signed-off-by: Justin Zemlyansky <jlstigman@live.com>
Signed-off-by: Brendan Le Foll <brendan.le.foll@intel.com>
This commit is contained in:
deadprogram 2016-03-15 16:04:38 +00:00 committed by Brendan Le Foll
parent 1aa445b74e
commit 224a915538
8 changed files with 733 additions and 0 deletions

View File

@ -240,6 +240,7 @@ add_example (rhusb)
add_example (apds9930) add_example (apds9930)
add_example (kxcjk1013) add_example (kxcjk1013)
add_example (ssd1351) add_example (ssd1351)
add_example (curieimu)
# These are special cases where you specify example binary, source file and module(s) # These are special cases where you specify example binary, source file and module(s)
include_directories (${PROJECT_SOURCE_DIR}/src) include_directories (${PROJECT_SOURCE_DIR}/src)

70
examples/c++/curieimu.cxx Normal file
View File

@ -0,0 +1,70 @@
/*
* Author: Brendan Le Foll <brendan.le.foll@intel.com>
* 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 <unistd.h>
#include <iostream>
#include "curieimu.hpp"
#include "mraa.h"
#include "mraa/firmata.h"
#include <math.h>
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;
}

View File

@ -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()

384
src/curieimu/curieimu.cpp Normal file
View File

@ -0,0 +1,384 @@
/*
* Author: Brendan Le Foll <brendan.le.foll@intel.com>
* 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 <iostream>
#include <string>
#include <string.h>
#include <stdexcept>
#include <unistd.h>
#include <stdlib.h>
#include <pthread.h>
#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;
}
}

235
src/curieimu/curieimu.hpp Normal file
View File

@ -0,0 +1,235 @@
/*
* Author: Brendan Le Foll <brendan.le.foll@intel.com>
* 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 <mraa/firmata.h>
#include <queue>
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<IMUDataItem*> m_shockData;
std::queue<int> m_stepData;
std::queue<IMUDataItem*> m_tapData;
};
}

View File

@ -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);
}
}
%}

View File

@ -0,0 +1,8 @@
%module jsupm_curieimu
%include "../upm.i"
%{
#include "curieimu.hpp"
%}
%include "curieimu.hpp"

View File

@ -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"
%}