diff --git a/.gitignore b/.gitignore index 243b377a..c7943b04 100644 --- a/.gitignore +++ b/.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 diff --git a/extras/config/default-config.json b/extras/config/default-config.json index 5fc29c11..92c9a2bb 100644 --- a/extras/config/default-config.json +++ b/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 } } - diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp index 9e99e71b..d84d682e 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp +++ b/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; diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.h b/lib/Adafruit_BNO055/Adafruit_BNO055.h index 5516d426..38e754f4 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.h +++ b/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 ); diff --git a/lib/KBoxHardware/src/KBoxHardware.cpp b/lib/KBoxHardware/src/KBoxHardware.cpp index 8e55bc5c..1797cb7c 100644 --- a/lib/KBoxHardware/src/KBoxHardware.cpp +++ b/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()){ diff --git a/src/common/ui/Event.h b/src/common/ui/Event.h index bfeba511..9a7c6f8f 100644 --- a/src/common/ui/Event.h +++ b/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; }; }; - - diff --git a/src/common/ui/Page.h b/src/common/ui/Page.h index 64ff85ca..039d5518 100644 --- a/src/common/ui/Page.h +++ b/src/common/ui/Page.h @@ -29,6 +29,8 @@ #include "Event.h" #include "Layer.h" +#include + /* 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); }; - diff --git a/src/host/config/IMUConfig.h b/src/host/config/IMUConfig.h index 27b3833b..90d7e5ae 100644 --- a/src/host/config/IMUConfig.h +++ b/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; }; - diff --git a/src/host/config/KBoxConfigParser.cpp b/src/host/config/KBoxConfigParser.cpp index dd59c770..7b4cda66 100644 --- a/src/host/config/KBoxConfigParser.cpp +++ b/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; -} \ No newline at end of file +} + +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; +} diff --git a/src/host/config/KBoxConfigParser.h b/src/host/config/KBoxConfigParser.h index 045cb9f1..1dc12873 100644 --- a/src/host/config/KBoxConfigParser.h +++ b/src/host/config/KBoxConfigParser.h @@ -41,6 +41,7 @@ class KBoxConfigParser { private: SerialMode convertSerialMode(const String &s); + IMUMounting convertIMUMounting(const String &s); public: /** diff --git a/src/host/main.cpp b/src/host/main.cpp index f3232c33..d06d6121 100644 --- a/src/host/main.cpp +++ b/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); diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp new file mode 100644 index 00000000..4ca86e8b --- /dev/null +++ b/src/host/pages/IMUMonitorPage.cpp @@ -0,0 +1,99 @@ +/* + __ __ ______ ______ __ __ + /\ \/ / /\ == \ /\ __ \ /\_\_\_\ + \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ + \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ + \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ + + The MIT License + + Copyright (c) 2018 Thomas Sarlandie thomas@sarlandie.net + Copyright (c) 2018 Ronnie Zeiller ronnie@zeiller.eu + + 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.... +} diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h new file mode 100644 index 00000000..33efa2da --- /dev/null +++ b/src/host/pages/IMUMonitorPage.h @@ -0,0 +1,56 @@ +/* + __ __ ______ ______ __ __ + /\ \/ / /\ == \ /\ __ \ /\_\_\_\ + \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ + \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ + \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ + + The MIT License + + Copyright (c) 2018 Thomas Sarlandie thomas@sarlandie.net + Copyright (c) 2018 Ronnie Zeiller ronnie@zeiller.eu + + 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" +//TODO: damping and automatic damping according to service frequency +#include "host/config/IMUConfig.h" + +class IMUMonitorPage : public Page, public SKSubscriber { + private: + TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; + IMUConfig &_config; + IMUService &_imuService; + + int _magCalibration, _accelCalibration, _sysCalibration; + 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); +}; diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 2817936f..6d5c2c85 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -26,38 +26,176 @@ #include "common/signalk/SKUpdateStatic.h" #include "common/signalk/SKUnits.h" #include "IMUService.h" +#include "host/util/PersistentStorage.h" + +IMUService::IMUService(IMUConfig &config, SKHub &skHub) : + Task("IMU"), _config(config), _skHub(skHub), + _timeSinceLastCalSave(resaveCalibrationTimeMs) { + +} void IMUService::setup() { + _offsetRoll = 0.0; + _offsetPitch = 0.0; + + // Remap is 00zzyyxx + // Where each can be 00:X, 01: Y, 10:Z + // Sign is the 3 lsb: 00000xyz + switch (_config.mounting) { + case HorizontalLeftSideToBow: + _axisConfig = 0b00100100; + _signConfig = 0b00000000; + break; + case VerticalTopToBow: + case VerticalStbHull: + case VerticalPortHull: + _axisConfig = 0b00001001; + _signConfig = 0b00000000; + } + DEBUG("Initing BNO055"); - if (!bno055.begin()) { + if (!bno055.begin(bno055.OPERATION_MODE_NDOF, _axisConfig, _signConfig)) { DEBUG("Error initializing BNO055"); } else { - DEBUG("Success!"); + // read calibrationData and offset values for heel & pitch from flash + restoreCalibration(); } } void IMUService::loop() { - bno055.getCalibration(&sysCalib, &gyroCalib, &accelCalib, &magCalib); + bno055.getCalibration(&_sysCalib, &_gyroCalib, &_accelCalib, &_magCalib); + imu::Vector<3> eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); + SKUpdateStatic<2> update; - //DEBUG("Calib Sys: %i Accel: %i Gyro: %i Mag: %i", sysCalib, accelCalib, gyroCalib, magCalib); - //DEBUG("Attitude roll: %f pitch: %f Mag heading: %f", eulerAngles.z(), eulerAngles.y(), eulerAngles.x()); + // In the SignalK Specification + // roll: Vessel roll, +ve is list to starboard + // pitch: Pitch, +ve is bow up + switch (_config.mounting) { + case VerticalPortHull: + _roll = SKDegToRad(eulerAngles.z()); + _pitch = SKDegToRad(eulerAngles.y()); + _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); + break; + case VerticalStbHull: + _roll = SKDegToRad(eulerAngles.z())*(-1); + _pitch = SKDegToRad(eulerAngles.y())*(-1); + _heading = SKDegToRad(eulerAngles.x()); + break; + case VerticalTopToBow: + _roll = SKDegToRad(eulerAngles.y()); + _pitch = SKDegToRad(eulerAngles.z()); + _heading = SKDegToRad(fmod(eulerAngles.x() + 180, 360)); + break; + case HorizontalLeftSideToBow: + _roll = SKDegToRad(eulerAngles.y()); + _pitch = SKDegToRad(eulerAngles.z()) * (-1); + _heading = SKDegToRad(eulerAngles.x()); + break; + } - SKUpdateStatic<2> update; - // Note: We could calculate yaw as the difference between the Magnetic - // Heading and the Course Over Ground Magnetic. + if (isMagCalibrated()) { + update.setNavigationHeadingMagnetic(_heading); + } - /* if orientation == MOUNTED_ON_PORT_HULL */ - double roll, pitch, heading; - roll = eulerAngles.z(); - pitch = eulerAngles.y(); - heading = fmod(eulerAngles.x() + 270, 360); + if (isRollAndPitchCalibrated()) { + // Yaw is always NAN. We cannot really measure it with our instruments. + update.setNavigationAttitude(SKTypeAttitude( _roll + _offsetRoll, + _pitch + _offsetPitch, + SKDoubleNAN)); + } + _skHub.publish(update); - if (sysCalib == 3) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ SKDegToRad(roll), /* pitch */ SKDegToRad(pitch), /* yaw */ SKDoubleNAN)); - update.setNavigationHeadingMagnetic(SKDegToRad(heading)); - _skHub.publish(update); + // Save calibrationData values to EEPROM every 30 Minutes, if BNO055 is fully calibrated + if (bno055.isFullyCalibrated() && + _timeSinceLastCalSave > resaveCalibrationTimeMs) { + saveCalibration(); + _timeSinceLastCalSave = 0; } } + +// get the actual values of calibrationData, heel & pitch (including offset) and heading for display +// all angles in radians +void IMUService::getLastValues(int &sysCalibration, int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading){ + sysCalibration = _sysCalib; + accelCalibration = _accelCalib; + pitch = _pitch + _offsetPitch; + roll = _roll + _offsetRoll; + magCalibration = _magCalib; + heading = _heading; +} + +// Calc offset for making Heel=0 and Pitch=0 +// (e.g. called with long button press in IMUMonitorPage) +// If an setOffset will be done a second time within 5 Seconds, +// the values will be stored into EEPROM and loaded at start of KBox +void IMUService::setRollPitchOffset() { + bool changed = false; + if (abs(_roll) < M_PI / 2 ) { + _offsetRoll = _roll * (-1); + changed = true; + } + if (abs(_pitch) < M_PI / 2 ) { + _offsetPitch = _pitch * (-1); + changed = true; + } + + if ( changed ) { + INFO("Offset changed: Heel -> %.3f | Pitch -> %.3f", + SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); + DEBUG("Offset changed: Heel -> %.3f | Pitch -> %.3f", _offsetRoll, + _offsetPitch); + saveCalibration(); + _timeSinceLastCalSave = 0; + } +} + +bool IMUService::saveCalibration() { + struct PersistentStorage::IMUCalibration calibration; + + // This will verify at compile time that there is enough space in the + // persistent storage structure to save all the calibration registers. + static_assert(sizeof(calibration.calibrationData) == + NUM_BNO055_OFFSET_REGISTERS, + "Not enough space in PersistentStorage::IMUCalibration to store BNO055 " + "calibration data."); + + calibration.mountingPosition = static_cast(_config.mounting); + if (! bno055.getSensorOffsets(calibration.calibrationData)) { + ERROR("Cannot save calibration while not fully calibrated."); + return false; + } + calibration.offsetPitch = + static_cast(_offsetPitch * offsetScalingValueToInt); + calibration.offsetRoll = + static_cast(_offsetRoll * offsetScalingValueToInt); + + if (PersistentStorage::writeIMUCalibration(calibration)) { + DEBUG("IMU Calibration written to flash."); + return true; + } else { + ERROR("Error saving IMU Calibration to flash."); + return false; + } +} + +bool IMUService::restoreCalibration() { + struct PersistentStorage::IMUCalibration calibration; + + if (PersistentStorage::readIMUCalibration(calibration)) { + if (calibration.mountingPosition != _config.mounting) { + DEBUG("Mounting position has changed. Discarding previous calibration."); + return false; + } + bno055.setSensorOffsets(calibration.calibrationData); + _offsetPitch = calibration.offsetPitch / offsetScalingValueToInt; + _offsetRoll = calibration.offsetRoll / offsetScalingValueToInt; + DEBUG("IMU Calibration recalled from flash."); + return true; + } else { + ERROR("Error recalling IMU Calibration from flash"); + return false; + } +} + diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 9e1646c2..26f0398f 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -22,19 +22,57 @@ THE SOFTWARE. */ +#pragma once + #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" +#include "host/config/IMUConfig.h" class IMUService : public Task { private: + // Re-save calibration every 30min. + const unsigned int resaveCalibrationTimeMs = 1800000; + + // Used when converting to an int to store with calibration data + // offset is an angle in radians (-3.14 -> 3.14) - scaled to -31459 -> + // 31459 which fits in an int16. + const int offsetScalingValueToInt = 10000; + + IMUConfig &_config; SKHub &_skHub; Adafruit_BNO055 bno055; - uint8_t sysCalib, gyroCalib, accelCalib, magCalib; - imu::Vector<3> eulerAngles; + uint8_t _sysCalib, _gyroCalib, _accelCalib, _magCalib; + uint8_t _axisConfig, _signConfig; + double _roll, _pitch, _heading; + double _offsetRoll, _offsetPitch; + elapsedMillis _timeSinceLastCalSave; + + bool restoreCalibration(); + bool saveCalibration(); public: - IMUService(SKHub& skHub) : Task("IMU"), _skHub(skHub) {}; + IMUService(IMUConfig &config, SKHub& skHub); void setup(); void loop(); + + /** + * Use current roll and pitch as offset so that if KBox does not move, + * future measurement will appear as 0 / 0. + */ + void setRollPitchOffset(); + + // sysCal '0' in NDOF mode means that the device has not yet found the 'north pole', + // and orientation values will be off The heading will jump to an absolute value + // once the BNO finds magnetic north (the system calibrationData status jumps to 1 or higher). + bool isMagCalibrated() { + return _magCalib == 3 && _sysCalib > 0; + }; + + bool isRollAndPitchCalibrated() { + return _accelCalib >= 2 && _gyroCalib >= 2; + }; + + void getLastValues(int &_sysCalibration, int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); + }; diff --git a/src/host/services/MFD.cpp b/src/host/services/MFD.cpp index 98c0c7f7..64fbe8f0 100644 --- a/src/host/services/MFD.cpp +++ b/src/host/services/MFD.cpp @@ -33,6 +33,7 @@ MFD::MFD(GC &gc, Encoder &e, Bounce &b) : Task("MFD"), gc(gc), encoder(e), butto void MFD::setup() { gc.fillRectangle(Origin, gc.getSize(), ColorBlack); lastTick = 0; + button.update(); } void MFD::processInputs() { @@ -45,9 +46,26 @@ void MFD::processInputs() { if (button.update()) { if (button.fallingEdge()) { events.add(new ButtonEvent(ButtonEventTypePressed)); + lastButtonDown = millis(); + lastMaintainedEvent = millis(); } else { events.add(new ButtonEvent(ButtonEventTypeReleased)); + + if (millis() - lastButtonDown > longClickDuration) { + events.add(new ButtonEvent(ButtonEventTypeLongClick)); + } + else { + events.add(new ButtonEvent(ButtonEventTypeClick)); + } + lastButtonDown = 0; + } + } + // If the button is currently down... + if (lastButtonDown != 0) { + if (millis() - lastMaintainedEvent > maintainedEventPeriod) { + events.add(new ButtonEvent(ButtonEventTypeMaintained)); + lastMaintainedEvent = millis(); } } } @@ -99,4 +117,3 @@ void MFD::loop() { (*pageIterator)->paint(gc); } - diff --git a/src/host/services/MFD.h b/src/host/services/MFD.h index 7a09409e..5568b240 100644 --- a/src/host/services/MFD.h +++ b/src/host/services/MFD.h @@ -51,6 +51,8 @@ class MFD : public Task { protected: // Define tick duration in ms. static const int tickDuration = 200; + static const int longClickDuration = 500; + static const int maintainedEventPeriod = 200; GC &gc; Encoder &encoder; Bounce &button; @@ -58,7 +60,8 @@ class MFD : public Task { LinkedList::circularIterator pageIterator; LinkedList events; unsigned long int lastTick; - + unsigned long int lastButtonDown = 0; + unsigned long int lastMaintainedEvent = 0; bool firstTick = true; void processInputs(); @@ -75,6 +78,3 @@ class MFD : public Task { pages.add(p); }; }; - - - diff --git a/src/host/util/PersistentStorage.cpp b/src/host/util/PersistentStorage.cpp index 61e1f6bd..2f59d508 100644 --- a/src/host/util/PersistentStorage.cpp +++ b/src/host/util/PersistentStorage.cpp @@ -108,3 +108,18 @@ bool PersistentStorage::writeNMEA2000Parameters(struct NMEA2000Parameters &p) { return write(nmea2000ParamsAddress, &p, sizeof(NMEA2000Parameters)); } +bool PersistentStorage::readIMUCalibration(struct IMUCalibration &o){ + if (!isInitialized()) { + return false; + } + + return read(imuCalibrationAddress, &o, sizeof(IMUCalibration)); +} + +bool PersistentStorage::writeIMUCalibration(struct IMUCalibration &o) { + if (!isInitialized()) { + initialize(); + } + + return write(imuCalibrationAddress, &o, sizeof(IMUCalibration)); +} diff --git a/src/host/util/PersistentStorage.h b/src/host/util/PersistentStorage.h index dbc02a20..9f49a3f5 100644 --- a/src/host/util/PersistentStorage.h +++ b/src/host/util/PersistentStorage.h @@ -36,7 +36,7 @@ static const uint32_t storageMagic = 0xB0A7F107; // This needs to be updated when the flash storage becomes incompatible! -static const uint32_t storageVersion = 1; +static const uint32_t storageVersion = 2; class PersistentStorage { @@ -65,15 +65,26 @@ class PersistentStorage { }; }; + struct IMUCalibration { + uint8_t mountingPosition; + uint8_t calibrationData[22]; // NUM_BNO055_OFFSET_REGISTERS + int16_t offsetRoll; + int16_t offsetPitch; + }; + private: // Addresses of elements in flash storage static const uint16_t magicAddress = 0; static const uint16_t versionAddress = 4; static const uint16_t nmea2000ParamsAddress = 8; + static const uint16_t imuCalibrationAddress = + nmea2000ParamsAddress + sizeof(NMEA2000Parameters); // Size of flash - static const uint16_t storageUsed = 8 + sizeof(struct NMEA2000Parameters); - static const uint16_t storageSize = 2048; + static const uint16_t storageUsed = 8 + sizeof(struct NMEA2000Parameters) + + sizeof(IMUCalibration); + static const uint16_t storageSize = 2048; // Teensy 3.2 + //static const uint16_t storageSize = 4096; // Teensy 3.6 static bool read(uint16_t address, void *dest, uint16_t size); static bool write(uint16_t address, const void *src, uint16_t size); @@ -86,5 +97,7 @@ class PersistentStorage { public: static bool readNMEA2000Parameters(struct NMEA2000Parameters &p); static bool writeNMEA2000Parameters(struct NMEA2000Parameters &p); -}; + static bool readIMUCalibration(struct IMUCalibration &o); + static bool writeIMUCalibration(struct IMUCalibration &o); +}; diff --git a/src/test/config/KBoxConfigParserTest.cpp b/src/test/config/KBoxConfigParserTest.cpp index b96894c9..cbc13143 100644 --- a/src/test/config/KBoxConfigParserTest.cpp +++ b/src/test/config/KBoxConfigParserTest.cpp @@ -61,12 +61,15 @@ TEST_CASE("KBoxConfigParser") { CHECK( config.serial1Config.inputMode == SerialModeNMEA ); CHECK( config.serial1Config.outputMode == SerialModeNMEA ); CHECK( config.serial2Config.baudRate == 4800 ); + + CHECK( config.imuConfig.enableHdg ); + CHECK( config.imuConfig.enableHeelPitch ); } SECTION("basic config") { const char* jsonConfig = "{ 'serial1': { 'inputMode': 'disabled', 'outputMode': 'nmea', 'baudRate': 4800 }, \ 'serial2': { 'baudRate': 38400 }, \ - 'imu': { 'enabled': false, frequency: 5 }\ + 'imu': { 'enabled': false, frequency: 5, 'mounting': 'horizontalLeftSideToBow' }\ }"; JsonObject& root = jsonBuffer.parseObject(jsonConfig); @@ -82,6 +85,8 @@ TEST_CASE("KBoxConfigParser") { CHECK( config.serial2Config.baudRate == 38400 ); CHECK( ! config.imuConfig.enabled ); + CHECK( config.imuConfig.frequency == 5 ); + CHECK( config.imuConfig.mounting == HorizontalLeftSideToBow ); } SECTION("NMEA Converter Config") {