From a97e0fe14708cc53660d26a6f5f1e246175a182c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?kA=C2=AE0=C5=A1h=C3=AE?= Date: Wed, 1 Mar 2017 14:31:52 +0100 Subject: [PATCH 01/13] Added setAxisRemap and setAxisSign functions Added these two functions to allow remapping of the axis and reversal of their direction also. The developer can then use the macros already provided in the original header. --- Adafruit_BNO055.cpp | 39 ++++++++++++++++++++++++++++++++++++++- Adafruit_BNO055.h | 4 +++- 2 files changed, 41 insertions(+), 2 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 6fe9816..c6b9732 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -107,7 +107,7 @@ bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1 delay(10); */ - + write8(BNO055_SYS_TRIGGER_ADDR, 0x0); delay(10); /* Set the requested operating mode (see section 3.3) */ @@ -129,6 +129,43 @@ void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) delay(30); } +/**************************************************************************/ +/*! + @brief Changes the chip's axis remap +*/ +/**************************************************************************/ +void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode ) +{ + adafruit_bno055_opmode_t modeback = _mode; + + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode); + delay(10); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + +/**************************************************************************/ +/*! + @brief Changes the chip's axis signs +*/ +/**************************************************************************/ +void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign ) +{ + adafruit_bno055_opmode_t modeback = _mode; + + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign); + delay(10); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + + /**************************************************************************/ /*! @brief Use the external 32.768KHz crystal diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 36ba836..54ae110 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -1,4 +1,4 @@ -/*************************************************************************** +z/*************************************************************************** This is a library for the BNO055 orientation sensor Designed specifically to work with the Adafruit BNO055 Breakout. @@ -287,6 +287,8 @@ class Adafruit_BNO055 : public Adafruit_Sensor #endif bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF ); void setMode ( adafruit_bno055_opmode_t mode ); + void setAxisRemap ( adafruit_bno055_axis_remap_config_t remapcode ); + void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal ); From c1689b82f0bbbca92a6b0e4071de47c82cabdf91 Mon Sep 17 00:00:00 2001 From: Louis Date: Wed, 8 Mar 2017 17:09:29 +0100 Subject: [PATCH 02/13] Adding interrupt functions, currently all bundled into one very complex function but will later be separated into several smaller ones. --- Adafruit_BNO055.cpp | 172 ++++++++++++++++++++++++++++++++++++++++++-- Adafruit_BNO055.h | 40 ++++++++++- 2 files changed, 206 insertions(+), 6 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index c6b9732..2d15759 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -586,6 +586,171 @@ bool Adafruit_BNO055::isFullyCalibrated(void) return true; } +/**************************************************************************/ +/*! +@brief Enables interrupt and links it to the INT pin +*/ +/**************************************************************************/ +bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, std::string flags ) +{ + // initialise flags + int X_HG_EN = 0; int Y_HG_EN = 0; int Z_HG_EN = 0; + if (!flags.compare("x")){ + + } + + adafruit_bno055_opmode_t lastMode = _mode; + setMode(OPERATION_MODE_CONFIG); // set to config and allow settings to be changed + delay(25); + + write8(BNO055_PAGE_ID_ADDR, 1); // set access to the second register map + delay(25); + + // clear INT_EN before writing to it + bool status = write8(BNO055_INTR_EN_ADDR, (int8_t)((0 << 7) | (0 << 6) | (0 << 5) | (0 << 3) | (0 << 2))); + + // write to INT_EN 0x10 with 7, 6, 5, 3 or 2 to enable + status = write8(BNO055_INTR_EN_ADDR, (1 << int_en_code)); + + /* + uint8_t int_en_code = (0 << 7) | // Accelerometer no-motion/slow-motion enable + (0 << 6) | // Accelerometer any-motion enable + (0 << 5) | // Accelerometer high-g enable + (1 << 3) | // Gyroscope high rate enable + (0 << 2); // Gyroscope any-motion enable + */ + + // clear INT_MSK before writing to it + status = write8(BNO055_INTR_MSK_ADDR, (int8_t)((0 << 7) | (0 << 6) | (0 << 5) | (0 << 3) | (0 << 2))); + + // write to INT_MSK 0x0F with 7, 6, 5, 3 or 2 to link to INT pin + status = write8(BNO055_INTR_MSK_ADDR, (1 << int_en_code)); + + /* + uint8_t int_en_code = (0 << 7) | // Accelerometer no-motion/slow-motion on INT + (0 << 6) | // Accelerometer any-motion on INT + (0 << 5) | // Accelerometer high-g on INT + (1 << 3) | // Gyroscope high rate on INT + (0 << 2); // Gyroscope any-motion on INT + */ + +// TODO: Add flags to the function to allow deviation from default values! + + switch(int_en_code){ + + case ACC_NM: + write8(BNO055_INTR_ACCEL_NM_SETT, (0 << 0)); // this is setting whether or not slow or no motion is selected + write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG + break; + + case ACC_SM: + write8(BNO055_INTR_ACCEL_NM_SETT, (1 << 0)); // this is setting whether or not slow or no motion is selected + write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG + break; + + case ACC_AM: + write8(BNO055_INTR_ACCEL_AM_THRES, (1 << threshold)); // also dependent on ACC_CONFIG + break; + + case ACC_HIGH_G: + // set enables of axes + write8(BNO055_INTR_ACCEL_SETT, (int8_t)((X_HG_EN << 5) | (Y_HG_EN << 6) | (Z_HG_EN << 7))); + + // set duration of event required + if (duration < '256' && duration > '0'){ + write8(BNO055_INTR_ACCEL_HG_DUR, duration); // literally write the value into the register + } else { + Serial.println("Duration value higher than 255!"); + } + + // set threshold of event required + if (threshold < '256' && threshold > '0'){ + write8(BNO055_INTR_ACCEL_HG_THRES, threshold); // write the threshold value into the register + } else { + Serial.println("Threshold value higher than 255!"); + } + break; + + // TODO: currently enables EVERY axis indiscriminately + // TODO: selects only filtered data, should this register be cleared beforehand? + + case GYR_HIGH_RATE: + // set GYR_INT_SET bit 7 and 3:5 + write8(BNO055_INTR_GYR_SETT, (int8_t)((0 << 7) | (1 << 5) | (1 << 4) | (1 << 3))); // selects filtered data for high-rate interrupt + // set threshold, hysteresis and duration for each axis + // TODO: specify values for durations! + write8(BNO055_INTR_GYR_HR_X_SET, int value); + write8(BNO055_INTR_GYR_DUR_X, int value); + write8(BNO055_INTR_GYR_HR_Y_SET, int value); + write8(BNO055_INTR_GYR_DUR_Y, int value); + write8(BNO055_INTR_GYR_HR_Z_SET, int value); + write8(BNO055_INTR_GYR_DUR_Z, int value); + break; + case GYRO_AM: + // set GYR_INT_SET bit 7 and GYR_INT_SET 3:5 + write8(BNO055_INTR_GYR_SETT, (int8_t)((0 << 6) | (1 << 2) | (1 << 1) | (1 << 0))); + // set hysteresis threshold and duration for each axis + write8(BNO055_INTR_GYR_AM_THRES, threshold); + // TODO: this is set to a default awake duration of 32 samples and 12 slope samples (section 4.4.24) + write8(BNO055_INTR_GYR_AM_SET, ((1 << 3) | (0 << 2) | (1 << 1) | (0 << 0))); + break; + +/* Interrupt setup parameters (section 4.4.8 to 4.4.24) + ACC_NM - set ACC_NM_THRES 0x15, ACC_NM_SET 0x16 + ACC_AM - set ACC_AM_THRES 0x11 + ACC_HIGH_G - set HG_*_AXIS 0x12, ACC_HG_DUR 0x13 and ACC_HG_THRES 0x14 + + GYR_HIGH_RATE + Filter Settings - set bit 7 GYR_INT_SET 0x17, bit 3:5 GYR_INT_SET 0x17 + for X axis - set bit 5:6 GYR_HR_X_SET 0x18 hysteresis, bit 0:4 threshold + set bit 0:7 GYR_DUR_X 0x19 duration + for Y axis - set bit 5:6 GYR_HR_Y_SET 0x1A hysteresis, bit 0:4 threshold + set bit 0:7 GYR_DUR_Y 0x1B duration + for Z axis - set bit 5:6 GYR_HR_Z_SET 0x1C hysteresis, bit 0:4 threshold + set bit 0:7 GYR_DUR_Z 0x1D duration + + GYRO_AM - set bit 6 GYR_INT_SET 0x17, bit 0:2 GYR_INT_SET 0x17 + set bit 0:6 GYR_AM_THRES 0x1E threshold + set bit 2:3 GYR_AM_SET 0x1F awake duration, bit 0:1 slope samples +*/ + + } + + setMode(lastMode); // return to non-config mode + + if (status > 0){ + return false; + } else { + return true; + } + +} + +/**************************************************************************/ +/*! +@brief On interrupt reads the interrupt status register +*/ +/**************************************************************************/ +int8_t Adafruit_BNO055::readIntStatus() +{ + setMode(OPERATION_MODE_CONFIG); // change to config to change page + delay(25); + + write8(BNO055_PAGE_ID_ADDR, 0); // set access to the first register page + delay(25); + // read from INT_STA to determine the type of interrupt occurance + int8_t intstatus = read8(BNO055_INTR_STAT_ADDR); + + /* Interrupt Status (see section 4.3.56) + --------------------------------- + 0 = No Status + 4 = Gyroscope any-motion interrupt + 8 = Gyroscope high-rate interrupt + 32 = Accelerometer any-motion interrupt + 49 = Accelerometer no-motion/slow-motion interrupt */ + + return intstatus; +} /*************************************************************************** PRIVATE FUNCTIONS @@ -596,7 +761,7 @@ bool Adafruit_BNO055::isFullyCalibrated(void) @brief Writes an 8 bit value over I2C */ /**************************************************************************/ -bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) +int8_t Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) { Wire.beginTransmission(_address); #if ARDUINO >= 100 @@ -606,10 +771,9 @@ bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) Wire.send(reg); Wire.send(value); #endif - Wire.endTransmission(); + int8_t status = Wire.endTransmission(); - /* ToDo: Check for error! */ - return true; + return status; } /**************************************************************************/ diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 54ae110..ce13534 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -1,4 +1,4 @@ -z/*************************************************************************** +/*************************************************************************** This is a library for the BNO055 orientation sensor Designed specifically to work with the Adafruit BNO055 Breakout. @@ -20,6 +20,9 @@ z/*************************************************************************** #ifndef __ADAFRUIT_BNO055_H__ #define __ADAFRUIT_BNO055_H__ +#include +#include + #if (ARDUINO >= 100) #include "Arduino.h" #else @@ -139,6 +142,8 @@ class Adafruit_BNO055 : public Adafruit_Sensor /* Status registers */ BNO055_CALIB_STAT_ADDR = 0X35, BNO055_SELFTEST_RESULT_ADDR = 0X36, + + /* Interrupt status register PAGE 0 */ BNO055_INTR_STAT_ADDR = 0X37, BNO055_SYS_CLK_STAT_ADDR = 0X38, @@ -156,6 +161,25 @@ class Adafruit_BNO055 : public Adafruit_Sensor BNO055_SYS_TRIGGER_ADDR = 0X3F, BNO055_TEMP_SOURCE_ADDR = 0X40, + /* Interrupt config registers PAGE 1 */ + BNO055_INTR_EN_ADDR = 0x10, + BNO055_INTR_MSK_ADDR = 0x0F, + BNO055_INTR_ACCEL_AM_THRES = 0x11, + BNO055_INTR_ACCEL_SETT = 0x12, + BNO055_INTR_ACCEL_HG_DUR = 0x13, + BNO055_INTR_ACCEL_HG_THRES = 0x14, + BNO055_INTR_ACCEL_NM_THRES = 0x15, + BNO055_INTR_ACCEL_NM_SETT = 0x16, + BNO055_INTR_GYR_SETT = 0x17, + BNO055_INTR_GYR_HR_X_SET = 0x18, + BNO055_INTR_GYR_DUR_X = 0x19, + BNO055_INTR_GYR_HR_Y_SET = 0x1A, + BNO055_INTR_GYR_DUR_Y = 0x1B, + BNO055_INTR_GYR_HR_Z_SET = 0x1C, + BNO055_INTR_GYR_DUR_Z = 0x1D, + BNO055_INTR_GYR_AM_THRES = 0x1E, + BNO055_INTR_GYR_AM_SET = 0x1F, + /* Axis remap registers */ BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, BNO055_AXIS_MAP_SIGN_ADDR = 0X42, @@ -260,6 +284,16 @@ class Adafruit_BNO055 : public Adafruit_Sensor REMAP_SIGN_P7 = 0x05 } adafruit_bno055_axis_remap_sign_t; + typedef enum + { + ACC_NM = 7, + ACC_SM = 7, + ACC_AM = 6, // default + ACC_HIGH_G = 5, + GYR_HIGH_RATE = 3, + GYRO_AM = 2 + } adafruit_bno055_intr_en_t; + typedef struct { uint8_t accel_rev; @@ -289,6 +323,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setMode ( adafruit_bno055_opmode_t mode ); void setAxisRemap ( adafruit_bno055_axis_remap_config_t remapcode ); void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign ); + bool enableMotionInt ( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, std::string flags ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal ); @@ -301,6 +336,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor imu::Vector<3> getVector ( adafruit_vector_type_t vector_type ); imu::Quaternion getQuat ( void ); int8_t getTemp ( void ); + int8_t readIntStatus ( void ); /* Adafruit_Sensor implementation */ bool getEvent ( sensors_event_t* ); @@ -316,7 +352,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor private: byte read8 ( adafruit_bno055_reg_t ); bool readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len ); - bool write8 ( adafruit_bno055_reg_t, byte value ); + int8_t write8 ( adafruit_bno055_reg_t, byte value ); uint8_t _address; int32_t _sensorID; From cbd7d59465101050114326c666d2cf5168b27411 Mon Sep 17 00:00:00 2001 From: Louis Date: Fri, 10 Mar 2017 12:21:14 +0100 Subject: [PATCH 03/13] Update to the enableMotionInt function. --- Adafruit_BNO055.cpp | 26 +++++++++++++------------- Adafruit_BNO055.h | 3 +-- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 2d15759..42a3fbb 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -636,6 +636,11 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in // TODO: Add flags to the function to allow deviation from default values! + if (int_en_code == ACC_AM){ + write8(BNO055_INTR_ACCEL_NM_SETT, (1 << 0)); // this is setting whether or not slow or no motion is selected + write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG + } + switch(int_en_code){ case ACC_NM: @@ -643,11 +648,6 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG break; - case ACC_SM: - write8(BNO055_INTR_ACCEL_NM_SETT, (1 << 0)); // this is setting whether or not slow or no motion is selected - write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG - break; - case ACC_AM: write8(BNO055_INTR_ACCEL_AM_THRES, (1 << threshold)); // also dependent on ACC_CONFIG break; @@ -657,14 +657,14 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in write8(BNO055_INTR_ACCEL_SETT, (int8_t)((X_HG_EN << 5) | (Y_HG_EN << 6) | (Z_HG_EN << 7))); // set duration of event required - if (duration < '256' && duration > '0'){ + if ((duration < 256) && (duration > 0)){ write8(BNO055_INTR_ACCEL_HG_DUR, duration); // literally write the value into the register } else { Serial.println("Duration value higher than 255!"); } // set threshold of event required - if (threshold < '256' && threshold > '0'){ + if ((threshold < 256) && (threshold > 0)){ write8(BNO055_INTR_ACCEL_HG_THRES, threshold); // write the threshold value into the register } else { Serial.println("Threshold value higher than 255!"); @@ -679,12 +679,12 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in write8(BNO055_INTR_GYR_SETT, (int8_t)((0 << 7) | (1 << 5) | (1 << 4) | (1 << 3))); // selects filtered data for high-rate interrupt // set threshold, hysteresis and duration for each axis // TODO: specify values for durations! - write8(BNO055_INTR_GYR_HR_X_SET, int value); - write8(BNO055_INTR_GYR_DUR_X, int value); - write8(BNO055_INTR_GYR_HR_Y_SET, int value); - write8(BNO055_INTR_GYR_DUR_Y, int value); - write8(BNO055_INTR_GYR_HR_Z_SET, int value); - write8(BNO055_INTR_GYR_DUR_Z, int value); + write8(BNO055_INTR_GYR_HR_X_SET, (int8_t)(1 << 0)); // 0x19 is the default + write8(BNO055_INTR_GYR_DUR_X, 0x19); + write8(BNO055_INTR_GYR_HR_Y_SET, (int8_t)(1 << 0)); + write8(BNO055_INTR_GYR_DUR_Y, 0x19); + write8(BNO055_INTR_GYR_HR_Z_SET, (int8_t)(1 << 0)); + write8(BNO055_INTR_GYR_DUR_Z, 0x19); break; case GYRO_AM: // set GYR_INT_SET bit 7 and GYR_INT_SET 3:5 diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index ce13534..1688582 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -21,7 +21,6 @@ #define __ADAFRUIT_BNO055_H__ #include -#include #if (ARDUINO >= 100) #include "Arduino.h" @@ -287,7 +286,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor typedef enum { ACC_NM = 7, - ACC_SM = 7, + ACC_SM = 0x7, ACC_AM = 6, // default ACC_HIGH_G = 5, GYR_HIGH_RATE = 3, From a582724d78ef82c7b807b7994a029d0b1e5ca792 Mon Sep 17 00:00:00 2001 From: mrkaroshi Date: Fri, 10 Mar 2017 14:13:04 +0100 Subject: [PATCH 04/13] Still having issue with linker picking up on 'collect2' error, won't build firmware on Teensy when this library is used. --- Adafruit_BNO055.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 1688582..252f74f 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -20,8 +20,6 @@ #ifndef __ADAFRUIT_BNO055_H__ #define __ADAFRUIT_BNO055_H__ -#include - #if (ARDUINO >= 100) #include "Arduino.h" #else From 4edcb652ff2260e96dab24fe5e36028fece65036 Mon Sep 17 00:00:00 2001 From: mrkaroshi Date: Fri, 10 Mar 2017 15:24:32 +0100 Subject: [PATCH 05/13] Added dependency back in to test for errors. --- Adafruit_BNO055.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 252f74f..873236b 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -35,6 +35,7 @@ #include #include +#include #define BNO055_ADDRESS_A (0x28) #define BNO055_ADDRESS_B (0x29) From 87c855cad0f2b8dd1c8dd0a1426de89dfabd33e4 Mon Sep 17 00:00:00 2001 From: mrkaroshi Date: Fri, 10 Mar 2017 16:01:25 +0100 Subject: [PATCH 06/13] Still checking dependencies... --- Adafruit_BNO055.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 873236b..2b691b7 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -35,7 +35,7 @@ #include #include -#include +#include #define BNO055_ADDRESS_A (0x28) #define BNO055_ADDRESS_B (0x29) From 50f2d412767855b5d146475df9a3591577b591ec Mon Sep 17 00:00:00 2001 From: mrkaroshi Date: Fri, 10 Mar 2017 16:05:57 +0100 Subject: [PATCH 07/13] Yawwwwwwwwn, more tweaking. --- Adafruit_BNO055.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 2b691b7..873236b 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -35,7 +35,7 @@ #include #include -#include +#include #define BNO055_ADDRESS_A (0x28) #define BNO055_ADDRESS_B (0x29) From 36467bea20bfa73eb98a66b64889ac95c983141d Mon Sep 17 00:00:00 2001 From: mrkaroshi Date: Fri, 10 Mar 2017 18:10:13 +0100 Subject: [PATCH 08/13] Removed dependency on std::string as it was causing issues. --- Adafruit_BNO055.cpp | 4 ++-- Adafruit_BNO055.h | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 42a3fbb..78e307f 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -591,11 +591,11 @@ bool Adafruit_BNO055::isFullyCalibrated(void) @brief Enables interrupt and links it to the INT pin */ /**************************************************************************/ -bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, std::string flags ) +bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, char flags[3] ) { // initialise flags int X_HG_EN = 0; int Y_HG_EN = 0; int Z_HG_EN = 0; - if (!flags.compare("x")){ + if (flags.indexOf("x") != '-1'){ } diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 873236b..a219219 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -35,7 +35,6 @@ #include #include -#include #define BNO055_ADDRESS_A (0x28) #define BNO055_ADDRESS_B (0x29) @@ -321,7 +320,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setMode ( adafruit_bno055_opmode_t mode ); void setAxisRemap ( adafruit_bno055_axis_remap_config_t remapcode ); void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign ); - bool enableMotionInt ( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, std::string flags ); + bool enableMotionInt ( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, char flags[3] ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal ); From e14a9231d2cb8a4f331bae9cecb419ef35d132db Mon Sep 17 00:00:00 2001 From: mrkaroshi Date: Mon, 13 Mar 2017 10:44:37 +0100 Subject: [PATCH 09/13] Bug fix. --- Adafruit_BNO055.cpp | 2 +- Adafruit_BNO055.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 78e307f..5cbc7e5 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -591,7 +591,7 @@ bool Adafruit_BNO055::isFullyCalibrated(void) @brief Enables interrupt and links it to the INT pin */ /**************************************************************************/ -bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, char flags[3] ) +bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, String flags ) { // initialise flags int X_HG_EN = 0; int Y_HG_EN = 0; int Z_HG_EN = 0; diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index a219219..5cafb1e 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -320,7 +320,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setMode ( adafruit_bno055_opmode_t mode ); void setAxisRemap ( adafruit_bno055_axis_remap_config_t remapcode ); void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign ); - bool enableMotionInt ( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, char flags[3] ); + bool enableMotionInt ( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, String flags ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal ); From 71ded28133c750b00a652390ffe635e215428169 Mon Sep 17 00:00:00 2001 From: kAr0sh1 Date: Mon, 13 Mar 2017 17:38:00 +0100 Subject: [PATCH 10/13] Added a series of debugging outputs for stages of configuration, added a missing option on the No Motion trigger duration. --- Adafruit_BNO055.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 5cbc7e5..af9fb9e 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -595,8 +595,18 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in { // initialise flags int X_HG_EN = 0; int Y_HG_EN = 0; int Z_HG_EN = 0; + // set flags based on input String if (flags.indexOf("x") != '-1'){ - + X_HG_EN = 1; + Serial.println("X axis trigger enabled."); + } + if (flags.indexOf("y") != '-1'){ + Y_HG_EN = 1; + Serial.println("Y axis trigger enabled."); + } + if (flags.indexOf("z") != '-1'){ + Z_HG_EN = 1; + Serial.println("Z axis trigger enabled."); } adafruit_bno055_opmode_t lastMode = _mode; @@ -644,7 +654,9 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in switch(int_en_code){ case ACC_NM: + Serial.println("ACC_NM mode chosen."); write8(BNO055_INTR_ACCEL_NM_SETT, (0 << 0)); // this is setting whether or not slow or no motion is selected + write8(BNO055_INTR_ACCEL_NM_SETT, (1 << duration)); // this sets the number of points that must be above the threshold to trigger write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG break; From 220dbe37568f73bfccee5972ce91bc64eea08b10 Mon Sep 17 00:00:00 2001 From: kAr0sh1 Date: Tue, 14 Mar 2017 18:08:52 +0100 Subject: [PATCH 11/13] Added the enableInterrupts and enableInterruptAxes functions. Untested. --- Adafruit_BNO055.cpp | 291 +++++++++++++++++++++++++++----------------- Adafruit_BNO055.h | 6 +- 2 files changed, 186 insertions(+), 111 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index af9fb9e..01a2ec6 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -588,147 +588,194 @@ bool Adafruit_BNO055::isFullyCalibrated(void) /**************************************************************************/ /*! -@brief Enables interrupt and links it to the INT pin +@brief Enables interrupt type specified and disables/enables interrupt pin */ /**************************************************************************/ -bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, String flags ) +bool Adafruit_BNO055::enableInterrupts( adafruit_bno055_intr_en_t int_en_code, bool triggerPin) { - // initialise flags - int X_HG_EN = 0; int Y_HG_EN = 0; int Z_HG_EN = 0; - // set flags based on input String - if (flags.indexOf("x") != '-1'){ - X_HG_EN = 1; - Serial.println("X axis trigger enabled."); - } - if (flags.indexOf("y") != '-1'){ - Y_HG_EN = 1; - Serial.println("Y axis trigger enabled."); - } - if (flags.indexOf("z") != '-1'){ - Z_HG_EN = 1; - Serial.println("Z axis trigger enabled."); - } + // create status variable + int8_t status = 0; // if greater than zero a failure has occured - adafruit_bno055_opmode_t lastMode = _mode; - setMode(OPERATION_MODE_CONFIG); // set to config and allow settings to be changed + // enter config mode + adafruit_bno055_opmode_t modeback = _mode; + setMode(OPERATION_MODE_CONFIG); delay(25); - write8(BNO055_PAGE_ID_ADDR, 1); // set access to the second register map - delay(25); + // save selected page ID and switch to page 1 + uint8_t savePageID = read8(BNO055_PAGE_ID_ADDR); + write8(BNO055_PAGE_ID_ADDR, 0x01); - // clear INT_EN before writing to it - bool status = write8(BNO055_INTR_EN_ADDR, (int8_t)((0 << 7) | (0 << 6) | (0 << 5) | (0 << 3) | (0 << 2))); + // enable specific interrupt type requested + switch (int_en_code) //TODO: change these definitions (ACC_NM etc) into a number sequence + { + case ACC_NM: + if(triggerPin){ + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (1 << 7)); // enable pin change + } else { + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 7)); // enable pin change + } + write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 7)); // enable interrupt + write8(BNO055_INTR_ACCEL_NM_SETT, (read8(BNO055_INTR_ACCEL_NM_SETT)) | (0 << 0)); // decides SM/NM + break; + case ACC_AM: + if(triggerPin){ + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (1 << 6)); // enable pin change + } else { + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 6)); // enable pin change + } + write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 6)); // enable interrupt + break; + case ACC_SM: + if(triggerPin){ + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (1 << 7)); // enable pin change + } else { + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 7)); // enable pin change + } + write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 7)); // enable interrupt + write8(BNO055_INTR_ACCEL_NM_SETT, (read8(BNO055_INTR_ACCEL_NM_SETT)) | (0 << 0)); // decides SM/NM + break; + case ACC_HIGH_G: + if(triggerPin){ + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (1 << 5)); // enable pin change + } else { + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 5)); // enable pin change + } + write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 5)); // enable interrupt + break; + case GYR_HIGH_RATE: + if(triggerPin){ + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (1 << 3)); // enable pin change + } else { + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 3)); // enable pin change + } + write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 3)); // enable interrupt + break; + case GYRO_AM: + if(triggerPin){ + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (1 << 2)); // enable pin change + } else { + status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 2)); // enable pin change + } + write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 2)); // enable interrupt + break; + } + // restore page ID + write8(BNO055_PAGE_ID_ADDR, savePageID); - // write to INT_EN 0x10 with 7, 6, 5, 3 or 2 to enable - status = write8(BNO055_INTR_EN_ADDR, (1 << int_en_code)); + // Set the requested operating mode (see section 3.3) + setMode(modeback); + delay(20); - /* - uint8_t int_en_code = (0 << 7) | // Accelerometer no-motion/slow-motion enable - (0 << 6) | // Accelerometer any-motion enable - (0 << 5) | // Accelerometer high-g enable - (1 << 3) | // Gyroscope high rate enable - (0 << 2); // Gyroscope any-motion enable - */ + if (status > 0){ + return false; + } else { + return true; + } - // clear INT_MSK before writing to it - status = write8(BNO055_INTR_MSK_ADDR, (int8_t)((0 << 7) | (0 << 6) | (0 << 5) | (0 << 3) | (0 << 2))); +} - // write to INT_MSK 0x0F with 7, 6, 5, 3 or 2 to link to INT pin - status = write8(BNO055_INTR_MSK_ADDR, (1 << int_en_code)); +/**************************************************************************/ +/*! +@brief Sets interrupt axes +*/ +/**************************************************************************/ - /* - uint8_t int_en_code = (0 << 7) | // Accelerometer no-motion/slow-motion on INT - (0 << 6) | // Accelerometer any-motion on INT - (0 << 5) | // Accelerometer high-g on INT - (1 << 3) | // Gyroscope high rate on INT - (0 << 2); // Gyroscope any-motion on INT - */ +bool Adafruit_BNO055::enableInterruptAxes( adafruit_bno055_intr_en_t int_en_code, String axes ) +{ + // create status variable + int8_t status = 0; // if greater than zero a failure has occured -// TODO: Add flags to the function to allow deviation from default values! + // enter config mode + adafruit_bno055_opmode_t modeback = _mode; + setMode(OPERATION_MODE_CONFIG); + delay(25); + + // save selected page ID and switch to page 1 + uint8_t savePageID = read8(BNO055_PAGE_ID_ADDR); + write8(BNO055_PAGE_ID_ADDR, 0x01); - if (int_en_code == ACC_AM){ - write8(BNO055_INTR_ACCEL_NM_SETT, (1 << 0)); // this is setting whether or not slow or no motion is selected - write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG + // parse axis flags + int X_EN = 0; int Y_EN = 0; int Z_EN = 0; + // set flags based on input String + if (axes.indexOf("x") == 0){ + X_EN = 1; + Serial.println("X axis trigger enabled."); + } + if (axes.indexOf("y") == 0){ + Y_EN = 1; + Serial.println("Y axis trigger enabled."); + } + if (axes.indexOf("z") == 0){ + Z_EN = 1; + Serial.println("Z axis trigger enabled."); } switch(int_en_code){ - case ACC_NM: - Serial.println("ACC_NM mode chosen."); - write8(BNO055_INTR_ACCEL_NM_SETT, (0 << 0)); // this is setting whether or not slow or no motion is selected - write8(BNO055_INTR_ACCEL_NM_SETT, (1 << duration)); // this sets the number of points that must be above the threshold to trigger - write8(BNO055_INTR_ACCEL_NM_THRES, (1 << threshold)); // this is dependent on ACC_CONFIG + if(X_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 2)); + } + if(Y_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 3)); + } + if(Z_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 4)); + } break; - case ACC_AM: - write8(BNO055_INTR_ACCEL_AM_THRES, (1 << threshold)); // also dependent on ACC_CONFIG + if(X_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 2)); + } + if(Y_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 3)); + } + if(Z_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 4)); + } break; + case ACC_SM: + break; case ACC_HIGH_G: - // set enables of axes - write8(BNO055_INTR_ACCEL_SETT, (int8_t)((X_HG_EN << 5) | (Y_HG_EN << 6) | (Z_HG_EN << 7))); - - // set duration of event required - if ((duration < 256) && (duration > 0)){ - write8(BNO055_INTR_ACCEL_HG_DUR, duration); // literally write the value into the register - } else { - Serial.println("Duration value higher than 255!"); + if(X_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 5)); } - - // set threshold of event required - if ((threshold < 256) && (threshold > 0)){ - write8(BNO055_INTR_ACCEL_HG_THRES, threshold); // write the threshold value into the register - } else { - Serial.println("Threshold value higher than 255!"); + if(Y_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 6)); + } + if(Z_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 7)); } break; - - // TODO: currently enables EVERY axis indiscriminately - // TODO: selects only filtered data, should this register be cleared beforehand? - case GYR_HIGH_RATE: - // set GYR_INT_SET bit 7 and 3:5 - write8(BNO055_INTR_GYR_SETT, (int8_t)((0 << 7) | (1 << 5) | (1 << 4) | (1 << 3))); // selects filtered data for high-rate interrupt - // set threshold, hysteresis and duration for each axis - // TODO: specify values for durations! - write8(BNO055_INTR_GYR_HR_X_SET, (int8_t)(1 << 0)); // 0x19 is the default - write8(BNO055_INTR_GYR_DUR_X, 0x19); - write8(BNO055_INTR_GYR_HR_Y_SET, (int8_t)(1 << 0)); - write8(BNO055_INTR_GYR_DUR_Y, 0x19); - write8(BNO055_INTR_GYR_HR_Z_SET, (int8_t)(1 << 0)); - write8(BNO055_INTR_GYR_DUR_Z, 0x19); + if(X_EN){ + status = write8(BNO055_INTR_GYR_SETT, (read8(BNO055_INTR_GYR_SETT)) | (1 << 3)); + } + if(Y_EN){ + status = write8(BNO055_INTR_GYR_SETT, (read8(BNO055_INTR_GYR_SETT)) | (1 << 4)); + } + if(Z_EN){ + status = write8(BNO055_INTR_GYR_SETT, (read8(BNO055_INTR_GYR_SETT)) | (1 << 5)); + } break; case GYRO_AM: - // set GYR_INT_SET bit 7 and GYR_INT_SET 3:5 - write8(BNO055_INTR_GYR_SETT, (int8_t)((0 << 6) | (1 << 2) | (1 << 1) | (1 << 0))); - // set hysteresis threshold and duration for each axis - write8(BNO055_INTR_GYR_AM_THRES, threshold); - // TODO: this is set to a default awake duration of 32 samples and 12 slope samples (section 4.4.24) - write8(BNO055_INTR_GYR_AM_SET, ((1 << 3) | (0 << 2) | (1 << 1) | (0 << 0))); + if(X_EN){ + status = write8(BNO055_INTR_GYR_SETT, (read8(BNO055_INTR_GYR_SETT)) | (1 << 2)); + } + if(Y_EN){ + status = write8(BNO055_INTR_GYR_SETT, (read8(BNO055_INTR_GYR_SETT)) | (1 << 1)); + } + if(Z_EN){ + status = write8(BNO055_INTR_GYR_SETT, (read8(BNO055_INTR_GYR_SETT)) | (1 << 0)); + } break; - -/* Interrupt setup parameters (section 4.4.8 to 4.4.24) - ACC_NM - set ACC_NM_THRES 0x15, ACC_NM_SET 0x16 - ACC_AM - set ACC_AM_THRES 0x11 - ACC_HIGH_G - set HG_*_AXIS 0x12, ACC_HG_DUR 0x13 and ACC_HG_THRES 0x14 - - GYR_HIGH_RATE - Filter Settings - set bit 7 GYR_INT_SET 0x17, bit 3:5 GYR_INT_SET 0x17 - for X axis - set bit 5:6 GYR_HR_X_SET 0x18 hysteresis, bit 0:4 threshold - set bit 0:7 GYR_DUR_X 0x19 duration - for Y axis - set bit 5:6 GYR_HR_Y_SET 0x1A hysteresis, bit 0:4 threshold - set bit 0:7 GYR_DUR_Y 0x1B duration - for Z axis - set bit 5:6 GYR_HR_Z_SET 0x1C hysteresis, bit 0:4 threshold - set bit 0:7 GYR_DUR_Z 0x1D duration - - GYRO_AM - set bit 6 GYR_INT_SET 0x17, bit 0:2 GYR_INT_SET 0x17 - set bit 0:6 GYR_AM_THRES 0x1E threshold - set bit 2:3 GYR_AM_SET 0x1F awake duration, bit 0:1 slope samples -*/ - } + // restore page ID + write8(BNO055_PAGE_ID_ADDR, savePageID); - setMode(lastMode); // return to non-config mode + // Set the requested operating mode (see section 3.3) + setMode(modeback); + delay(20); if (status > 0){ return false; @@ -736,6 +783,32 @@ bool Adafruit_BNO055::enableMotionInt( adafruit_bno055_intr_en_t int_en_code, in return true; } +} +// TODO: setThreshold and setDuration, setHysteresis. Also disableInterrupt, disableInterruptAxes and checkInterruptStates + +/**************************************************************************/ +/*! +@brief Retrieve interrupt states and settings - DEBUG FUNCTION +*/ +/**************************************************************************/ +char * Adafruit_BNO055::checkInterruptStates() +{ + // check the interrupt status register + + // check the enable registers + + // check the interrupt mask + + // check the axis enable states + + // check the threshold levels + + // check the duration levels + + // check the hysteresis settings + + // print settings as serial output + } /**************************************************************************/ diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 5cafb1e..38ff99d 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -284,7 +284,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor typedef enum { ACC_NM = 7, - ACC_SM = 0x7, + ACC_SM = 8, ACC_AM = 6, // default ACC_HIGH_G = 5, GYR_HIGH_RATE = 3, @@ -320,10 +320,12 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setMode ( adafruit_bno055_opmode_t mode ); void setAxisRemap ( adafruit_bno055_axis_remap_config_t remapcode ); void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign ); - bool enableMotionInt ( adafruit_bno055_intr_en_t int_en_code, int8_t duration, int8_t threshold, String flags ); + bool enableInterrupts ( adafruit_bno055_intr_en_t int_en_code, bool triggerPin); + bool enableInterruptAxes ( adafruit_bno055_intr_en_t int_en_code, String axes ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal ); + char * checkInterruptStates( void ); void getSystemStatus ( uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error); From 2775a77ef1b982612f0a2429434dd6889adec9be Mon Sep 17 00:00:00 2001 From: kAr0sh1 Date: Thu, 16 Mar 2017 13:22:17 +0100 Subject: [PATCH 12/13] Added debugging function checkInterruptStates to allow developers to sanity check their configuration. --- Adafruit_BNO055.cpp | 119 ++++++++++++++++++++++++++++++++++++++------ Adafruit_BNO055.h | 11 +++- 2 files changed, 113 insertions(+), 17 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 01a2ec6..9931501 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -791,24 +791,82 @@ bool Adafruit_BNO055::enableInterruptAxes( adafruit_bno055_intr_en_t int_en_code @brief Retrieve interrupt states and settings - DEBUG FUNCTION */ /**************************************************************************/ -char * Adafruit_BNO055::checkInterruptStates() +void Adafruit_BNO055::checkInterruptStates( ) { + // enter config mode + adafruit_bno055_opmode_t modeback = _mode; + setMode(OPERATION_MODE_CONFIG); + delay(25); // check the interrupt status register - - // check the enable registers - + uint8_t interrupt_status = readIntStatus(); + // save selected page ID and switch to page 1 + uint8_t savePageID = read8(BNO055_PAGE_ID_ADDR); + write8(BNO055_PAGE_ID_ADDR, 0x01); + // check all config registers + int8_t accel_config = read8(BNO055_ACCEL_CONFIG); + int8_t magneto_config = read8(BNO055_MAG_CONFIG); + int8_t gyro_config0 = read8(BNO055_GYRO_CONFIG0); + int8_t gyro_config1 = read8(BNO055_GYRO_CONFIG1); + // check the interrupt enable registers + int8_t interrupt_enables = read8(BNO055_INTR_EN_ADDR); // check the interrupt mask - - // check the axis enable states - + int8_t interrupt_mask = read8(BNO055_INTR_MSK_ADDR); + // check the accel axis enable states + int8_t accel_axis_enables = read8(BNO055_INTR_ACCEL_SETT); + // check the gyro axis enable states + int8_t gyro_axis_enables = read8(BNO055_INTR_GYR_SETT); // check the threshold levels - + int8_t accel_nm_thresh = read8(BNO055_INTR_ACCEL_NM_THRES); + int8_t accel_am_thresh = read8(BNO055_INTR_ACCEL_AM_THRES); + int8_t accel_hg_thresh = read8(BNO055_INTR_ACCEL_HG_THRES); + int8_t gyro_am_thresh = read8(BNO055_INTR_GYR_AM_THRES); // check the duration levels - + int8_t accel_nm_dur = read8(BNO055_INTR_ACCEL_NM_SETT); + int8_t accel_hg_dur = read8(BNO055_INTR_ACCEL_HG_DUR); + int8_t gyro_x_dur = read8(BNO055_INTR_GYR_DUR_X); + int8_t gyro_y_dur = read8(BNO055_INTR_GYR_DUR_Y); + int8_t gyro_z_dur = read8(BNO055_INTR_GYR_DUR_Z); + int8_t gyro_am_set = read8(BNO055_INTR_GYR_AM_SET); // check the hysteresis settings - - // print settings as serial output - + int8_t gyro_x_hyst = read8(BNO055_INTR_GYR_HR_X_SET); + int8_t gyro_y_hyst = read8(BNO055_INTR_GYR_HR_Y_SET); + int8_t gyro_z_hyst = read8(BNO055_INTR_GYR_HR_Z_SET); + // restore page ID + write8(BNO055_PAGE_ID_ADDR, savePageID); + // Set the previous operating mode (see section 3.3) + setMode(modeback); + delay(20); + // print current call on interrupt status (clears status register) + Serial.print(" INTERRUPT_STA: "); printWithZeros(interrupt_status, 'e'); + // print all config registers + Serial.print(" ACCEL_CONFIG: "); printWithZeros(accel_config, 'e'); + Serial.print(" MAGNETO_CONFIG: "); printWithZeros(magneto_config, 'e'); + Serial.print(" GYRO_CONFIG0: "); printWithZeros(gyro_config0, 'e'); + Serial.print(" GYRO_CONFIG1: "); printWithZeros(gyro_config1, 'e'); + // print the interrupt enable registers + Serial.print(" INTERRUPT_ENABLES: "); printWithZeros(interrupt_enables, 'e'); + // print the interrupt mask + Serial.print(" INTERRUPT_MASK: "); printWithZeros(interrupt_mask, 'e'); + // print the accel axis enable states + Serial.print("ACCEL_AXIS_ENABLES: "); printWithZeros(accel_axis_enables, 'e'); + // print the gyro axis enable states + Serial.print(" GYRO_AXIS_ENABLES: "); printWithZeros(gyro_axis_enables, 'e'); + // print the threshold levels + Serial.print(" ACCEL_NM_THRESH: "); printWithZeros(accel_nm_thresh, 'e'); + Serial.print(" ACCEL_AM_THRESH: "); printWithZeros(accel_am_thresh, 'e'); + Serial.print(" ACCEL_HG_THRESH: "); printWithZeros(accel_hg_thresh, 'e'); + Serial.print(" GYRO_AM_THRESH: "); printWithZeros(gyro_am_thresh, 'e'); + // print the duration levels + Serial.print(" ACCEL_NM_DUR: "); printWithZeros(accel_nm_dur, 'e'); + Serial.print(" ACCEL_HG_DUR: "); printWithZeros(accel_hg_dur, 'e'); + Serial.print(" GYRO_X_DUR: "); printWithZeros(gyro_x_dur, 'e'); + Serial.print(" GYRO_Y_DUR: "); printWithZeros(gyro_y_dur, 'e'); + Serial.print(" GYRO_Z_DUR: "); printWithZeros(gyro_z_dur, 'e'); + Serial.print(" GYRO_AM_SET: "); printWithZeros(gyro_am_set, 'e'); + // print the hysteresis settings + Serial.print(" GYRO_X_HYST: "); printWithZeros(gyro_x_hyst, 'e'); + Serial.print(" GYRO_Y_HYST: "); printWithZeros(gyro_y_hyst, 'e'); + Serial.print(" GYRO_Z_HYST: "); printWithZeros(gyro_z_hyst, 'e'); } /**************************************************************************/ @@ -818,11 +876,15 @@ char * Adafruit_BNO055::checkInterruptStates() /**************************************************************************/ int8_t Adafruit_BNO055::readIntStatus() { - setMode(OPERATION_MODE_CONFIG); // change to config to change page + // enter config mode + adafruit_bno055_opmode_t modeback = _mode; + setMode(OPERATION_MODE_CONFIG); delay(25); - write8(BNO055_PAGE_ID_ADDR, 0); // set access to the first register page - delay(25); + // save selected page ID and switch to page 0 + uint8_t savePageID = read8(BNO055_PAGE_ID_ADDR); + write8(BNO055_PAGE_ID_ADDR, 0x00); + // read from INT_STA to determine the type of interrupt occurance int8_t intstatus = read8(BNO055_INTR_STAT_ADDR); @@ -834,6 +896,13 @@ int8_t Adafruit_BNO055::readIntStatus() 32 = Accelerometer any-motion interrupt 49 = Accelerometer no-motion/slow-motion interrupt */ + // restore page ID + write8(BNO055_PAGE_ID_ADDR, savePageID); + + // Set the requested operating mode (see section 3.3) + setMode(modeback); + delay(20); + return intstatus; } @@ -915,3 +984,23 @@ bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t /* ToDo: Check for errors! */ return true; } + +/**************************************************************************/ +/*! + @brief Prints 8-bit register value with leading zeros +*/ +/**************************************************************************/ +void Adafruit_BNO055::printWithZeros ( uint8_t input, char flag ) +{ + for (uint8_t mask = 0x80; mask; mask >>= 1) { + if (mask & input) { + Serial.print('1'); + } + else { + Serial.print('0'); + } + } + if (flag == 'e'){ + Serial.print('\n'); + } +} diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 38ff99d..0be8ef6 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -177,6 +177,12 @@ class Adafruit_BNO055 : public Adafruit_Sensor BNO055_INTR_GYR_AM_THRES = 0x1E, BNO055_INTR_GYR_AM_SET = 0x1F, + /* Sensor config registers */ + BNO055_ACCEL_CONFIG = 0x08, + BNO055_MAG_CONFIG = 0x09, + BNO055_GYRO_CONFIG0 = 0x0A, + BNO055_GYRO_CONFIG1 = 0x0B, + /* Axis remap registers */ BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, BNO055_AXIS_MAP_SIGN_ADDR = 0X42, @@ -325,7 +331,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal ); - char * checkInterruptStates( void ); + void checkInterruptStates( void ); void getSystemStatus ( uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error); @@ -335,7 +341,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor imu::Vector<3> getVector ( adafruit_vector_type_t vector_type ); imu::Quaternion getQuat ( void ); int8_t getTemp ( void ); - int8_t readIntStatus ( void ); + int8_t readIntStatus ( void ); /* Adafruit_Sensor implementation */ bool getEvent ( sensors_event_t* ); @@ -352,6 +358,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor byte read8 ( adafruit_bno055_reg_t ); bool readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len ); int8_t write8 ( adafruit_bno055_reg_t, byte value ); + void printWithZeros ( uint8_t input, char flag ); uint8_t _address; int32_t _sensorID; From 7e3f2aa9c241695b84c720c55779e99ee2d744bb Mon Sep 17 00:00:00 2001 From: kAr0sh1 Date: Thu, 16 Mar 2017 16:38:24 +0100 Subject: [PATCH 13/13] Halfway through threshold setting command that outputs the real value of the threshold set. --- Adafruit_BNO055.cpp | 145 ++++++++++++++++++++++++++++++++++++++++++-- Adafruit_BNO055.h | 1 + 2 files changed, 140 insertions(+), 6 deletions(-) diff --git a/Adafruit_BNO055.cpp b/Adafruit_BNO055.cpp index 9931501..cce8815 100644 --- a/Adafruit_BNO055.cpp +++ b/Adafruit_BNO055.cpp @@ -632,7 +632,7 @@ bool Adafruit_BNO055::enableInterrupts( adafruit_bno055_intr_en_t int_en_code, b status = write8(BNO055_INTR_MSK_ADDR, (read8(BNO055_INTR_MSK_ADDR)) | (0 << 7)); // enable pin change } write8(BNO055_INTR_EN_ADDR, (read8(BNO055_INTR_EN_ADDR)) | (1 << 7)); // enable interrupt - write8(BNO055_INTR_ACCEL_NM_SETT, (read8(BNO055_INTR_ACCEL_NM_SETT)) | (0 << 0)); // decides SM/NM + write8(BNO055_INTR_ACCEL_NM_SETT, (read8(BNO055_INTR_ACCEL_NM_SETT)) | (1 << 0)); // decides SM/NM break; case ACC_HIGH_G: if(triggerPin){ @@ -697,15 +697,15 @@ bool Adafruit_BNO055::enableInterruptAxes( adafruit_bno055_intr_en_t int_en_code // parse axis flags int X_EN = 0; int Y_EN = 0; int Z_EN = 0; // set flags based on input String - if (axes.indexOf("x") == 0){ + if (axes.indexOf("x") != -1){ X_EN = 1; Serial.println("X axis trigger enabled."); } - if (axes.indexOf("y") == 0){ + if (axes.indexOf("y") != -1){ Y_EN = 1; Serial.println("Y axis trigger enabled."); } - if (axes.indexOf("z") == 0){ + if (axes.indexOf("z") != -1){ Z_EN = 1; Serial.println("Z axis trigger enabled."); } @@ -734,7 +734,15 @@ bool Adafruit_BNO055::enableInterruptAxes( adafruit_bno055_intr_en_t int_en_code } break; case ACC_SM: - + if(X_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 2)); + } + if(Y_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 3)); + } + if(Z_EN){ + status = write8(BNO055_INTR_ACCEL_SETT, (read8(BNO055_INTR_ACCEL_SETT)) | (1 << 4)); + } break; case ACC_HIGH_G: if(X_EN){ @@ -791,7 +799,7 @@ bool Adafruit_BNO055::enableInterruptAxes( adafruit_bno055_intr_en_t int_en_code @brief Retrieve interrupt states and settings - DEBUG FUNCTION */ /**************************************************************************/ -void Adafruit_BNO055::checkInterruptStates( ) +void Adafruit_BNO055::checkInterruptStates( void ) { // enter config mode adafruit_bno055_opmode_t modeback = _mode; @@ -869,6 +877,131 @@ void Adafruit_BNO055::checkInterruptStates( ) Serial.print(" GYRO_Z_HYST: "); printWithZeros(gyro_z_hyst, 'e'); } +/**************************************************************************/ +/*! +@brief setIntThreshold +*/ +/**************************************************************************/ +bool Adafruit_BNO055::setIntThreshold( adafruit_bno055_intr_en_t int_en_code, int duration ) +{ + // TODO: create different code set to group similar processes and increase code reuse + + // create status variable + int8_t status = 0; // if greater than zero a failure has occured + + // enter config mode + adafruit_bno055_opmode_t modeback = _mode; + setMode(OPERATION_MODE_CONFIG); + delay(25); + + // save selected page ID and switch to page 1 + uint8_t savePageID = read8(BNO055_PAGE_ID_ADDR); + write8(BNO055_PAGE_ID_ADDR, 0x01); + + switch(int_en_code){ + case ACC_NM: + // check duration range is 8-bit + if(duration > 0 && duration < 256){ + // mask register value to just the first two bits + uint8_t masked_config = 0b00000011 & read8(BNO055_ACCEL_CONFIG); + // write to the register + status = write8(BNO055_INTR_ACCEL_NM_THRES, duration); + // display real value + float thresh_mg = 0.0; + if (masked_config == 0){ + // threshold is in 2g range, 3.91mg per bit + thresh_mg = duration * 3.91; + } else if (masked_config == 1){ + // threshold is in 4g range, 7.81mg per bit + thresh_mg = duration * 7.81; + } else if (masked_config == 2){ + // threshold is in 8g range, 15.63mg per bit + thresh_mg = duration * 15.63; + } else if (masked_config == 3){ + // threshold is in 16g range, 31.25mg per bit + thresh_mg = duration * 31.25; + } else { + Serial.println("Threshhold not set!"); + } + delay(500); + Serial.print("NM threshold set to: "); Serial.print(thresh_mg); + Serial.println("mg."); + } else { + // display error + Serial.println("Duration value not within 1 - 255 range!"); + } + break; + case ACC_AM: + // check duration range is 8-bit + if(duration > 0 && duration < 256){ + // mask register value to just the first two bits + uint8_t masked_config = 0b00000011 & read8(BNO055_ACCEL_CONFIG); + // write to the register + status = write8(BNO055_INTR_ACCEL_AM_THRES, duration); + // display real value + float thresh_mg = 0.0; + if (masked_config == 0){ + // threshold is in 2g range, 3.91mg per bit + thresh_mg = duration * 3.91; + } else if (masked_config == 1){ + // threshold is in 4g range, 7.81mg per bit + thresh_mg = duration * 7.81; + } else if (masked_config == 2){ + // threshold is in 8g range, 15.63mg per bit + thresh_mg = duration * 15.63; + } else if (masked_config == 3){ + // threshold is in 16g range, 31.25mg per bit + thresh_mg = duration * 31.25; + } else { + Serial.println("Threshhold not set!"); + } + delay(500); + Serial.print("AM threshold set to: "); Serial.print(thresh_mg); + Serial.println("mg."); + } else { + // display error + Serial.println("Duration value not within 1 - 255 range!"); + } + break; + case ACC_SM: + if(duration > 0 && duration < 256){ + // write to the register + status = write8(BNO055_INTR_ACCEL_NM_THRES, duration); + } else { + // display error + Serial.println("Duration value not within 1 - 255 range!"); + } + break; + case ACC_HIGH_G: + if(duration > 0 && duration < 256){ + // write to the register + status = write8(BNO055_INTR_ACCEL_NM_THRES, duration); + } else { + // display error + Serial.println("Duration value not within 1 - 255 range!"); + } + break; + case GYR_HIGH_RATE: + + break; + case GYRO_AM: + + break; + } + + // restore page ID + write8(BNO055_PAGE_ID_ADDR, savePageID); + // Set the previous operating mode (see section 3.3) + setMode(modeback); + delay(20); + + if (status > 0){ + return false; + } else { + return true; + } +} + /**************************************************************************/ /*! @brief On interrupt reads the interrupt status register diff --git a/Adafruit_BNO055.h b/Adafruit_BNO055.h index 0be8ef6..c1830eb 100644 --- a/Adafruit_BNO055.h +++ b/Adafruit_BNO055.h @@ -328,6 +328,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign ); bool enableInterrupts ( adafruit_bno055_intr_en_t int_en_code, bool triggerPin); bool enableInterruptAxes ( adafruit_bno055_intr_en_t int_en_code, String axes ); + bool setIntThreshold ( adafruit_bno055_intr_en_t int_en_code, int duration ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); void setExtCrystalUse ( boolean usextal );