IMUMonitorPage added

.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,11 @@
 {
   "imu": {
     "enabled": true,
-    "frequency": 20
+    "frequency": 20,
+    "enableHdg": true,
+    "enableHeelPitch": true,
+    "calHdg": 3,
+    "calHeelPitch": 2
   },
   "barometer": {
     "enabled": true,
@@ -49,4 +53,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/signalk/SKUnits.h

@@ -57,4 +57,4 @@ inline double SKNormalizeAngle(double x) {
   return x - M_PI;
 }
 
-
+const double SKDoubleNAN = -1e9;

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

@@ -33,5 +33,8 @@
 struct IMUConfig {
   bool enabled;
   int frequency;
+  bool enableHdg;
+  bool enableHeelPitch;
+  int  calHdg;
+  int  calHeelPitch;
 };
-

src/host/config/KBoxConfigParser.cpp

@@ -54,6 +54,11 @@ 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.calHdg = 3;               // hdg valid, if calibration value greater equal
+  config.imuConfig.calHeelPitch = 2;         // heel (roll) & pitch valid, if calibration value greater equal
 
   config.barometerConfig.enabled = true;
   config.barometerConfig.frequency = 1;
@@ -75,6 +80,10 @@ 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_INT_VALUE_WRANGE(calHdg, 0, 3);
+  READ_INT_VALUE_WRANGE(calHeelPitch, 0, 3);
 }
 
 void KBoxConfigParser::parseBarometerConfig(const JsonObject &json, BarometerConfig &config){
@@ -131,4 +140,4 @@ enum SerialMode KBoxConfigParser::convertSerialMode(const String &s) {
     return SerialModeNMEA;
   }
   return SerialModeDisabled;
-}
+}

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,102 @@
+/*
+     __  __     ______     ______     __  __
+    /\ \/ /    /\  == \   /\  __ \   /\_\_\_\
+    \ \  _"-.  \ \  __<   \ \ \/\ \  \/_/\_\/_
+     \ \_\ \_\  \ \_____\  \ \_____\   /\_\/\_\
+       \/_/\/_/   \/_____/   \/_____/   \/_/\/_/
+
+  The MIT License
+
+  Copyright (c) 2017 Thomas Sarlandie [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 <stdio.h>
+#include "common/signalk/SKUpdate.h"
+#include "IMUMonitorPage.h"
+#include <KBoxLogging.h>
+#include "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), "Cal: 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){
+  // DEBUG("EventTypeButton: %i", be.clickType);
+  if (be.clickType == ButtonEventTypeClick) {
+    DEBUG("Button ButtonEventTypeClick !!!");
+    // Change page on single click.
+    return false;
+  }
+  if (be.clickType == ButtonEventTypeLongClick) {
+    _imuService.setOffset();
+  }
+  return true;
+}
+
+bool IMUMonitorPage::processEvent(const TickEvent &te){
+  _imuService.getLastValues(_accelCalibration, _pitch, _roll, _magCalibration, _heading);
+  //DEBUG("AccelCalibration: %i | MagCalibration: %i", _accelCalibration, _magCalibration);
+
+  // TODO: Some damping for the display
+  _hdgTL->setText(String( SKRadToDeg(_heading), 1) + "°        ");
+  _calTL->setText(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
+  // calibration below config setting, change color to red
+	if ((_magCalibration <= _config.calHdg)||(_accelCalibration <= _config.calHeelPitch)) {
+    _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....
+}

src/host/pages/IMUMonitorPage.h

@@ -0,0 +1,53 @@
+/*
+     __  __     ______     ______     __  __
+    /\ \/ /    /\  == \   /\  __ \   /\_\_\_\
+    \ \  _"-.  \ \  __<   \ \ \/\ \  \/_/\_\/_
+     \ \_\ \_\  \ \_____\  \ \_____\   /\_\/\_\
+       \/_/\/_/   \/_____/   \/_____/   \/_/\/_/
+
+  The MIT License
+
+  Copyright (c) 2017 Thomas Sarlandie [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 "common/ui/Page.h"
+#include "common/ui/TextLayer.h"
+#include "common/signalk/SKHub.h"
+#include "common/signalk/SKSubscriber.h"
+#include "services/IMUService.h"
+#include "host/config/IMUConfig.h"
+
+class IMUMonitorPage : public Page, public SKSubscriber {
+	private:
+		TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL;
+		IMUConfig &_config;
+		IMUService &_imuService;
+
+		int _magCalibration, _accelCalibration;
+		double _pitch, _roll, _heading;
+
+	public:
+		IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService);
+		virtual void updateReceived(const SKUpdate& up);
+
+		bool processEvent(const TickEvent &te);
+		bool processEvent(const ButtonEvent &be);
+};