Write & Read BNO055 Calibration and heel/pitch offsets to/from EEPROM

.gitignore

@@ -33,6 +33,9 @@ Network Trash Folder
 Temporary Items
 .apdisk
 
+# Netbeans project folder
+nbproject
+
 # Platformio likes to auto-create this
 lib/readme.txt
 CMakeListsPrivate.txt

extras/config/default-config.json

@@ -1,7 +1,10 @@
 {
   "imu": {
     "enabled": true,
-    "frequency": 20
+    "frequency": 20,
+    "enableHdg": true,
+    "enableHeelPitch": true,
+    "mounting": "verticalPortHull"
   },
   "barometer": {
     "enabled": true,
@@ -49,4 +52,3 @@
     "rxEnabled": true
   }
 }
-

lib/Adafruit_BNO055/Adafruit_BNO055.cpp

@@ -53,16 +53,14 @@ Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
  PUBLIC FUNCTIONS
  ***************************************************************************/
 
+
 /**************************************************************************/
 /*!
-    @brief  Sets up the HW
+    Sets up the IMU Sensor for KBox, including axis- and sign mapping
 */
 /**************************************************************************/
-bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
+bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode, uint8_t axis_remap_orientation, uint8_t axis_remap_sign)
 {
-  /* Enable I2C */
-  //Wire.begin();
-
   /* Make sure we have the right device */
   uint8_t id = read8(BNO055_CHIP_ID_ADDR);
   if(id != BNO055_ID)
@@ -90,8 +88,7 @@ bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
     //delay(100);
   //}
   //delay(50);
-
-  DEBUG("rebooted");
+  //DEBUG("rebooted");
 
   /* Set to normal power mode */
   DEBUG("going into normal power mode");
@@ -111,14 +108,12 @@ bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
   write8(BNO055_UNIT_SEL_ADDR, unitsel);
   */
 
-  /* Configure axis mapping (see section 3.4) */
-  //write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
-  write8(BNO055_AXIS_MAP_CONFIG_ADDR, 0b00001001); // P0-P7, Default is P1
+  /* Configure axis mapping (see section 3.4 Bosch manual) */
+  write8(BNO055_AXIS_MAP_CONFIG_ADDR, axis_remap_orientation);
   delay(10);
-  //write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
-  write8(BNO055_AXIS_MAP_SIGN_ADDR, 0b00000000); // P0-P7, Default is P1
+  write8(BNO055_AXIS_MAP_SIGN_ADDR, axis_remap_sign);
   delay(10);
-  
+
   write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
   delay(10);
   /* Set the requested operating mode (see section 3.3) */
@@ -612,7 +607,7 @@ byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
   #else
     value = Wire.receive();
   #endif
-    
+
   //DEBUG("read => %x", value);
 
   return value;

lib/Adafruit_BNO055/Adafruit_BNO055.h

@@ -280,8 +280,11 @@ class Adafruit_BNO055 : public Adafruit_Sensor
     } adafruit_vector_type_t;
 
     Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A );
+    // MOD for KBox to set axis and sign
+    bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF,
+                                  uint8_t axis_remap_orientation = REMAP_CONFIG_P1,
+                                  uint8_t axis_remap_sign = REMAP_SIGN_P1);
 
-    bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
     void  setMode             ( adafruit_bno055_opmode_t mode );
     void  getRevInfo          ( adafruit_bno055_rev_info_t* );
     void  displayRevInfo      ( void );

lib/KBoxHardware/src/KBoxHardware.cpp

@@ -147,7 +147,6 @@ void KBoxHardware::espRebootInProgram() {
 }
 
 bool KBoxHardware::sdCardInit() {
-  //TODO: check if sdcard_cs is working with SdFatSdio
   #if defined(__MK66FX1M0__)
     // SDIO support for Builtin SD-Card in Teensy 3.6
     if (!_sd.begin()){

src/common/ui/Event.h

@@ -40,8 +40,16 @@ class Event {
 };
 
 enum ButtonEventType {
+  // Those two events are always sent when button goes up/down
   ButtonEventTypePressed,
-  ButtonEventTypeReleased
+  ButtonEventTypeReleased,
+
+  // Those events are also sent when a single click, or a double click happen
+  ButtonEventTypeClick,
+  ButtonEventTypeLongClick,
+
+  // This event is sent multiple times, as long as button is maintained
+  ButtonEventTypeMaintained,
 };
 class ButtonEvent : public Event {
   private:
@@ -68,5 +76,3 @@ class TickEvent : public Event {
     TickEvent(unsigned long int millis) : Event(EventTypeTick), millis(millis) {};
     time_ms_t getMillis() const { return millis; };
 };
-
-

src/common/ui/Page.h

@@ -29,6 +29,8 @@
 #include "Event.h"
 #include "Layer.h"
 
+#include <KBoxLogging.h>
+
 /* A page is one simple app available on the MFD.
  *
  * - Only one page can be active at a time.
@@ -44,7 +46,9 @@ class BasePage {
 
     virtual bool processEvent(const ButtonEvent &e) {
       // By default button down will force switching to the next page.
-      return !(e.clickType == ButtonEventTypePressed);
+      DEBUG("EventTypeButton: %i", e.clickType);
+      // false -> next page
+      return !(e.clickType == ButtonEventTypeClick);
     }
     virtual bool processEvent(const EncoderEvent &e) {
       // By default this will be ignored.
@@ -59,7 +63,7 @@ class BasePage {
     virtual ~BasePage() {};
 };
 
-/* BasePage offers a very generic paint implementation. 
+/* BasePage offers a very generic paint implementation.
  * Page assumes the use of Layers.
  *
  * Adding a layer via addLayer() will transfer ownership to the Page who will
@@ -76,4 +80,3 @@ class Page : public BasePage {
     void addLayer(Layer *l);
     void paint(GC &context);
 };
-

src/host/config/IMUConfig.h

@@ -30,8 +30,19 @@
 
 #pragma once
 
+enum IMUMounting {
+  VerticalStbHull,
+  VerticalPortHull,
+  VerticalTopToBow,
+  //VerticalTopToStern,
+  HorizontalLeftSideToBow,
+  //HorizontalRightSideToBow
+};
+
 struct IMUConfig {
   bool enabled;
   int frequency;
+  bool enableHdg;
+  bool enableHeelPitch;
+  enum IMUMounting mounting = VerticalPortHull;
 };
-

src/host/config/KBoxConfigParser.cpp

@@ -54,6 +54,10 @@ void KBoxConfigParser::defaultConfig(KBoxConfig &config) {
 
   config.imuConfig.enabled = true;
   config.imuConfig.frequency = 20;
+  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.barometerConfig.enabled = true;
   config.barometerConfig.frequency = 1;
@@ -75,6 +79,9 @@ void KBoxConfigParser::parseKBoxConfig(const JsonObject &json, KBoxConfig &confi
 void KBoxConfigParser::parseIMUConfig(const JsonObject &json, IMUConfig &config) {
   READ_BOOL_VALUE(enabled);
   READ_INT_VALUE_WRANGE(frequency, 1, 100);
+  READ_BOOL_VALUE(enableHdg);
+  READ_BOOL_VALUE(enableHeelPitch);
+  READ_ENUM_VALUE(mounting, convertIMUMounting);
 }
 
 void KBoxConfigParser::parseBarometerConfig(const JsonObject &json, BarometerConfig &config){
@@ -131,4 +138,21 @@ enum SerialMode KBoxConfigParser::convertSerialMode(const String &s) {
     return SerialModeNMEA;
   }
   return SerialModeDisabled;
-}
+}
+
+enum IMUMounting KBoxConfigParser::convertIMUMounting(const String &s) {
+  if (s == "verticalPortHull") {
+    return VerticalPortHull;
+  }
+  if (s == "verticalStarboardHull") {
+    return VerticalStbHull;
+  }
+  if (s == "verticalTopToBow") {
+    return VerticalTopToBow;
+  }
+  if (s == "horizontalLeftSideToBow") {
+    return HorizontalLeftSideToBow;
+  }
+  // default
+  return VerticalPortHull;
+}

src/host/config/KBoxConfigParser.h

@@ -41,6 +41,7 @@
 class KBoxConfigParser {
   private:
     SerialMode convertSerialMode(const String &s);
+    IMUMounting convertIMUMounting(const String &s);
 
   public:
     /**

src/host/main.cpp

@@ -34,6 +34,7 @@
 #include "host/services/ADCService.h"
 #include "host/services/BarometerService.h"
 #include "host/services/IMUService.h"
+#include "host/pages/IMUMonitorPage.h"
 #include "host/services/NMEA2000Service.h"
 #include "host/services/SerialService.h"
 #include "host/services/RunningLightService.h"
@@ -98,7 +99,7 @@ void setup() {
 
   ADCService *adcService = new ADCService(skHub, KBox.getADC());
   BarometerService *baroService = new BarometerService(skHub);
-  IMUService *imuService = new IMUService(skHub);
+  IMUService *imuService = new IMUService(config.imuConfig, skHub);
 
   NMEA2000Service *n2kService = new NMEA2000Service(config.nmea2000Config,
                                                     skHub);
@@ -134,6 +135,12 @@ void setup() {
   BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub);
   mfd.addPage(batPage);
 
+  if (config.imuConfig.enabled) {
+    // At the moment the IMUMonitorPage is working with built-in sensor only
+    IMUMonitorPage *imuPage = new IMUMonitorPage(config.imuConfig, skHub, *imuService);
+    mfd.addPage(imuPage);
+  }
+
   StatsPage *statsPage = new StatsPage();
   statsPage->setSDCardTask(sdcardTask);
 

src/host/pages/IMUMonitorPage.cpp

@@ -0,0 +1,99 @@
+/*
+     __  __     ______     ______     __  __
+    /\ \/ /    /\  == \   /\  __ \   /\_\_\_\
+    \ \  _"-.  \ \  __<   \ \ \/\ \  \/_/\_\/_
+     \ \_\ \_\  \ \_____\  \ \_____\   /\_\/\_\
+       \/_/\/_/   \/_____/   \/_____/   \/_/\/_/
+
+  The MIT License
+
+  Copyright (c) 2018 Thomas Sarlandie [email protected]
+  Copyright (c) 2018 Ronnie Zeiller [email protected]
+
+  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 "IMUMonitorPage.h"
+
+#include "common/signalk/SKUpdate.h"
+#include "common/signalk/SKUnits.h"
+
+IMUMonitorPage::IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService) : _config(config), _imuService(imuService) {
+  static const int col1 = 5;
+  static const int col2 = 200;
+  static const int row1 = 26;
+  static const int row2 = 50;
+  static const int row3 = 152;
+  static const int row4 = 182;
+
+  addLayer(new TextLayer(Point(col1, row1), Size(20, 20), "HDG ° Mag", ColorWhite, ColorBlack, FontDefault));
+  addLayer(new TextLayer(Point(col2, row1), Size(20, 20), "Sys/Mag/Acc", ColorWhite, ColorBlack, FontDefault));
+  addLayer(new TextLayer(Point(col1, row3), Size(20, 20), "Heel °", ColorWhite, ColorBlack, FontDefault));
+  addLayer(new TextLayer(Point(col2, row3), Size(20, 20), "Pitch °", ColorWhite, ColorBlack, FontDefault));
+
+  _hdgTL = new TextLayer(Point(col1, row2), Size(20, 20), "--", ColorWhite, ColorBlack, FontLarge);
+  _calTL = new TextLayer(Point(col2, row2), Size(20, 20), "--", ColorWhite, ColorBlack, FontLarge);
+  _rollTL  = new TextLayer(Point(col1, row4), Size(20, 20), "--", ColorWhite, ColorBlack, FontLarge);
+  _pitchTL = new TextLayer(Point(col2, row4), Size(20, 20), "--", ColorWhite, ColorBlack, FontLarge);
+
+  addLayer(_hdgTL);
+  addLayer(_calTL);
+  addLayer(_rollTL);
+  addLayer(_pitchTL);
+
+  hub.subscribe(this);
+}
+
+bool IMUMonitorPage::processEvent(const ButtonEvent &be){
+  if (be.clickType == ButtonEventTypeClick) {
+    // Change page on single click.
+    return false;
+  }
+  if (be.clickType == ButtonEventTypeLongClick) {
+    _imuService.setRollPitchOffset();
+  }
+  return true;
+}
+
+bool IMUMonitorPage::processEvent(const TickEvent &te){
+  _imuService.getLastValues(_sysCalibration, _accelCalibration, _pitch, _roll, _magCalibration, _heading);
+
+  // TODO: Some damping for the display
+  _hdgTL->setText(String( SKRadToDeg(_heading), 1) + "°        ");
+  _calTL->setText(String( _sysCalibration) + "/" + String( _magCalibration) + "/" + String( _accelCalibration) + "   ");
+  _pitchTL->setText(String( SKRadToDeg(_pitch), 1) + "°     ");
+  _rollTL->setText(String( SKRadToDeg(_roll), 1) + "°     ");
+
+  // Always show Hdg from IMU-sensor, but if the value is not trusted (which means
+  // calibrationData below default setting, change color to red
+  if ( ! _imuService.isMagCalibrated() || ! _imuService.isRollAndPitchCalibrated()) {
+    _hdgTL->setColor(ColorRed);
+    _calTL->setColor(ColorRed);
+  } else {
+    _hdgTL->setColor(ColorWhite);
+    _calTL->setColor(ColorWhite);
+  };
+
+  return true;
+}
+
+void IMUMonitorPage::updateReceived(const SKUpdate& up) {
+  // may be needed for something....
+}