Adafruit bno055 update + added position configs

.gitignore

@@ -42,3 +42,6 @@ CMakeListsPrivate.txt
 
 # CLion
 cmake-build-debug
+
+# individual user's config file
+extras/config/kbox-config.json

extras/config/default-config.json

@@ -4,7 +4,7 @@
     "frequency": 20,
     "enableHdg": true,
     "enableHeelPitch": true,
-    "mounting": "verticalPortHull"
+    "mounting": "verticalRightSideToBow"
   },
   "barometer": {
     "enabled": true,

lib/Adafruit_BNO055/Adafruit_BNO055.cpp

@@ -442,19 +442,35 @@ bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
         setMode(OPERATION_MODE_CONFIG);
         delay(25);
 
+        /* Accel offset range depends on the G-range:
+           +/-2g  = +/- 2000 mg
+           +/-4g  = +/- 4000 mg
+           +/-8g  = +/- 8000 mg
+           +/-1§g = +/- 16000 mg */
         offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
         offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
         offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
 
-        offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
-        offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
-        offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
-
+        /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
         offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
         offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
         offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
 
+        /* Gyro offset range depends on the DPS range:
+          2000 dps = +/- 32000 LSB
+          1000 dps = +/- 16000 LSB
+           500 dps = +/- 8000 LSB
+           250 dps = +/- 4000 LSB
+           125 dps = +/- 2000 LSB
+           ... where 1 DPS = 16 LSB */
+        offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
+        offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
+        offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
+
+        /* Accelerometer radius = +/- 1000 LSB */
         offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
+
+        /* Magnetometer radius = +/- 960 LSB */
         offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
 
         setMode(lastMode);
@@ -475,6 +491,11 @@ void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData)
     setMode(OPERATION_MODE_CONFIG);
     delay(25);
 
+    /* Note: Configuration will take place only when user writes to the last
+       byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
+       Therefore the last byte must be written whenever the user wants to
+       changes the configuration. */
+
     /* A writeLen() would make this much cleaner */
     write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
     write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
@@ -483,19 +504,19 @@ void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData)
     write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
     write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
 
-    write8(GYRO_OFFSET_X_LSB_ADDR, calibData[6]);
-    write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]);
-    write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]);
-    write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]);
-    write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]);
-    write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]);
+    write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
+    write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
+    write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
+    write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
+    write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
+    write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
 
-    write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]);
-    write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]);
-    write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]);
-    write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]);
-    write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]);
-    write8(MAG_OFFSET_Z_MSB_ADDR, calibData[17]);
+    write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
+    write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
+    write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
+    write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
+    write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
+    write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
 
     write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
     write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
@@ -517,27 +538,32 @@ void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_
     setMode(OPERATION_MODE_CONFIG);
     delay(25);
 
+    /* Note: Configuration will take place only when user writes to the last
+       byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
+       Therefore the last byte must be written whenever the user wants to
+       changes the configuration. */
+
     write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
     write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
     write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
     write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
     write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
     write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
 
-    write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
-    write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
-    write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
-    write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
-    write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
-    write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
-
     write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
     write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
     write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
     write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
     write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
     write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
 
+    write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
+    write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
+    write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
+    write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
+    write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
+    write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
+
     write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
     write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
 
@@ -547,6 +573,11 @@ void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_
     setMode(lastMode);
 }
 
+/**************************************************************************/
+/*!
+    @brief  Checks of all cal status values are set to 3 (fully calibrated)
+*/
+/**************************************************************************/
 bool Adafruit_BNO055::isFullyCalibrated(void)
 {
     uint8_t system, gyro, accel, mag;

lib/Adafruit_BNO055/Adafruit_BNO055.h

@@ -44,18 +44,18 @@
 
 typedef struct
 {
-    uint16_t accel_offset_x;
-    uint16_t accel_offset_y;
-    uint16_t accel_offset_z;
-    uint16_t gyro_offset_x;
-    uint16_t gyro_offset_y;
-    uint16_t gyro_offset_z;
-    uint16_t mag_offset_x;
-    uint16_t mag_offset_y;
-    uint16_t mag_offset_z;
-
-    uint16_t accel_radius;
-    uint16_t mag_radius;
+    int16_t accel_offset_x;
+    int16_t accel_offset_y;
+    int16_t accel_offset_z;
+    int16_t mag_offset_x;
+    int16_t mag_offset_y;
+    int16_t mag_offset_z;
+    int16_t gyro_offset_x;
+    int16_t gyro_offset_y;
+    int16_t gyro_offset_z;
+
+    int16_t accel_radius;
+    int16_t mag_radius;
 } adafruit_bno055_offsets_t;
 
 class Adafruit_BNO055 : public Adafruit_Sensor

lib/Adafruit_BNO055/library.properties

@@ -1,10 +1,9 @@
 name=Adafruit BNO055
-version=1.0.6
+version=1.1.6
 author=Adafruit <[email protected]>
 maintainer=Adafruit <[email protected]>
 sentence=Library for the Adafruit BNO055 Absolute Orientation Sensor.
 paragraph=Designed specifically to work with the Adafruit BNO055 Breakout, and is based on Adafruit's Unified Sensor Library.
 category=Sensors
 url=https://github.com/adafruit/Adafruit_BNO055
 architectures=*
-

platformio.ini

@@ -31,7 +31,7 @@ build_flags =
     -DSERIAL3_RX_BUFFER_SIZE=256 -DSERIAL3_TX_BUFFER_SIZE=256
 platform = teensy
 framework = arduino
-board = teensy31
+board = teensy36
 lib_deps =
   ${common.lib_deps_common}
   ${common.lib_deps_teensy}

src/host/config/IMUConfig.h

@@ -27,22 +27,37 @@
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
 */
+/*          horizontal top (KBox or sensor)
+              __________________
+             / o               /|
+            /                 / /
+Left Side  /     BNO055      / /   Right Side
+          /  vertical top   / /
+         /_________________/ /
+         |________________ |/
+
+       Bottom (KBox or sensor)
+*/
 
 #pragma once
 
 enum IMUMounting {
-  VerticalStbHull,
-  VerticalPortHull,
-  VerticalTopToBow,
-  //VerticalTopToStern,
-  HorizontalLeftSideToBow,
-  //HorizontalRightSideToBow
+  verticalPortHull,           // KBox mounted on port hull
+  verticalStbHull,            // KBox mounted on stb. hull
+  verticalRightSideToBow,     // KBox mounted on port hull
+  verticalLeftSideToBow,      // KBox mounted on stb. hull
+  verticalTopToBow,           // KBox mounted on rear bulkhead
+  verticalBottomToBow,        // KBox mounted on front bulkhead,
+  horizontalTopToBow,
+  horizontalLeftSideToBow,
+  horizontalBottomToBow,
+  horizontalRightSideToBow,
 };
 
 struct IMUConfig {
   bool enabled;
   int frequency;
   bool enableHdg;
   bool enableHeelPitch;
-  enum IMUMounting mounting = VerticalPortHull;
+  enum IMUMounting mounting = verticalPortHull;
 };

src/host/config/KBoxConfigParser.cpp

@@ -75,7 +75,7 @@ void KBoxConfigParser::defaultConfig(KBoxConfig &config) {
   config.imuConfig.enabled = true;           // enable internal IMU sensor
   config.imuConfig.enableHdg = true;         // true if values taken from internal sensor
   config.imuConfig.enableHeelPitch = true;   // true if values taken from internal sensor
-  config.imuConfig.mounting = VerticalStbHull;
+  config.imuConfig.mounting = verticalPortHull;
 
   config.barometerConfig.enabled = true;
   config.barometerConfig.frequency = 1;
@@ -181,17 +181,35 @@ enum SerialMode KBoxConfigParser::convertSerialMode(const String &s) {
 
 enum IMUMounting KBoxConfigParser::convertIMUMounting(const String &s) {
   if (s == "verticalPortHull") {
-    return VerticalPortHull;
+    return  verticalPortHull;
   }
-  if (s == "verticalStarboardHull") {
-    return VerticalStbHull;
+  if (s == "verticalStbHull") {
+    return  verticalStbHull;
+  }
+  if (s == "verticalRightSideToBow") {
+    return  verticalRightSideToBow;
+  }
+  if (s == "verticalLeftSideToBow") {
+    return  verticalLeftSideToBow;
   }
   if (s == "verticalTopToBow") {
-    return VerticalTopToBow;
+    return  verticalTopToBow;
+  }
+  if (s == "verticalBottomToBow") {
+    return  verticalBottomToBow;
+  }
+  if (s == "horizontalTopToBow") {  // Bosch P0
+    return  horizontalTopToBow;
+  }
+  if (s == "horizontalLeftSideToBow") { // Bosch P1 (default)
+    return  horizontalLeftSideToBow;
+  }
+  if (s == "horizontalBottomToBow") { // Bosch P2
+    return  horizontalBottomToBow;
   }
-  if (s == "horizontalLeftSideToBow") {
-    return HorizontalLeftSideToBow;
+  if (s == "horizontalRightSideToBow") { // Bosch P3
+    return  horizontalRightSideToBow;
   }
   // default
-  return VerticalPortHull;
+  return verticalRightSideToBow;
 }

src/host/main.cpp

@@ -91,7 +91,7 @@ void setup() {
     JsonObject &root =jsonBuffer.parseObject(configFile);
 
     if (root.success()) {
-      DEBUG("Loading configuration from SDCard");
+      DEBUG("Loading configuration file %s from SDCard", configFilename);
       configParser.parseKBoxConfig(root, config);
     }
     else {

src/host/pages/IMUMonitorPage.cpp

@@ -83,13 +83,24 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){
 
   // Always show Hdg from IMU-sensor, but if the value is not trusted (which means
   // calibrationData below default setting, change color to red
+  // red color also means, that values are not sent as SKUpdate to SKHub
   if ( ! _imuService.isMagCalibrated() || ! _imuService.isRollAndPitchCalibrated()) {
-    _hdgTL->setColor(ColorRed);
     _calTL->setColor(ColorRed);
   } else {
-    _hdgTL->setColor(ColorWhite);
     _calTL->setColor(ColorWhite);
   };
+  if ( ! _imuService.isMagCalibrated() ) {
+    _hdgTL->setColor(ColorRed);
+  } else {
+    _hdgTL->setColor(ColorWhite);
+  };
+  if ( ! _imuService.isRollAndPitchCalibrated()) {
+    _pitchTL->setColor(ColorRed);
+    _rollTL->setColor(ColorRed);
+  } else {
+    _pitchTL->setColor(ColorWhite);
+    _rollTL->setColor(ColorWhite);
+  };
 
   return true;
 }