From bd9cb0ca39197095a38c7ba437748819b057e21e Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 20:45:00 +0100 Subject: [PATCH 01/81] Netbeans project folder added --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 3d55e68a..47d0e9fe 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,6 @@ Temporary Items # Platformio likes to auto-create this lib/readme.txt + +# Netbeans project folder +nbproject \ No newline at end of file From ab7ccfba3f2fee30d7dcd631b7a3e49fe85926f3 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 23:43:08 +0100 Subject: [PATCH 02/81] initial commit with some settings for IMUService & IMUMonitorPage --- lib/KBoxHardware/src/KBoxConfig.h | 64 +++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 lib/KBoxHardware/src/KBoxConfig.h diff --git a/lib/KBoxHardware/src/KBoxConfig.h b/lib/KBoxHardware/src/KBoxConfig.h new file mode 100644 index 00000000..4efeff56 --- /dev/null +++ b/lib/KBoxHardware/src/KBoxConfig.h @@ -0,0 +1,64 @@ +/* + __ __ ______ ______ __ __ + /\ \/ / /\ == \ /\ __ \ /\_\_\_\ + \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ + \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ + \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ + + The MIT License + + Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net + + 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. +*/ + +#pragma once +#include + +// --------------------------------------------------------------------------- +// Define different tasks and pages +// if you don't like a task or page just comment it out +// --------------------------------------------------------------------------- +#define IMU_MONITOR_PAGE + +// --------------------------------------------------------------------------- +// Use of internal Sensors +// --------------------------------------------------------------------------- +// [sensors] +extern bool cfUseHdgFromBusIfExists; // Get Heading from NMEA2000? +extern bool cfUseHdgFromIMUSensor; // Get Heading from internal IMU-Sensor? +extern bool cfUseHeelPitchFromBus; // Get Heel/Pitch from NMEA2000? +extern bool cfUseHeelPitchFromIMUSensor; // Get Heel/Pitch from internal IMU-Sensor? +extern int8_t cfHdgMinCal; // Minimum Calibration to take HDG from internal sensor +extern int8_t cfHeelPitchMinCal; // Minimum Calibration to take Heel & Pitch from internal sensor + +enum KBoxOrientation { + MOUNTED_UPRIGHT_STB_HULL, + MOUNTED_UPRIGHT_PORT_HULL, + LAYING_READ_DIR_TO_BOW, + LAYING_LEFT_TO_BOW +}; +extern KBoxOrientation cfKBoxOrientation; + +// --------------------------------------------------------------------------- +// Interval Services +// --------------------------------------------------------------------------- +extern int16_t cfIMUServiceInterval; +extern int16_t cfBaroServiceInterval; +extern int16_t cfAdcServiceInterval; \ No newline at end of file From 9fc5355001cd76825621e82e8a2396930cd8f6f5 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 23:43:17 +0100 Subject: [PATCH 03/81] initial commit with some settings for IMUService & IMUMonitorPage --- lib/KBoxHardware/src/KBoxConfig.cpp | 52 +++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 lib/KBoxHardware/src/KBoxConfig.cpp diff --git a/lib/KBoxHardware/src/KBoxConfig.cpp b/lib/KBoxHardware/src/KBoxConfig.cpp new file mode 100644 index 00000000..c9c47360 --- /dev/null +++ b/lib/KBoxHardware/src/KBoxConfig.cpp @@ -0,0 +1,52 @@ +/* + __ __ ______ ______ __ __ + /\ \/ / /\ == \ /\ __ \ /\_\_\_\ + \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ + \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ + \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ + + The MIT License + + Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net + + 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 "KBoxConfig.h" + +// --------------------------------------------------------------------------- +// Use of internal Sensors +// --------------------------------------------------------------------------- +// [sensors] +bool cfUseHdgFromBusIfExists = true; // Get Heading from NMEA2000? +bool cfUseHdgFromIMUSensor = true; // Get Heading from internal IMU-Sensor? +bool cfUseHeelPitchFromBus = true; // Get Heel/Pitch from NMEA2000? +bool cfUseHeelPitchFromIMUSensor = true; // Get Heel/Pitch from internal IMU-Sensor? +int8_t cfHdgMinCal = 2; // Minimum Calibration to take HDG from internal sensor +int8_t cfHeelPitchMinCal = 2; // Minimum Calibration to take Heel & Pitch from internal sensor + +// MOUNTED_UPRIGHT_STB_HULL, MOUNTED_UPRIGHT_PORT_HULL, LAYING_READ_DIR_TO_BOW, LAYING_LEFT_TO_BOW +KBoxOrientation cfKBoxOrientation = MOUNTED_UPRIGHT_STB_HULL; + +// --------------------------------------------------------------------------- +// Interval Services +// --------------------------------------------------------------------------- +int16_t cfIMUServiceInterval = 50; +int16_t cfBaroServiceInterval = 1000; +int16_t cfAdcServiceInterval = 1000; From 9cc6d2e0215bc151e1df648b40ad76c0db3a60bb Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 23:43:57 +0100 Subject: [PATCH 04/81] Start of simplified IMUMonitorPage --- src/host/pages/IMUMonitorPage.cpp | 106 ++++++++++++++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 src/host/pages/IMUMonitorPage.cpp diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp new file mode 100644 index 00000000..8a5817ce --- /dev/null +++ b/src/host/pages/IMUMonitorPage.cpp @@ -0,0 +1,106 @@ +/* + __ __ ______ ______ __ __ + /\ \/ / /\ == \ /\ __ \ /\_\_\_\ + \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ + \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ + \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ + + The MIT License + + Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net + + 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 +#include "common/signalk/SKUpdate.h" +#include "IMUMonitorPage.h" +#include "KBoxConfig.h" +#include +#include "signalk/SKUnits.h" +#include "services/IMUService.h" + +IMUMonitorPage::IMUMonitorPage(SKHub& hub) { + 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), "Calibration", 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); + _heelTL = 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(_heelTL); + addLayer(_pitchTL); + + hub.subscribe(this); +} + + +void IMUMonitorPage::updateReceived(const SKUpdate& up) { + + // No Updates coming when Calib below cfIMU_MIN_CAL + // Aproach was for trusted values only + // Disadvantage is, that the display is stuck to the last value! + + if ( up.hasNavigationHeadingMagnetic() ) { + const SKValue& vm = up.getNavigationHeadingMagnetic(); + // DEBUG( "Heading Magnetic %f", SKRadToDeg( vm.getNumberValue() ) ); + _hdgTL->setText(String( SKRadToDeg( vm.getNumberValue() ), 1) + "° "); + } + + + // Always show Hdg from IMU-sensor, but if the value is not trusted change color to Red + // TODO: Some damping for the display + /* + _hdgTL->setText(String( IMUService::IMU_HdgFiltered, 1) + "° "); + _calTL->setText(String( IMUService::magCAL) + " "); + + if ( IMUService::magCAL < cfHdgMinCal ) { + _hdgTL->setColor(ColorRed); + _calTL->setColor(ColorRed); + } else { + _hdgTL->setColor(ColorWhite); + _calTL->setColor(ColorWhite); + }; + */ + + // GyroCalib is more likely better than magCalib, so we hope there is an update + // from IMUService + // TODO: An own Visitor for Display Values (damped values with lower frequency) + if ( up.hasNavigationAttitude() ) { + const SKValue& vm = up.getNavigationAttitude(); + //DEBUG("MagCAL from IMUService: %i ", IMUService::magCAL ); + _pitchTL->setText(String( SKRadToDeg( vm.getAttitudeValue().pitch ), 1) + "° "); + _heelTL->setText(String( SKRadToDeg( vm.getAttitudeValue().roll ), 1) + "° "); + } else { + //DEBUG("up.hasNavigationAttitude() == FALSE"); + } +} + From 1e183f473c40d6b047c5e069f8679f17c86e470e Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 23:44:04 +0100 Subject: [PATCH 05/81] Start of simplified IMUMonitorPage --- src/host/pages/IMUMonitorPage.h | 47 +++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 src/host/pages/IMUMonitorPage.h diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h new file mode 100644 index 00000000..95aedfcf --- /dev/null +++ b/src/host/pages/IMUMonitorPage.h @@ -0,0 +1,47 @@ +/* + __ __ ______ ______ __ __ + /\ \/ / /\ == \ /\ __ \ /\_\_\_\ + \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ + \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ + \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ + + The MIT License + + Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net + + 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. +*/ + +#pragma once + +#include "common/ui/Page.h" +#include "common/ui/TextLayer.h" +#include "common/signalk/SKHub.h" +#include "common/signalk/SKSubscriber.h" + +class IMUMonitorPage : public Page, public SKSubscriber { + private: + TextLayer *_hdgTL, *_heelTL, *_pitchTL, *_calTL; + int _magCal; + + public: + IMUMonitorPage(SKHub& hub); + + virtual void updateReceived(const SKUpdate& up); +}; \ No newline at end of file From e9ad204db2e3d27706f1bbec788d622b05a9bea7 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 23:45:04 +0100 Subject: [PATCH 06/81] include KBoxConfig, IMUMonitorPage, settings for service intervals --- src/host/main.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index 54c8740f..d6fd228e 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -23,11 +23,13 @@ */ #include +#include #include "common/os/TaskManager.h" #include "common/os/Task.h" #include "common/signalk/SKHub.h" #include "host/drivers/ILI9341GC.h" #include "host/pages/BatteryMonitorPage.h" +#include "host/pages/IMUMonitorPage.h" #include "host/pages/StatsPage.h" #include "host/services/MFD.h" #include "host/services/ADCService.h" @@ -90,9 +92,9 @@ void setup() { // Add all the tasks taskManager.addTask(&mfd); taskManager.addTask(new IntervalTask(new RunningLightService(), 250)); - taskManager.addTask(new IntervalTask(adcService, 1000)); - taskManager.addTask(new IntervalTask(imuService, 50)); - taskManager.addTask(new IntervalTask(baroService, 1000)); + taskManager.addTask(new IntervalTask(adcService, cfAdcServiceInterval)); + taskManager.addTask(new IntervalTask(imuService, cfIMUServiceInterval)); + taskManager.addTask(new IntervalTask(baroService, cfBaroServiceInterval)); taskManager.addTask(n2kService); taskManager.addTask(reader1); taskManager.addTask(reader2); @@ -102,6 +104,11 @@ void setup() { BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub); mfd.addPage(batPage); + + #ifdef IMU_MONITOR_PAGE + IMUMonitorPage *imuPage = new IMUMonitorPage(skHub); + mfd.addPage(imuPage); + #endif StatsPage *statsPage = new StatsPage(); statsPage->setSDCardTask(sdcardTask); From 14a7546ab4c22e8c4d7ef109c2372dd23938d626 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 9 Dec 2017 23:46:49 +0100 Subject: [PATCH 07/81] MagHdg and heel/pitch update separated to diff. calibration settings --- src/host/services/IMUService.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index b17b3a83..c3c18282 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -22,6 +22,7 @@ THE SOFTWARE. */ +#include #include #include "common/signalk/SKUpdateStatic.h" #include "common/signalk/SKUnits.h" @@ -55,9 +56,13 @@ void IMUService::loop() { pitch = eulerAngles.y(); heading = fmod(eulerAngles.x() + 270, 360); - if (sysCalib == 3) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ SKDegToRad(roll), /* pitch */ SKDegToRad(pitch), /* yaw */ 0)); + if (magCalib >= cfHdgMinCal) { update.setNavigationHeadingMagnetic(SKDegToRad(heading)); _skHub.publish(update); + } + + if (accelCalib >= cfHeelPitchMinCal && gyroCalib >= cfHeelPitchMinCal) { + update.setNavigationAttitude(SKTypeAttitude(/* roll */ SKDegToRad(roll), /* pitch */ SKDegToRad(pitch), /* yaw */ 0)); + _skHub.publish(update); } } From 7898cb9e10933b052a7609c0855161628e676170 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 01:23:31 +0100 Subject: [PATCH 08/81] Some functions added for IMUMonitorPage --- src/host/services/IMUService.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 9e1646c2..94aa7613 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -21,7 +21,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ - +#pragma once #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" @@ -30,11 +30,18 @@ class IMUService : public Task { private: SKHub &_skHub; Adafruit_BNO055 bno055; - uint8_t sysCalib, gyroCalib, accelCalib, magCalib; + uint8_t _sysCalib, _gyroCalib, _accelCalib, _magCalib; + double _roll, _pitch, _heading; imu::Vector<3> eulerAngles; public: IMUService(SKHub& skHub) : Task("IMU"), _skHub(skHub) {}; void setup(); void loop(); + + void getLastValues(int &accelCalibration, double &pitch, double &heel, int &magCalibration, double &heading); + void saveAccelCalibration(); + void resetAccelCalibration(); + void saveMagCalibration(); + void resetMagCalibration(); }; From e9b4cea86a618740db00dee2e5d8c28db2e399e0 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 01:23:53 +0100 Subject: [PATCH 09/81] Some functions added for the IMUMonitorPage --- src/host/services/IMUService.cpp | 33 +++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index c3c18282..133a30bd 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -22,8 +22,8 @@ THE SOFTWARE. */ -#include #include +#include "common/config/KBoxConfig.h" #include "common/signalk/SKUpdateStatic.h" #include "common/signalk/SKUnits.h" #include "IMUService.h" @@ -39,30 +39,37 @@ void IMUService::setup() { } void IMUService::loop() { - bno055.getCalibration(&sysCalib, &gyroCalib, &accelCalib, &magCalib); + bno055.getCalibration(&_sysCalib, &_gyroCalib, &_accelCalib, &_magCalib); eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - //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()); + 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()); SKUpdateStatic<2> update; // Note: We could calculate yaw as the difference between the Magnetic // Heading and the Course Over Ground Magnetic. /* if orientation == MOUNTED_ON_PORT_HULL */ - double roll, pitch, heading; - roll = eulerAngles.z(); - pitch = eulerAngles.y(); - heading = fmod(eulerAngles.x() + 270, 360); + _roll = eulerAngles.z(); + _pitch = eulerAngles.y(); + _heading = fmod(eulerAngles.x() + 270, 360); - if (magCalib >= cfHdgMinCal) { - update.setNavigationHeadingMagnetic(SKDegToRad(heading)); + if (_magCalib >= cfHdgMinCal) { + update.setNavigationHeadingMagnetic(SKDegToRad(_heading)); _skHub.publish(update); } - - if (accelCalib >= cfHeelPitchMinCal && gyroCalib >= cfHeelPitchMinCal) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ SKDegToRad(roll), /* pitch */ SKDegToRad(pitch), /* yaw */ 0)); + + if (_accelCalib >= cfHeelPitchMinCal && _gyroCalib >= cfHeelPitchMinCal) { + update.setNavigationAttitude(SKTypeAttitude(/* roll */ SKDegToRad(_roll), /* pitch */ SKDegToRad(_pitch), /* yaw */ 0)); _skHub.publish(update); } } + +void IMUService::getLastValues(int &accelCalibration, double &pitch, double &heel, int &magCalibration, double &heading){ + accelCalibration = _accelCalib; + pitch = _pitch; + heel = _roll; + magCalibration = _magCalib; + heading = _heading; +} From b67754c80ecf3794afc9d2a2ba6d347b984b775e Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 01:25:47 +0100 Subject: [PATCH 10/81] more defines, passing a reference to IMUService for IMUMonitorPage --- src/host/main.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index d6fd228e..14f4fc95 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -23,18 +23,18 @@ */ #include -#include +#include "common/config/KBoxConfig.h" #include "common/os/TaskManager.h" #include "common/os/Task.h" #include "common/signalk/SKHub.h" #include "host/drivers/ILI9341GC.h" #include "host/pages/BatteryMonitorPage.h" -#include "host/pages/IMUMonitorPage.h" #include "host/pages/StatsPage.h" #include "host/services/MFD.h" #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/NMEAService.h" #include "host/services/RunningLightService.h" @@ -72,9 +72,6 @@ void setup() { WiFiService *wifi = new WiFiService(skHub, gc); ADCService *adcService = new ADCService(skHub, KBox.getADC()); - BarometerService *baroService = new BarometerService(skHub); - IMUService *imuService = new IMUService(skHub); - NMEA2000Service *n2kService = new NMEA2000Service(skHub); n2kService->connectTo(*wifi); @@ -93,8 +90,6 @@ void setup() { taskManager.addTask(&mfd); taskManager.addTask(new IntervalTask(new RunningLightService(), 250)); taskManager.addTask(new IntervalTask(adcService, cfAdcServiceInterval)); - taskManager.addTask(new IntervalTask(imuService, cfIMUServiceInterval)); - taskManager.addTask(new IntervalTask(baroService, cfBaroServiceInterval)); taskManager.addTask(n2kService); taskManager.addTask(reader1); taskManager.addTask(reader2); @@ -102,13 +97,22 @@ void setup() { taskManager.addTask(sdcardTask); taskManager.addTask(&usbService); + #ifdef SERVICE_BARO + BarometerService *baroService = new BarometerService(skHub); + taskManager.addTask(new IntervalTask(baroService, cfBaroServiceInterval)); + #endif + + #ifdef SERVICE_IMU + IMUService *imuService = new IMUService(skHub); + taskManager.addTask(new IntervalTask(imuService, cfIMUServiceInterval)); + #ifdef PAGE_IMU + IMUMonitorPage *imuPage = new IMUMonitorPage(skHub, *imuService); + mfd.addPage(imuPage); + #endif + #endif + BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub); mfd.addPage(batPage); - - #ifdef IMU_MONITOR_PAGE - IMUMonitorPage *imuPage = new IMUMonitorPage(skHub); - mfd.addPage(imuPage); - #endif StatsPage *statsPage = new StatsPage(); statsPage->setSDCardTask(sdcardTask); From f041061743c91da3905de070cfa2480539300922 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 01:26:45 +0100 Subject: [PATCH 11/81] reference to IMUService added --- src/host/pages/IMUMonitorPage.h | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index 95aedfcf..ef62abf6 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -34,14 +34,18 @@ #include "common/ui/TextLayer.h" #include "common/signalk/SKHub.h" #include "common/signalk/SKSubscriber.h" +#include "services/IMUService.h" -class IMUMonitorPage : public Page, public SKSubscriber { +class IMUMonitorPage: public Page, public SKSubscriber { private: TextLayer *_hdgTL, *_heelTL, *_pitchTL, *_calTL; - int _magCal; + int _magCalibration, _accelCalibration; + double _pitch; + double _heel; + double _heading; public: - IMUMonitorPage(SKHub& hub); - + IMUMonitorPage(SKHub& hub, IMUService &imuService); + //IMUService imuService; virtual void updateReceived(const SKUpdate& up); -}; \ No newline at end of file +}; From de083c1c281afc87f66741ce42948f8be8fc6865 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 01:27:46 +0100 Subject: [PATCH 12/81] getLastValues from IMUService added --- src/host/pages/IMUMonitorPage.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 8a5817ce..68a5c9ad 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -31,12 +31,12 @@ #include #include "common/signalk/SKUpdate.h" #include "IMUMonitorPage.h" -#include "KBoxConfig.h" +#include "common/config/KBoxConfig.h" #include #include "signalk/SKUnits.h" -#include "services/IMUService.h" -IMUMonitorPage::IMUMonitorPage(SKHub& hub) { + +IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) { static const int col1 = 5; static const int col2 = 200; static const int row1 = 26; @@ -60,11 +60,16 @@ IMUMonitorPage::IMUMonitorPage(SKHub& hub) { addLayer(_pitchTL); hub.subscribe(this); + + imuService.getLastValues(_accelCalibration, _pitch, _heel, _magCalibration, _heading); + DEBUG("AccelCalibration: %i | MagCalibration: %i", _accelCalibration, _magCalibration); } void IMUMonitorPage::updateReceived(const SKUpdate& up) { + + // No Updates coming when Calib below cfIMU_MIN_CAL // Aproach was for trusted values only // Disadvantage is, that the display is stuck to the last value! @@ -81,7 +86,7 @@ void IMUMonitorPage::updateReceived(const SKUpdate& up) { /* _hdgTL->setText(String( IMUService::IMU_HdgFiltered, 1) + "° "); _calTL->setText(String( IMUService::magCAL) + " "); - + if ( IMUService::magCAL < cfHdgMinCal ) { _hdgTL->setColor(ColorRed); _calTL->setColor(ColorRed); @@ -103,4 +108,3 @@ void IMUMonitorPage::updateReceived(const SKUpdate& up) { //DEBUG("up.hasNavigationAttitude() == FALSE"); } } - From d11d220448a45010d8e5d652782f8115ad4ad9d4 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 01:29:36 +0100 Subject: [PATCH 13/81] KBoxConfig moved to src/common/config, 2 more service-defines added --- {lib/KBoxHardware/src => src/common/config}/KBoxConfig.cpp | 0 {lib/KBoxHardware/src => src/common/config}/KBoxConfig.h | 7 +++++-- 2 files changed, 5 insertions(+), 2 deletions(-) rename {lib/KBoxHardware/src => src/common/config}/KBoxConfig.cpp (100%) rename {lib/KBoxHardware/src => src/common/config}/KBoxConfig.h (96%) diff --git a/lib/KBoxHardware/src/KBoxConfig.cpp b/src/common/config/KBoxConfig.cpp similarity index 100% rename from lib/KBoxHardware/src/KBoxConfig.cpp rename to src/common/config/KBoxConfig.cpp diff --git a/lib/KBoxHardware/src/KBoxConfig.h b/src/common/config/KBoxConfig.h similarity index 96% rename from lib/KBoxHardware/src/KBoxConfig.h rename to src/common/config/KBoxConfig.h index 4efeff56..ced3fca8 100644 --- a/lib/KBoxHardware/src/KBoxConfig.h +++ b/src/common/config/KBoxConfig.h @@ -35,7 +35,10 @@ // Define different tasks and pages // if you don't like a task or page just comment it out // --------------------------------------------------------------------------- -#define IMU_MONITOR_PAGE +#define SERVICE_BARO +#define SERVICE_IMU + +#define PAGE_IMU // --------------------------------------------------------------------------- // Use of internal Sensors @@ -61,4 +64,4 @@ extern KBoxOrientation cfKBoxOrientation; // --------------------------------------------------------------------------- extern int16_t cfIMUServiceInterval; extern int16_t cfBaroServiceInterval; -extern int16_t cfAdcServiceInterval; \ No newline at end of file +extern int16_t cfAdcServiceInterval; From db7fa7785cf0ebb14d60ddfd5060c050dd9a200b Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 22:39:59 +0100 Subject: [PATCH 14/81] extended with button long click for offset-to-zero of heel/pitch --- src/host/pages/IMUMonitorPage.cpp | 66 +++++++++++++++---------------- 1 file changed, 32 insertions(+), 34 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 68a5c9ad..adfc6977 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -36,7 +36,7 @@ #include "signalk/SKUnits.h" -IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) { +IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) : _imuService(imuService) { static const int col1 = 5; static const int col2 = 200; static const int row1 = 26; @@ -45,66 +45,64 @@ IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) { 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), "Calibration", 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); - _heelTL = new TextLayer(Point(col1, row4), 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(_heelTL); + addLayer(_rollTL); addLayer(_pitchTL); hub.subscribe(this); - imuService.getLastValues(_accelCalibration, _pitch, _heel, _magCalibration, _heading); - DEBUG("AccelCalibration: %i | MagCalibration: %i", _accelCalibration, _magCalibration); } +bool IMUMonitorPage::processEvent(const ButtonEvent &be){ -void IMUMonitorPage::updateReceived(const SKUpdate& up) { + // DEBUG("EventTypeButton: %i", be.clickType); + if (be.clickType == ButtonEventTypeClick) { + DEBUG("Button ButtonEventTypeClick !!!"); + // Change page on single click. + return false; + } + if (be.clickType == ButtonEventTypeLongClick) { + DEBUG("Button ButtonEventTypeLongClick !!!"); + // TODO: here will start the offset to zero calibration of heel & pitch + } + return true; +} +bool IMUMonitorPage::processEvent(const TickEvent &te){ + _imuService.getLastValues(_accelCalibration, _pitch, _roll, _magCalibration, _heading); + //DEBUG("AccelCalibration: %i | MagCalibration: %i", _accelCalibration, _magCalibration); - // No Updates coming when Calib below cfIMU_MIN_CAL - // Aproach was for trusted values only - // Disadvantage is, that the display is stuck to the last value! + // TODO: Some damping for the display - if ( up.hasNavigationHeadingMagnetic() ) { - const SKValue& vm = up.getNavigationHeadingMagnetic(); - // DEBUG( "Heading Magnetic %f", SKRadToDeg( vm.getNumberValue() ) ); - _hdgTL->setText(String( SKRadToDeg( vm.getNumberValue() ), 1) + "° "); - } + _hdgTL->setText(String( _heading, 1) + "° "); + _calTL->setText(String( _magCalibration) + "/" + String( _accelCalibration) + " "); + _pitchTL->setText(String( _pitch, 1) + "° "); + _rollTL->setText(String( _roll, 1) + "° "); // Always show Hdg from IMU-sensor, but if the value is not trusted change color to Red - // TODO: Some damping for the display - /* - _hdgTL->setText(String( IMUService::IMU_HdgFiltered, 1) + "° "); - _calTL->setText(String( IMUService::magCAL) + " "); - - if ( IMUService::magCAL < cfHdgMinCal ) { + if ((_magCalibration <= cfHdgMinCal)||(_accelCalibration <= cfHeelPitchMinCal)) { _hdgTL->setColor(ColorRed); _calTL->setColor(ColorRed); } else { _hdgTL->setColor(ColorWhite); _calTL->setColor(ColorWhite); }; - */ - - // GyroCalib is more likely better than magCalib, so we hope there is an update - // from IMUService - // TODO: An own Visitor for Display Values (damped values with lower frequency) - if ( up.hasNavigationAttitude() ) { - const SKValue& vm = up.getNavigationAttitude(); - //DEBUG("MagCAL from IMUService: %i ", IMUService::magCAL ); - _pitchTL->setText(String( SKRadToDeg( vm.getAttitudeValue().pitch ), 1) + "° "); - _heelTL->setText(String( SKRadToDeg( vm.getAttitudeValue().roll ), 1) + "° "); - } else { - //DEBUG("up.hasNavigationAttitude() == FALSE"); - } + + return true; +} + +void IMUMonitorPage::updateReceived(const SKUpdate& up) { + // may be needed for something.... } From 100a6d425b31dcd7badf42bfd9864c471f39ca5a Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 22:40:13 +0100 Subject: [PATCH 15/81] extended with button long click for offset-to-zero of heel/pitch --- src/host/pages/IMUMonitorPage.h | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index ef62abf6..01a66c75 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -28,24 +28,26 @@ THE SOFTWARE. */ -#pragma once - #include "common/ui/Page.h" #include "common/ui/TextLayer.h" #include "common/signalk/SKHub.h" #include "common/signalk/SKSubscriber.h" #include "services/IMUService.h" -class IMUMonitorPage: public Page, public SKSubscriber { - private: - TextLayer *_hdgTL, *_heelTL, *_pitchTL, *_calTL; +class IMUMonitorPage : public Page, public SKSubscriber { + private: + TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; + IMUService &_imuService; + int _magCalibration, _accelCalibration; - double _pitch; - double _heel; - double _heading; - - public: - IMUMonitorPage(SKHub& hub, IMUService &imuService); - //IMUService imuService; - virtual void updateReceived(const SKUpdate& up); + double _pitch; + double _roll; + double _heading; + + public: + IMUMonitorPage(SKHub& hub, IMUService &imuService); + virtual void updateReceived(const SKUpdate& up); + + bool processEvent(const TickEvent &te); + bool processEvent(const ButtonEvent &be); }; From 24b5ec455832ec7186af73d26861bf987df4e485 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:15:31 +0100 Subject: [PATCH 16/81] make write8() public to access from IMUService --- lib/Adafruit_BNO055/Adafruit_BNO055.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.h b/lib/Adafruit_BNO055/Adafruit_BNO055.h index 5516d426..f56e5b0e 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.h +++ b/lib/Adafruit_BNO055/Adafruit_BNO055.h @@ -307,10 +307,13 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); bool isFullyCalibrated(void); + // make public to write in imuService + bool write8 ( adafruit_bno055_reg_t, byte value ); + 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 ); + uint8_t _address; int32_t _sensorID; From 1229eb462034db41d4d5399b41d23378f17cbf77 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:16:24 +0100 Subject: [PATCH 17/81] axis- and sign remapping cancelled here as made on start of IMUService --- lib/Adafruit_BNO055/Adafruit_BNO055.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp index 9e99e71b..9186e9ab 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp +++ b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp @@ -113,12 +113,12 @@ bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) /* 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 - delay(10); + //write8(BNO055_AXIS_MAP_CONFIG_ADDR, 0b00001001); // P0-P7, Default is P1 + //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 - delay(10); - + //write8(BNO055_AXIS_MAP_SIGN_ADDR, 0b00000000); // P0-P7, Default is P1 + //delay(10); + write8(BNO055_SYS_TRIGGER_ADDR, 0x0); delay(10); /* Set the requested operating mode (see section 3.3) */ @@ -612,7 +612,7 @@ byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg ) #else value = Wire.receive(); #endif - + //DEBUG("read => %x", value); return value; From f45d44590af1ee5faec7cdd9b04b32a1956ea573 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:17:09 +0100 Subject: [PATCH 18/81] ButtonEvent LongClick added, default for switch page is Click now --- src/common/ui/Event.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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; }; }; - - From 91f52d2728b5e8267db02e3d590442a9044e3431 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:17:48 +0100 Subject: [PATCH 19/81] changed to ButtonEventTypeClick for switching patches --- src/common/ui/Page.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) 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); }; - From a58f66edad19f2f6306f988bb320ea71b4b67867 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:19:05 +0100 Subject: [PATCH 20/81] Long Button press for setOffset in IMUService implemented --- src/host/pages/IMUMonitorPage.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index adfc6977..8404f4f4 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -72,8 +72,7 @@ bool IMUMonitorPage::processEvent(const ButtonEvent &be){ return false; } if (be.clickType == ButtonEventTypeLongClick) { - DEBUG("Button ButtonEventTypeLongClick !!!"); - // TODO: here will start the offset to zero calibration of heel & pitch + _imuService.setOffset(); } return true; } From 0f883bdc70fcde480eccf70cc5c8f58f6f1f3d6a Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:19:35 +0100 Subject: [PATCH 21/81] Long button click added --- src/host/services/MFD.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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); }; }; - - - From 65add4f7c9ac8db26017994ad2b11feb7c718911 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:19:44 +0100 Subject: [PATCH 22/81] Long button click added --- src/host/services/MFD.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/host/services/MFD.cpp b/src/host/services/MFD.cpp index 98c0c7f7..d2d32d12 100644 --- a/src/host/services/MFD.cpp +++ b/src/host/services/MFD.cpp @@ -45,9 +45,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 +116,3 @@ void MFD::loop() { (*pageIterator)->paint(gc); } - From 6948ccfb52c94c5332f5852a4b7fa7caa4c3401f Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:49:07 +0100 Subject: [PATCH 23/81] setOffset, recall and saveCalibration, axis & sign setting added --- src/host/services/IMUService.h | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 94aa7613..68480c96 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -25,6 +25,7 @@ #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" +#include class IMUService : public Task { private: @@ -32,6 +33,7 @@ class IMUService : public Task { Adafruit_BNO055 bno055; uint8_t _sysCalib, _gyroCalib, _accelCalib, _magCalib; double _roll, _pitch, _heading; + double _offsetRoll, _offsetPitch; imu::Vector<3> eulerAngles; public: @@ -39,9 +41,9 @@ class IMUService : public Task { void setup(); void loop(); - void getLastValues(int &accelCalibration, double &pitch, double &heel, int &magCalibration, double &heading); - void saveAccelCalibration(); - void resetAccelCalibration(); - void saveMagCalibration(); - void resetMagCalibration(); + void getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); + void resetIMU(); + bool recallCalibration(); + bool saveCalibration(); + void setOffset(); }; From 7b61c06f69626cff672d1459c0737633b1e59305 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:49:16 +0100 Subject: [PATCH 24/81] setOffset, recall and saveCalibration, axis & sign setting added --- src/host/services/IMUService.cpp | 102 ++++++++++++++++++++++++++++--- 1 file changed, 94 insertions(+), 8 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 133a30bd..51a612a4 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -29,12 +29,30 @@ #include "IMUService.h" void IMUService::setup() { + _offsetRoll = 0.0; + _offsetPitch = 0.0; + DEBUG("Initing BNO055"); if (!bno055.begin()) { DEBUG("Error initializing BNO055"); } else { - DEBUG("Success!"); + // put BNO055 into config mode + bno055.setMode(bno055.OPERATION_MODE_CONFIG); + delay(50); + // Configure axis mapping (see Bosch manual section 3.4) + //bno055.write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1 + // set axis remap to default KBox mounting starboard hull + bno055.write8(bno055.BNO055_AXIS_MAP_CONFIG_ADDR, 0b00001001 ); + delay(50); + //bno055.write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1 + // set sign remap to default KBox mounting on starboard hull + bno055.write8(bno055.BNO055_AXIS_MAP_SIGN_ADDR, 0b00000000 ); + delay(50); + bno055.setMode( bno055.OPERATION_MODE_NDOF ); // OPERATION_MODE_NDOF_FMC_OFF + delay(1000); + if (recallCalibration()) + DEBUG("Success!"); } } @@ -43,18 +61,20 @@ void IMUService::loop() { eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - 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()); + //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()); SKUpdateStatic<2> update; // Note: We could calculate yaw as the difference between the Magnetic // Heading and the Course Over Ground Magnetic. - /* if orientation == MOUNTED_ON_PORT_HULL */ - _roll = eulerAngles.z(); - _pitch = eulerAngles.y(); + /* if orientation == MOUNTED_ON_STARBOARD_HULL */ + _roll = eulerAngles.z() + _offsetRoll; + _pitch = eulerAngles.y() + _offsetPitch; _heading = fmod(eulerAngles.x() + 270, 360); + DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", _roll, _pitch, _heading); + if (_magCalib >= cfHdgMinCal) { update.setNavigationHeadingMagnetic(SKDegToRad(_heading)); _skHub.publish(update); @@ -66,10 +86,76 @@ void IMUService::loop() { } } -void IMUService::getLastValues(int &accelCalibration, double &pitch, double &heel, int &magCalibration, double &heading){ +void IMUService::getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading){ accelCalibration = _accelCalib; pitch = _pitch; - heel = _roll; + roll = _roll; magCalibration = _magCalib; heading = _heading; } + +// Offset for Heel=0 and Pitch=0 with long button press in IMUMonitorPage +void IMUService::setOffset() { + // FIXIT: Why it is called at startup of KBox? event longClick coming? + if ( millis() > 10000 ) { + _offsetRoll = _roll * (-1); + _offsetPitch = _pitch * (-1); + DEBUG("Offset changed: Heel -> %.3f | Pitch -> %.3f", _offsetRoll, _offsetPitch); + // TODO: angle of heel and pitch must be smaller than 90°, check overflow! + } +} + +bool IMUService::saveCalibration() { + + int _eeAddress = 0; + long bnoID; + sensor_t sensor; + adafruit_bno055_offsets_t newCalib; + bno055.getSensorOffsets(newCalib); + + DEBUG("\n\nStoring calibration data to EEPROM..."); + + bno055.getSensor(&sensor); + bnoID = sensor.sensor_id; + + EEPROM.put(_eeAddress, bnoID); + + _eeAddress += sizeof(long); + EEPROM.put(_eeAddress, newCalib); + DEBUG("Data stored to EEPROM."); + delay(500); + + return true; +} + +bool IMUService::recallCalibration() { + + int _eeAddress = 0; + long bnoID; + + EEPROM.get(_eeAddress, bnoID); + adafruit_bno055_offsets_t calibrationData; + sensor_t sensor; + + /* + * Look for the sensor's unique ID at the beginning oF EEPROM. + * This isn't foolproof, but it's better than nothing. + */ + bno055.getSensor(&sensor); + if (bnoID != sensor.sensor_id) { + DEBUG("No Calibration Data for this sensor exists in EEPROM"); + } + else { + DEBUG("Found Calibration for this sensor in EEPROM."); + _eeAddress += sizeof(long); + EEPROM.get(_eeAddress, calibrationData); + + DEBUG("Restoring Calibration data to the BNO055..."); + bno055.setSensorOffsets(calibrationData); + + DEBUG("Calibration data loaded into BNO055"); + delay(1000); + return true; + } + return false; +} From 48fe0a3cc16ed10da351ff78517f03f27a4c929c Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 11 Dec 2017 23:51:14 +0100 Subject: [PATCH 25/81] debug msg commented out --- src/host/services/IMUService.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 51a612a4..4362d758 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -72,8 +72,7 @@ void IMUService::loop() { _roll = eulerAngles.z() + _offsetRoll; _pitch = eulerAngles.y() + _offsetPitch; _heading = fmod(eulerAngles.x() + 270, 360); - - DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", _roll, _pitch, _heading); + //DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", _roll, _pitch, _heading); if (_magCalib >= cfHdgMinCal) { update.setNavigationHeadingMagnetic(SKDegToRad(_heading)); From 644643ad46c3ebd2cf7b8bf6658e24b1c42fbb9c Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 11:44:31 +0100 Subject: [PATCH 26/81] put back to original + overload begin function for KBox mounting --- lib/Adafruit_BNO055/Adafruit_BNO055.cpp | 84 ++++++++++++++++++++++--- 1 file changed, 74 insertions(+), 10 deletions(-) diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp index 9186e9ab..e786979b 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp +++ b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp @@ -61,8 +61,75 @@ Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) { /* Enable I2C */ - //Wire.begin(); + Wire.begin(); + // BNO055 clock stretches for 500us or more! +#ifdef ESP8266 + Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching +#endif + + /* Make sure we have the right device */ + uint8_t id = read8(BNO055_CHIP_ID_ADDR); + if(id != BNO055_ID) + { + delay(1000); // hold on for boot + id = read8(BNO055_CHIP_ID_ADDR); + if(id != BNO055_ID) { + return false; // still not? ok bail + } + } + + /* Switch to config mode (just in case since this is the default) */ + setMode(OPERATION_MODE_CONFIG); + + /* Reset */ + write8(BNO055_SYS_TRIGGER_ADDR, 0x20); + while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) + { + delay(10); + } + delay(50); + + /* Set to normal power mode */ + write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); + delay(10); + + write8(BNO055_PAGE_ID_ADDR, 0); + + /* Set the output units */ + /* + uint8_t unitsel = (0 << 7) | // Orientation = Android + (0 << 4) | // Temperature = Celsius + (0 << 2) | // Euler = Degrees + (1 << 1) | // Gyro = Rads + (0 << 0); // Accelerometer = m/s^2 + 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 + delay(10); + 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) */ + setMode(mode); + delay(20); + + return true; +} + +/**************************************************************************/ +/*! + Sets up the IMU Sensor for KBox, including axis- and sign mapping +*/ +/**************************************************************************/ +bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode, uint8_t axis_remap_orientation, uint8_t axis_remap_sign) +{ /* Make sure we have the right device */ uint8_t id = read8(BNO055_CHIP_ID_ADDR); if(id != BNO055_ID) @@ -90,8 +157,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,13 +177,11 @@ 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 - //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 - //delay(10); + /* 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, axis_remap_sign); + delay(10); write8(BNO055_SYS_TRIGGER_ADDR, 0x0); delay(10); From f67950acd48944328e710d81cadf1d987efbdf22 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 11:45:44 +0100 Subject: [PATCH 27/81] restore original + overload begin function for KBox mounting --- lib/Adafruit_BNO055/Adafruit_BNO055.h | 40 +++++++++++++++------------ 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.h b/lib/Adafruit_BNO055/Adafruit_BNO055.h index f56e5b0e..dae1a1fd 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.h +++ b/lib/Adafruit_BNO055/Adafruit_BNO055.h @@ -1,19 +1,17 @@ /*************************************************************************** - This is a library for the BNO055 orientation sensor - Designed specifically to work with the Adafruit BNO055 Breakout. +Edited Library for KBox use! + + This is a library for the BNO055 orientation sensor + Designed specifically to work with the Adafruit BNO055 Breakout. Pick one up today in the adafruit shop! ------> http://www.adafruit.com/products - These sensors use I2C to communicate, 2 pins are required to interface. - Adafruit invests time and resources providing this open source code, please support Adafruit andopen-source hardware by purchasing products from Adafruit! - Written by KTOWN for Adafruit Industries. - MIT license, all text above must be included in any redistribution ***************************************************************************/ @@ -26,12 +24,14 @@ #include "WProgram.h" #endif -//#ifdef __AVR_ATtiny85__ - //#include - //#define Wire TinyWireM -//#else - //#include -//#endif +/* Disabled, not needed for KBox +#ifdef __AVR_ATtiny85__ + #include + #define Wire TinyWireM +#else + #include +#endif +*/ #include #include @@ -279,9 +279,18 @@ class Adafruit_BNO055 : public Adafruit_Sensor VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR } adafruit_vector_type_t; +#if defined (ARDUINO_SAMD_ZERO) && ! (ARDUINO_SAMD_FEATHER_M0) +#error "On an arduino Zero, BNO055's ADR pin must be high. Fix that, then delete this line." + Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_B ); +#else Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A ); - +#endif bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF ); + // 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); + void setMode ( adafruit_bno055_opmode_t mode ); void getRevInfo ( adafruit_bno055_rev_info_t* ); void displayRevInfo ( void ); @@ -307,13 +316,10 @@ class Adafruit_BNO055 : public Adafruit_Sensor void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); bool isFullyCalibrated(void); - // make public to write in imuService - bool write8 ( adafruit_bno055_reg_t, byte value ); - 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 ); uint8_t _address; int32_t _sensorID; From b1b45522e4ce2b1dd3afde652993c0dec2b5b608 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 11:46:58 +0100 Subject: [PATCH 28/81] Axis & Sign config now in bno055.begin function --- src/host/services/IMUService.cpp | 65 +++++++++++++------------------- 1 file changed, 26 insertions(+), 39 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 4362d758..d81610d0 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -32,25 +32,15 @@ void IMUService::setup() { _offsetRoll = 0.0; _offsetPitch = 0.0; + // until config is made, declare default mounting position KBox here + uint8_t axisConfig = 0b00001001; + uint8_t signConfig = 0b00000000; + DEBUG("Initing BNO055"); - if (!bno055.begin()) { + if (!bno055.begin(bno055.OPERATION_MODE_NDOF, axisConfig, signConfig)) { DEBUG("Error initializing BNO055"); } else { - // put BNO055 into config mode - bno055.setMode(bno055.OPERATION_MODE_CONFIG); - delay(50); - // Configure axis mapping (see Bosch manual section 3.4) - //bno055.write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1 - // set axis remap to default KBox mounting starboard hull - bno055.write8(bno055.BNO055_AXIS_MAP_CONFIG_ADDR, 0b00001001 ); - delay(50); - //bno055.write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1 - // set sign remap to default KBox mounting on starboard hull - bno055.write8(bno055.BNO055_AXIS_MAP_SIGN_ADDR, 0b00000000 ); - delay(50); - bno055.setMode( bno055.OPERATION_MODE_NDOF ); // OPERATION_MODE_NDOF_FMC_OFF - delay(1000); if (recallCalibration()) DEBUG("Success!"); } @@ -62,25 +52,25 @@ void IMUService::loop() { eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); //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()); + //DEBUG("Attitude roll: %.3f pitch: %.3f Mag heading: %.3f", eulerAngles.z(), eulerAngles.y(), eulerAngles.x()); SKUpdateStatic<2> update; // Note: We could calculate yaw as the difference between the Magnetic // Heading and the Course Over Ground Magnetic. /* if orientation == MOUNTED_ON_STARBOARD_HULL */ - _roll = eulerAngles.z() + _offsetRoll; - _pitch = eulerAngles.y() + _offsetPitch; - _heading = fmod(eulerAngles.x() + 270, 360); - //DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", _roll, _pitch, _heading); + _roll = SKDegToRad(eulerAngles.z() + _offsetRoll); + _pitch = SKDegToRad(eulerAngles.y() + _offsetPitch); + _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); + //DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", SKRadToDeg(_roll), SKRadToDeg(_pitch), SKRadToDeg(_heading)); if (_magCalib >= cfHdgMinCal) { - update.setNavigationHeadingMagnetic(SKDegToRad(_heading)); + update.setNavigationHeadingMagnetic(_heading); _skHub.publish(update); } if (_accelCalib >= cfHeelPitchMinCal && _gyroCalib >= cfHeelPitchMinCal) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ SKDegToRad(_roll), /* pitch */ SKDegToRad(_pitch), /* yaw */ 0)); + update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll, /* pitch */ _pitch, /* yaw */ 0)); _skHub.publish(update); } } @@ -99,31 +89,28 @@ void IMUService::setOffset() { if ( millis() > 10000 ) { _offsetRoll = _roll * (-1); _offsetPitch = _pitch * (-1); - DEBUG("Offset changed: Heel -> %.3f | Pitch -> %.3f", _offsetRoll, _offsetPitch); + DEBUG("Offset changed: Heel -> %.3f | Pitch -> %.3f", SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); // TODO: angle of heel and pitch must be smaller than 90°, check overflow! } } bool IMUService::saveCalibration() { + int _eeAddress = 0; + long bnoID; + sensor_t sensor; + adafruit_bno055_offsets_t newCalib; + bno055.getSensorOffsets(newCalib); - int _eeAddress = 0; - long bnoID; - sensor_t sensor; - adafruit_bno055_offsets_t newCalib; - bno055.getSensorOffsets(newCalib); - - DEBUG("\n\nStoring calibration data to EEPROM..."); - - bno055.getSensor(&sensor); - bnoID = sensor.sensor_id; - - EEPROM.put(_eeAddress, bnoID); + DEBUG("\n\nStoring calibration data to EEPROM..."); + bno055.getSensor(&sensor); + bnoID = sensor.sensor_id; - _eeAddress += sizeof(long); - EEPROM.put(_eeAddress, newCalib); - DEBUG("Data stored to EEPROM."); - delay(500); + EEPROM.put(_eeAddress, bnoID); + _eeAddress += sizeof(long); + EEPROM.put(_eeAddress, newCalib); + DEBUG("Data stored to EEPROM."); + delay(500); return true; } From 0eb4e27c19fc345ac0f33398f6489b535dd86d4e Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 11:59:50 +0100 Subject: [PATCH 29/81] restore to original (w/o config) just new IMUPage added --- src/host/main.cpp | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index 14f4fc95..db5b4d4c 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -23,7 +23,6 @@ */ #include -#include "common/config/KBoxConfig.h" #include "common/os/TaskManager.h" #include "common/os/Task.h" #include "common/signalk/SKHub.h" @@ -72,6 +71,9 @@ void setup() { WiFiService *wifi = new WiFiService(skHub, gc); ADCService *adcService = new ADCService(skHub, KBox.getADC()); + BarometerService *baroService = new BarometerService(skHub); + IMUService *imuService = new IMUService(skHub); + NMEA2000Service *n2kService = new NMEA2000Service(skHub); n2kService->connectTo(*wifi); @@ -89,7 +91,9 @@ void setup() { // Add all the tasks taskManager.addTask(&mfd); taskManager.addTask(new IntervalTask(new RunningLightService(), 250)); - taskManager.addTask(new IntervalTask(adcService, cfAdcServiceInterval)); + taskManager.addTask(new IntervalTask(adcService, 1000)); + taskManager.addTask(new IntervalTask(imuService, 50)); + taskManager.addTask(new IntervalTask(baroService, 1000)); taskManager.addTask(n2kService); taskManager.addTask(reader1); taskManager.addTask(reader2); @@ -97,19 +101,11 @@ void setup() { taskManager.addTask(sdcardTask); taskManager.addTask(&usbService); - #ifdef SERVICE_BARO - BarometerService *baroService = new BarometerService(skHub); - taskManager.addTask(new IntervalTask(baroService, cfBaroServiceInterval)); - #endif - - #ifdef SERVICE_IMU - IMUService *imuService = new IMUService(skHub); - taskManager.addTask(new IntervalTask(imuService, cfIMUServiceInterval)); - #ifdef PAGE_IMU - IMUMonitorPage *imuPage = new IMUMonitorPage(skHub, *imuService); - mfd.addPage(imuPage); - #endif - #endif + BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub); + mfd.addPage(batPage); + + IMUMonitorPage *imuPage = new IMUMonitorPage(skHub, *imuService); + mfd.addPage(imuPage); BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub); mfd.addPage(batPage); From dd57cd3ad04b96cc598db3b2970f6fe5d2458e76 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:04:10 +0100 Subject: [PATCH 30/81] original restored w/o config settings, just IMUPage added --- src/host/main.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index db5b4d4c..b532f367 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -107,9 +107,6 @@ void setup() { IMUMonitorPage *imuPage = new IMUMonitorPage(skHub, *imuService); mfd.addPage(imuPage); - BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub); - mfd.addPage(batPage); - StatsPage *statsPage = new StatsPage(); statsPage->setSDCardTask(sdcardTask); From 098756f5ccc7c87f6fbb50e98703926ea1be119d Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:07:37 +0100 Subject: [PATCH 31/81] settings now private, w/o config file, all angle values radians --- src/host/services/IMUService.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 68480c96..401fc90d 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -36,6 +36,9 @@ class IMUService : public Task { double _offsetRoll, _offsetPitch; imu::Vector<3> eulerAngles; + // TODO: change to config setting + uint8_t _cfHdgMinCal, _cfHeelPitchMinCal; + public: IMUService(SKHub& skHub) : Task("IMU"), _skHub(skHub) {}; void setup(); From 1e23958898f612c95974e4a87e27e3c357c17843 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:07:45 +0100 Subject: [PATCH 32/81] settings now private, w/o config file, all angle values radians --- src/host/services/IMUService.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index d81610d0..a2eab62a 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -23,7 +23,6 @@ */ #include -#include "common/config/KBoxConfig.h" #include "common/signalk/SKUpdateStatic.h" #include "common/signalk/SKUnits.h" #include "IMUService.h" @@ -31,6 +30,8 @@ void IMUService::setup() { _offsetRoll = 0.0; _offsetPitch = 0.0; + _cfHdgMinCal = 2; + _cfHeelPitchMinCal = 2; // until config is made, declare default mounting position KBox here uint8_t axisConfig = 0b00001001; @@ -64,12 +65,12 @@ void IMUService::loop() { _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); //DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", SKRadToDeg(_roll), SKRadToDeg(_pitch), SKRadToDeg(_heading)); - if (_magCalib >= cfHdgMinCal) { + if (_magCalib >= _cfHdgMinCal) { update.setNavigationHeadingMagnetic(_heading); _skHub.publish(update); } - if (_accelCalib >= cfHeelPitchMinCal && _gyroCalib >= cfHeelPitchMinCal) { + if (_accelCalib >= _cfHeelPitchMinCal && _gyroCalib >= _cfHeelPitchMinCal) { update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll, /* pitch */ _pitch, /* yaw */ 0)); _skHub.publish(update); } From 841c3de2a3b4ac7c22b2264d5ab8ad7f43f2712a Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:08:06 +0100 Subject: [PATCH 33/81] settings now private, w/o config file, all angle values radians --- src/host/pages/IMUMonitorPage.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index 01a66c75..bbaaa6a5 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -40,9 +40,8 @@ class IMUMonitorPage : public Page, public SKSubscriber { IMUService &_imuService; int _magCalibration, _accelCalibration; - double _pitch; - double _roll; - double _heading; + double _pitch, _roll, _heading; + uint8_t _cfHdgMinCal, _cfHeelPitchMinCal; public: IMUMonitorPage(SKHub& hub, IMUService &imuService); From 94e5f1708b1bbaf39c168014d450d7153897da35 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:08:18 +0100 Subject: [PATCH 34/81] settings now private, w/o config file, all angle values radians --- src/host/pages/IMUMonitorPage.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 8404f4f4..ae5a60fd 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -31,12 +31,15 @@ #include #include "common/signalk/SKUpdate.h" #include "IMUMonitorPage.h" -#include "common/config/KBoxConfig.h" #include #include "signalk/SKUnits.h" IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) : _imuService(imuService) { + + _cfHdgMinCal = 2; + _cfHeelPitchMinCal = 2; + static const int col1 = 5; static const int col2 = 200; static const int row1 = 26; @@ -84,14 +87,14 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ // TODO: Some damping for the display - _hdgTL->setText(String( _heading, 1) + "° "); + _hdgTL->setText(String( SKRadToDeg(_heading), 1) + "° "); _calTL->setText(String( _magCalibration) + "/" + String( _accelCalibration) + " "); - _pitchTL->setText(String( _pitch, 1) + "° "); - _rollTL->setText(String( _roll, 1) + "° "); + _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 change color to Red - if ((_magCalibration <= cfHdgMinCal)||(_accelCalibration <= cfHeelPitchMinCal)) { + if ((_magCalibration <= _cfHdgMinCal)||(_accelCalibration <= _cfHeelPitchMinCal)) { _hdgTL->setColor(ColorRed); _calTL->setColor(ColorRed); } else { From 0e0f54e5601a1e189afdaf9bf9bf7aefb8033609 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:09:22 +0100 Subject: [PATCH 35/81] KBoxConfig deleted as it will come in another PR --- src/common/config/KBoxConfig.cpp | 52 ------------------------- src/common/config/KBoxConfig.h | 67 -------------------------------- 2 files changed, 119 deletions(-) delete mode 100644 src/common/config/KBoxConfig.cpp delete mode 100644 src/common/config/KBoxConfig.h diff --git a/src/common/config/KBoxConfig.cpp b/src/common/config/KBoxConfig.cpp deleted file mode 100644 index c9c47360..00000000 --- a/src/common/config/KBoxConfig.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - __ __ ______ ______ __ __ - /\ \/ / /\ == \ /\ __ \ /\_\_\_\ - \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ - \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ - \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ - - The MIT License - - Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net - - 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 "KBoxConfig.h" - -// --------------------------------------------------------------------------- -// Use of internal Sensors -// --------------------------------------------------------------------------- -// [sensors] -bool cfUseHdgFromBusIfExists = true; // Get Heading from NMEA2000? -bool cfUseHdgFromIMUSensor = true; // Get Heading from internal IMU-Sensor? -bool cfUseHeelPitchFromBus = true; // Get Heel/Pitch from NMEA2000? -bool cfUseHeelPitchFromIMUSensor = true; // Get Heel/Pitch from internal IMU-Sensor? -int8_t cfHdgMinCal = 2; // Minimum Calibration to take HDG from internal sensor -int8_t cfHeelPitchMinCal = 2; // Minimum Calibration to take Heel & Pitch from internal sensor - -// MOUNTED_UPRIGHT_STB_HULL, MOUNTED_UPRIGHT_PORT_HULL, LAYING_READ_DIR_TO_BOW, LAYING_LEFT_TO_BOW -KBoxOrientation cfKBoxOrientation = MOUNTED_UPRIGHT_STB_HULL; - -// --------------------------------------------------------------------------- -// Interval Services -// --------------------------------------------------------------------------- -int16_t cfIMUServiceInterval = 50; -int16_t cfBaroServiceInterval = 1000; -int16_t cfAdcServiceInterval = 1000; diff --git a/src/common/config/KBoxConfig.h b/src/common/config/KBoxConfig.h deleted file mode 100644 index ced3fca8..00000000 --- a/src/common/config/KBoxConfig.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - __ __ ______ ______ __ __ - /\ \/ / /\ == \ /\ __ \ /\_\_\_\ - \ \ _"-. \ \ __< \ \ \/\ \ \/_/\_\/_ - \ \_\ \_\ \ \_____\ \ \_____\ /\_\/\_\ - \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ - - The MIT License - - Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net - - 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. -*/ - -#pragma once -#include - -// --------------------------------------------------------------------------- -// Define different tasks and pages -// if you don't like a task or page just comment it out -// --------------------------------------------------------------------------- -#define SERVICE_BARO -#define SERVICE_IMU - -#define PAGE_IMU - -// --------------------------------------------------------------------------- -// Use of internal Sensors -// --------------------------------------------------------------------------- -// [sensors] -extern bool cfUseHdgFromBusIfExists; // Get Heading from NMEA2000? -extern bool cfUseHdgFromIMUSensor; // Get Heading from internal IMU-Sensor? -extern bool cfUseHeelPitchFromBus; // Get Heel/Pitch from NMEA2000? -extern bool cfUseHeelPitchFromIMUSensor; // Get Heel/Pitch from internal IMU-Sensor? -extern int8_t cfHdgMinCal; // Minimum Calibration to take HDG from internal sensor -extern int8_t cfHeelPitchMinCal; // Minimum Calibration to take Heel & Pitch from internal sensor - -enum KBoxOrientation { - MOUNTED_UPRIGHT_STB_HULL, - MOUNTED_UPRIGHT_PORT_HULL, - LAYING_READ_DIR_TO_BOW, - LAYING_LEFT_TO_BOW -}; -extern KBoxOrientation cfKBoxOrientation; - -// --------------------------------------------------------------------------- -// Interval Services -// --------------------------------------------------------------------------- -extern int16_t cfIMUServiceInterval; -extern int16_t cfBaroServiceInterval; -extern int16_t cfAdcServiceInterval; From 35e1662c21c130b8d0bd069dbc0e31e5451f16d7 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:23:32 +0100 Subject: [PATCH 36/81] back to Sarfatas BNO055 library but with extended begin() function --- lib/Adafruit_BNO055/Adafruit_BNO055.cpp | 69 ------------------------- 1 file changed, 69 deletions(-) diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp index e786979b..d84d682e 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp +++ b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp @@ -53,75 +53,6 @@ Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) PUBLIC FUNCTIONS ***************************************************************************/ -/**************************************************************************/ -/*! - @brief Sets up the HW -*/ -/**************************************************************************/ -bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) -{ - /* Enable I2C */ - Wire.begin(); - - // BNO055 clock stretches for 500us or more! -#ifdef ESP8266 - Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching -#endif - - /* Make sure we have the right device */ - uint8_t id = read8(BNO055_CHIP_ID_ADDR); - if(id != BNO055_ID) - { - delay(1000); // hold on for boot - id = read8(BNO055_CHIP_ID_ADDR); - if(id != BNO055_ID) { - return false; // still not? ok bail - } - } - - /* Switch to config mode (just in case since this is the default) */ - setMode(OPERATION_MODE_CONFIG); - - /* Reset */ - write8(BNO055_SYS_TRIGGER_ADDR, 0x20); - while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) - { - delay(10); - } - delay(50); - - /* Set to normal power mode */ - write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); - delay(10); - - write8(BNO055_PAGE_ID_ADDR, 0); - - /* Set the output units */ - /* - uint8_t unitsel = (0 << 7) | // Orientation = Android - (0 << 4) | // Temperature = Celsius - (0 << 2) | // Euler = Degrees - (1 << 1) | // Gyro = Rads - (0 << 0); // Accelerometer = m/s^2 - 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 - delay(10); - 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) */ - setMode(mode); - delay(20); - - return true; -} /**************************************************************************/ /*! From 55f0e11bcdd1307e7515469e7442c6d5e558bbf4 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:23:40 +0100 Subject: [PATCH 37/81] back to Sarfatas BNO055 library but with extended begin() function --- lib/Adafruit_BNO055/Adafruit_BNO055.h | 30 +++++++++++---------------- 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.h b/lib/Adafruit_BNO055/Adafruit_BNO055.h index dae1a1fd..38e754f4 100644 --- a/lib/Adafruit_BNO055/Adafruit_BNO055.h +++ b/lib/Adafruit_BNO055/Adafruit_BNO055.h @@ -1,17 +1,19 @@ /*************************************************************************** - -Edited Library for KBox use! - - This is a library for the BNO055 orientation sensor + Designed specifically to work with the Adafruit BNO055 Breakout. + Pick one up today in the adafruit shop! ------> http://www.adafruit.com/products + These sensors use I2C to communicate, 2 pins are required to interface. + Adafruit invests time and resources providing this open source code, please support Adafruit andopen-source hardware by purchasing products from Adafruit! + Written by KTOWN for Adafruit Industries. + MIT license, all text above must be included in any redistribution ***************************************************************************/ @@ -24,14 +26,12 @@ Edited Library for KBox use! #include "WProgram.h" #endif -/* Disabled, not needed for KBox -#ifdef __AVR_ATtiny85__ - #include - #define Wire TinyWireM -#else - #include -#endif -*/ +//#ifdef __AVR_ATtiny85__ + //#include + //#define Wire TinyWireM +//#else + //#include +//#endif #include #include @@ -279,13 +279,7 @@ class Adafruit_BNO055 : public Adafruit_Sensor VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR } adafruit_vector_type_t; -#if defined (ARDUINO_SAMD_ZERO) && ! (ARDUINO_SAMD_FEATHER_M0) -#error "On an arduino Zero, BNO055's ADR pin must be high. Fix that, then delete this line." - Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_B ); -#else Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A ); -#endif - bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF ); // 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, From 6f3652c32aca0c6c6faf017295d2b5010672794f Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:29:34 +0100 Subject: [PATCH 38/81] removed unnecessary include --- src/host/services/IMUService.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 401fc90d..9e1512a8 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -25,7 +25,6 @@ #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" -#include class IMUService : public Task { private: From 7c7f608835d31e32d9381999696d94743fa9f994 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:34:16 +0100 Subject: [PATCH 39/81] extra lines removed --- src/host/pages/IMUMonitorPage.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index ae5a60fd..db83dbde 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -67,7 +67,6 @@ IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) : _imuService } bool IMUMonitorPage::processEvent(const ButtonEvent &be){ - // DEBUG("EventTypeButton: %i", be.clickType); if (be.clickType == ButtonEventTypeClick) { DEBUG("Button ButtonEventTypeClick !!!"); @@ -81,15 +80,12 @@ bool IMUMonitorPage::processEvent(const ButtonEvent &be){ } 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) + "° "); From 406aee70d0f2af378b049d1d6a201eb6143c332c Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 12:40:32 +0100 Subject: [PATCH 40/81] moved #include here --- src/host/services/IMUService.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index a2eab62a..6eae268d 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -26,6 +26,7 @@ #include "common/signalk/SKUpdateStatic.h" #include "common/signalk/SKUnits.h" #include "IMUService.h" +#include void IMUService::setup() { _offsetRoll = 0.0; From add7009e4604e0c121c4e7c57386ce02a1f53cff Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Thu, 14 Dec 2017 13:52:21 +0100 Subject: [PATCH 41/81] Removed bug in calculating offset, some comments added --- src/host/services/IMUService.cpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 6eae268d..74fcbd54 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -50,48 +50,51 @@ void IMUService::setup() { void IMUService::loop() { bno055.getCalibration(&_sysCalib, &_gyroCalib, &_accelCalib, &_magCalib); + //DEBUG("Calib Sys: %i Accel: %i Gyro: %i Mag: %i", _sysCalib, _accelCalib, _gyroCalib, _magCalib); eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - - //DEBUG("Calib Sys: %i Accel: %i Gyro: %i Mag: %i", _sysCalib, _accelCalib, _gyroCalib, _magCalib); - //DEBUG("Attitude roll: %.3f pitch: %.3f Mag heading: %.3f", eulerAngles.z(), eulerAngles.y(), eulerAngles.x()); + //DEBUG("Sensor Raw-datas: eulerAngles.z=%.3f eulerAngles.y=%.3f eulerAngles.x=%.3f", eulerAngles.z(), eulerAngles.y(), eulerAngles.x()); SKUpdateStatic<2> update; // Note: We could calculate yaw as the difference between the Magnetic // Heading and the Course Over Ground Magnetic. + // TODO: different calcs according to mounting position /* if orientation == MOUNTED_ON_STARBOARD_HULL */ - _roll = SKDegToRad(eulerAngles.z() + _offsetRoll); - _pitch = SKDegToRad(eulerAngles.y() + _offsetPitch); + _roll = SKDegToRad(eulerAngles.z()); + _pitch = SKDegToRad(eulerAngles.y()); _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); - //DEBUG("Attitude heel: %.3f pitch: %.3f Mag heading: %.3f", SKRadToDeg(_roll), SKRadToDeg(_pitch), SKRadToDeg(_heading)); + // update NavigationHeadingMagnetic if quality (= calibration value) is ok if (_magCalib >= _cfHdgMinCal) { update.setNavigationHeadingMagnetic(_heading); _skHub.publish(update); } - + // update NavigationAttitude if quality (= calibration value) is ok if (_accelCalib >= _cfHeelPitchMinCal && _gyroCalib >= _cfHeelPitchMinCal) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll, /* pitch */ _pitch, /* yaw */ 0)); + update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ 0)); + //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); _skHub.publish(update); } } +// get the actual values of calibration, heel & pitch (including offset) and heading for display +// all angles in radians void IMUService::getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading){ accelCalibration = _accelCalib; - pitch = _pitch; - roll = _roll; + pitch = _pitch + _offsetPitch; + roll = _roll + _offsetRoll; magCalibration = _magCalib; heading = _heading; } -// Offset for Heel=0 and Pitch=0 with long button press in IMUMonitorPage +// Calc offset for making Heel=0 and Pitch=0 (e.g. called with long button press in IMUMonitorPage) void IMUService::setOffset() { - // FIXIT: Why it is called at startup of KBox? event longClick coming? + // FIXIT: Why is it called at startup of KBox? event longClick coming? if ( millis() > 10000 ) { _offsetRoll = _roll * (-1); _offsetPitch = _pitch * (-1); - DEBUG("Offset changed: Heel -> %.3f | Pitch -> %.3f", SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); + INFO("Offset changed: Heel -> %.3f | Pitch -> %.3f", SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); // TODO: angle of heel and pitch must be smaller than 90°, check overflow! } } From 16d732aaee5b561e4f073e29149c6a0ca5f6fb87 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Wed, 27 Dec 2017 16:00:51 +0200 Subject: [PATCH 42/81] NaN for not available values in N2k and SKUpdate --- src/common/signalk/SKUnits.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/signalk/SKUnits.h b/src/common/signalk/SKUnits.h index d5438eea..bbc473d6 100644 --- a/src/common/signalk/SKUnits.h +++ b/src/common/signalk/SKUnits.h @@ -57,4 +57,4 @@ inline double SKNormalizeAngle(double x) { return x - M_PI; } - +const double SKDoubleNAN = -1e9; From 7ff16fcf57420288ceba16b356c05ff17ecad5aa Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 11:03:34 +0200 Subject: [PATCH 43/81] TODO deleted as SDIO has no sdcard_cs --- lib/KBoxHardware/src/KBoxHardware.cpp | 1 - 1 file changed, 1 deletion(-) 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()){ From ab61dcce8ce032fbe3d1d9d015357a6769d0217a Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 11:04:07 +0200 Subject: [PATCH 44/81] more config for IMU-sensor added --- src/host/config/IMUConfig.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/host/config/IMUConfig.h b/src/host/config/IMUConfig.h index 27b3833b..9292aa31 100644 --- a/src/host/config/IMUConfig.h +++ b/src/host/config/IMUConfig.h @@ -33,5 +33,8 @@ struct IMUConfig { bool enabled; int frequency; + bool enableHdg; + bool enableHeelPitch; + int calHdg; + int calHeelPitch; }; - From 51247f01706874dd8c242b25cc91e1ba32d62acd Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 15:01:17 +0200 Subject: [PATCH 45/81] more IMU-sensor config settings --- extras/config/default-config.json | 7 +++++-- src/host/config/KBoxConfigParser.cpp | 11 ++++++++++- src/test/config/KBoxConfigParserTest.cpp | 5 +++++ 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/extras/config/default-config.json b/extras/config/default-config.json index 5fc29c11..c1de92c8 100644 --- a/extras/config/default-config.json +++ b/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 } } - diff --git a/src/host/config/KBoxConfigParser.cpp b/src/host/config/KBoxConfigParser.cpp index dd59c770..24844b42 100644 --- a/src/host/config/KBoxConfigParser.cpp +++ b/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; -} \ No newline at end of file +} diff --git a/src/test/config/KBoxConfigParserTest.cpp b/src/test/config/KBoxConfigParserTest.cpp index b96894c9..9d7dbf15 100644 --- a/src/test/config/KBoxConfigParserTest.cpp +++ b/src/test/config/KBoxConfigParserTest.cpp @@ -61,6 +61,11 @@ 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 ); + CHECK( config.imuConfig.calHdg == 3 ); + CHECK( config.imuConfig.calHeelPitch == 2 ); } SECTION("basic config") { From ef4e46a2c5759520ded2c872f9ed526790c57797 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 15:26:56 +0200 Subject: [PATCH 46/81] some corrections for new config --- src/host/services/IMUService.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 9e1512a8..64e0cff3 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -25,6 +25,7 @@ #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" +#include "host/config/SerialConfig.h" class IMUService : public Task { private: @@ -34,9 +35,7 @@ class IMUService : public Task { double _roll, _pitch, _heading; double _offsetRoll, _offsetPitch; imu::Vector<3> eulerAngles; - - // TODO: change to config setting - uint8_t _cfHdgMinCal, _cfHeelPitchMinCal; + SerialConfig &_config; public: IMUService(SKHub& skHub) : Task("IMU"), _skHub(skHub) {}; From d9311b5075b92dcf79111080fe0f3f4dc82fc669 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 15:31:52 +0200 Subject: [PATCH 47/81] correction for new config --- src/host/services/IMUService.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 74fcbd54..c484a79d 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -31,10 +31,8 @@ void IMUService::setup() { _offsetRoll = 0.0; _offsetPitch = 0.0; - _cfHdgMinCal = 2; - _cfHeelPitchMinCal = 2; - // until config is made, declare default mounting position KBox here + // until config for positioning is made, declare default mounting position KBox here uint8_t axisConfig = 0b00001001; uint8_t signConfig = 0b00000000; @@ -66,7 +64,7 @@ void IMUService::loop() { _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); // update NavigationHeadingMagnetic if quality (= calibration value) is ok - if (_magCalib >= _cfHdgMinCal) { + if (_magCalib >= _config.imu) { update.setNavigationHeadingMagnetic(_heading); _skHub.publish(update); } From fe6c5cf2b8c3f4713ddb55cd8dd4eb02b4ade641 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 17:54:08 +0200 Subject: [PATCH 48/81] IMUMonitorPage if IMU enabled, imuConfig added --- src/host/main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index 36e6f338..d06d6121 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -99,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); @@ -135,8 +135,11 @@ void setup() { BatteryMonitorPage *batPage = new BatteryMonitorPage(skHub); mfd.addPage(batPage); - IMUMonitorPage *imuPage = new IMUMonitorPage(skHub, *imuService); - mfd.addPage(imuPage); + 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); From f37782a7fa27ec61b2cecb2ff00e714561dbfcdc Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 17:55:12 +0200 Subject: [PATCH 49/81] IMUConfig for settings of IMU-cal values added --- src/host/services/IMUService.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 64e0cff3..77be7290 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -25,20 +25,20 @@ #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" -#include "host/config/SerialConfig.h" +#include "host/config/IMUConfig.h" class IMUService : public Task { private: + IMUConfig &_config; SKHub &_skHub; Adafruit_BNO055 bno055; uint8_t _sysCalib, _gyroCalib, _accelCalib, _magCalib; double _roll, _pitch, _heading; double _offsetRoll, _offsetPitch; imu::Vector<3> eulerAngles; - SerialConfig &_config; public: - IMUService(SKHub& skHub) : Task("IMU"), _skHub(skHub) {}; + IMUService(IMUConfig &config, SKHub& skHub) : Task("IMU"), _config(config), _skHub(skHub) {}; void setup(); void loop(); From 8e03c94d9b4d94c1cd51c95000e148e1432804d5 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 17:55:20 +0200 Subject: [PATCH 50/81] IMUConfig for settings of IMU-cal values added --- src/host/services/IMUService.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index c484a79d..163b0e89 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -64,12 +64,12 @@ void IMUService::loop() { _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); // update NavigationHeadingMagnetic if quality (= calibration value) is ok - if (_magCalib >= _config.imu) { + if (_magCalib >= _config.calHdg) { update.setNavigationHeadingMagnetic(_heading); _skHub.publish(update); } // update NavigationAttitude if quality (= calibration value) is ok - if (_accelCalib >= _cfHeelPitchMinCal && _gyroCalib >= _cfHeelPitchMinCal) { + if (_accelCalib >= _config.calHeelPitch && _gyroCalib >= _config.calHeelPitch) { update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ 0)); //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); _skHub.publish(update); From 2c0b16fa282b545b4165b033ddf6c570d86035a8 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 17:55:46 +0200 Subject: [PATCH 51/81] IMUConfig for settings of IMU-cal values added --- src/host/pages/IMUMonitorPage.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index bbaaa6a5..6d8ce065 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -33,18 +33,19 @@ #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; - uint8_t _cfHdgMinCal, _cfHeelPitchMinCal; public: - IMUMonitorPage(SKHub& hub, IMUService &imuService); + IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); virtual void updateReceived(const SKUpdate& up); bool processEvent(const TickEvent &te); From d68ec3d10d3cdbcdaf48989f9d70bb5ca12c6652 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Fri, 29 Dec 2017 17:55:55 +0200 Subject: [PATCH 52/81] IMUConfig for settings of IMU-cal values added --- src/host/pages/IMUMonitorPage.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index db83dbde..823ef490 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -34,11 +34,7 @@ #include #include "signalk/SKUnits.h" - -IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) : _imuService(imuService) { - - _cfHdgMinCal = 2; - _cfHeelPitchMinCal = 2; +IMUMonitorPage::IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService) : _config(config), _imuService(imuService) { static const int col1 = 5; static const int col2 = 200; @@ -63,7 +59,6 @@ IMUMonitorPage::IMUMonitorPage(SKHub& hub, IMUService &imuService) : _imuService addLayer(_pitchTL); hub.subscribe(this); - } bool IMUMonitorPage::processEvent(const ButtonEvent &be){ @@ -89,8 +84,9 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ _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 change color to Red - if ((_magCalibration <= _cfHdgMinCal)||(_accelCalibration <= _cfHeelPitchMinCal)) { + // 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 { From cc7127318aebc3d12e06911725d1cadfc19c1390 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sat, 30 Dec 2017 20:22:50 +0200 Subject: [PATCH 53/81] config for different KBox mounting positions added --- extras/config/default-config.json | 3 +- src/host/config/IMUConfig.h | 10 +++++++ src/host/config/KBoxConfigParser.cpp | 15 ++++++++++ src/host/config/KBoxConfigParser.h | 1 + src/host/services/IMUService.cpp | 37 ++++++++++++++++++------ src/test/config/KBoxConfigParserTest.cpp | 4 ++- 6 files changed, 59 insertions(+), 11 deletions(-) diff --git a/extras/config/default-config.json b/extras/config/default-config.json index c1de92c8..297ce9a5 100644 --- a/extras/config/default-config.json +++ b/extras/config/default-config.json @@ -5,7 +5,8 @@ "enableHdg": true, "enableHeelPitch": true, "calHdg": 3, - "calHeelPitch": 2 + "calHeelPitch": 2, + "mounting": "verticalStarboardHull" }, "barometer": { "enabled": true, diff --git a/src/host/config/IMUConfig.h b/src/host/config/IMUConfig.h index 9292aa31..3b1b4b80 100644 --- a/src/host/config/IMUConfig.h +++ b/src/host/config/IMUConfig.h @@ -30,6 +30,15 @@ #pragma once +enum IMUMounting { + VerticalStbHull, + VerticalPortHull, + VerticalTopToBow, + VerticalTopToAft, + HorizontalLeftSideToBow, + HorizontalRightSideToBow +}; + struct IMUConfig { bool enabled; int frequency; @@ -37,4 +46,5 @@ struct IMUConfig { bool enableHeelPitch; int calHdg; int calHeelPitch; + enum IMUMounting mounting = VerticalStbHull; }; diff --git a/src/host/config/KBoxConfigParser.cpp b/src/host/config/KBoxConfigParser.cpp index 24844b42..4a75912d 100644 --- a/src/host/config/KBoxConfigParser.cpp +++ b/src/host/config/KBoxConfigParser.cpp @@ -59,6 +59,7 @@ void KBoxConfigParser::defaultConfig(KBoxConfig &config) { 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.imuConfig.mounting = VerticalStbHull; config.barometerConfig.enabled = true; config.barometerConfig.frequency = 1; @@ -84,6 +85,7 @@ void KBoxConfigParser::parseIMUConfig(const JsonObject &json, IMUConfig &config) READ_BOOL_VALUE(enableHeelPitch); READ_INT_VALUE_WRANGE(calHdg, 0, 3); READ_INT_VALUE_WRANGE(calHeelPitch, 0, 3); + READ_ENUM_VALUE(mounting, convertIMUMounting); } void KBoxConfigParser::parseBarometerConfig(const JsonObject &json, BarometerConfig &config){ @@ -141,3 +143,16 @@ enum SerialMode KBoxConfigParser::convertSerialMode(const String &s) { } return SerialModeDisabled; } + +enum IMUMounting KBoxConfigParser::convertIMUMounting(const String &s) { + if (s == "verticalStarboardHull") { + return VerticalStbHull; + } + if (s == "verticalTopToBow") { + return VerticalTopToBow; + } + if (s == "horizontalLeftSideToBow") { + return HorizontalLeftSideToBow; + } + return VerticalStbHull; +} 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/services/IMUService.cpp b/src/host/services/IMUService.cpp index 163b0e89..ff25c7cd 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -31,11 +31,19 @@ void IMUService::setup() { _offsetRoll = 0.0; _offsetPitch = 0.0; - - // until config for positioning is made, declare default mounting position KBox here + // Default mounting position: VerticalStbHull uint8_t axisConfig = 0b00001001; uint8_t signConfig = 0b00000000; + if (_config.mounting == VerticalTopToBow) { + axisConfig = 0b00001001; + signConfig = 0b00000000; + } + if (_config.mounting == HorizontalLeftSideToBow) { + axisConfig = 0b00100100; + signConfig = 0b00000000; + } + DEBUG("Initing BNO055"); if (!bno055.begin(bno055.OPERATION_MODE_NDOF, axisConfig, signConfig)) { DEBUG("Error initializing BNO055"); @@ -57,11 +65,22 @@ void IMUService::loop() { // Note: We could calculate yaw as the difference between the Magnetic // Heading and the Course Over Ground Magnetic. - // TODO: different calcs according to mounting position - /* if orientation == MOUNTED_ON_STARBOARD_HULL */ - _roll = SKDegToRad(eulerAngles.z()); - _pitch = SKDegToRad(eulerAngles.y()); - _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); + if (_config.mounting == VerticalStbHull) { + _roll = SKDegToRad(eulerAngles.z()); + _pitch = SKDegToRad(eulerAngles.y()); + _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); + } + if (_config.mounting == VerticalTopToBow) { + //TODO: check position for CaptainRon47 + _roll = SKDegToRad(eulerAngles.y()); + _pitch = SKDegToRad(eulerAngles.z()); + _heading = SKDegToRad(fmod(eulerAngles.x() + 180, 360)); + } + if (_config.mounting == HorizontalLeftSideToBow) { + _roll = SKDegToRad(eulerAngles.y()); + _pitch = SKDegToRad(eulerAngles.z()) * (-1); + _heading = SKDegToRad(eulerAngles.x()); + } // update NavigationHeadingMagnetic if quality (= calibration value) is ok if (_magCalib >= _config.calHdg) { @@ -70,7 +89,7 @@ void IMUService::loop() { } // update NavigationAttitude if quality (= calibration value) is ok if (_accelCalib >= _config.calHeelPitch && _gyroCalib >= _config.calHeelPitch) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ 0)); + update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ SKDoubleNAN)); //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); _skHub.publish(update); } @@ -104,7 +123,7 @@ bool IMUService::saveCalibration() { adafruit_bno055_offsets_t newCalib; bno055.getSensorOffsets(newCalib); - DEBUG("\n\nStoring calibration data to EEPROM..."); + DEBUG("Storing calibration data to EEPROM..."); bno055.getSensor(&sensor); bnoID = sensor.sensor_id; diff --git a/src/test/config/KBoxConfigParserTest.cpp b/src/test/config/KBoxConfigParserTest.cpp index 9d7dbf15..37d4fcf2 100644 --- a/src/test/config/KBoxConfigParserTest.cpp +++ b/src/test/config/KBoxConfigParserTest.cpp @@ -71,7 +71,7 @@ TEST_CASE("KBoxConfigParser") { 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); @@ -87,6 +87,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") { From 621c5d050716d984563f8928878156f97f3698e8 Mon Sep 17 00:00:00 2001 From: Ronnie Zeiller Date: Sun, 31 Dec 2017 13:32:20 +0200 Subject: [PATCH 54/81] ident to tab 2 From a818895534c5f3d84134b0b501f1fb75eca10191 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Sun, 31 Dec 2017 15:59:53 +0200 Subject: [PATCH 55/81] small corrections, config for mounting --- src/host/pages/IMUMonitorPage.cpp | 3 +-- src/host/pages/IMUMonitorPage.h | 2 +- src/host/services/IMUService.cpp | 7 +++---- src/host/services/IMUService.h | 12 +++++++++++- 4 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 823ef490..67850afb 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -76,7 +76,6 @@ bool IMUMonitorPage::processEvent(const ButtonEvent &be){ 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) + "° "); @@ -86,7 +85,7 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ // 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)) { + if ( ! _imuService.isMagCalibrated() || ! _imuService.isHeelAndPitchCalibrated()) { _hdgTL->setColor(ColorRed); _calTL->setColor(ColorRed); } else { diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index 6d8ce065..ab0f6894 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -40,7 +40,6 @@ class IMUMonitorPage : public Page, public SKSubscriber { TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; IMUConfig &_config; IMUService &_imuService; - int _magCalibration, _accelCalibration; double _pitch, _roll, _heading; @@ -50,4 +49,5 @@ class IMUMonitorPage : public Page, public SKSubscriber { 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 ff25c7cd..f6038342 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -83,16 +83,15 @@ void IMUService::loop() { } // update NavigationHeadingMagnetic if quality (= calibration value) is ok - if (_magCalib >= _config.calHdg) { + if (isMagCalibrated()) { update.setNavigationHeadingMagnetic(_heading); - _skHub.publish(update); } // update NavigationAttitude if quality (= calibration value) is ok - if (_accelCalib >= _config.calHeelPitch && _gyroCalib >= _config.calHeelPitch) { + if (isHeelAndPitchCalibrated()) { update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ SKDoubleNAN)); //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); - _skHub.publish(update); } + _skHub.publish(update); } // get the actual values of calibration, heel & pitch (including offset) and heading for display diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 77be7290..f85cf287 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -42,9 +42,19 @@ class IMUService : public Task { void setup(); void loop(); - void getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); void resetIMU(); bool recallCalibration(); bool saveCalibration(); void setOffset(); + + bool isMagCalibrated() { + return _magCalib == 3; + } + + bool isHeelAndPitchCalibrated() { + return _accelCalib >= 2 && _gyroCalib >= 2; + } + + void getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); + }; From 4edf570b0a723f1efdc77cc0d35ea6abe5920aab Mon Sep 17 00:00:00 2001 From: Ronnie Zeiller Date: Mon, 1 Jan 2018 19:34:15 +0200 Subject: [PATCH 56/81] indent corrected From cb01893150f4710f3163f17e1a6fa9172e125092 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:41:14 +0200 Subject: [PATCH 57/81] indention corrected --- src/host/pages/IMUMonitorPage.h | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index ab0f6894..08035bca 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -36,18 +36,18 @@ #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; + private: + TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; + IMUConfig &_config; + IMUService &_imuService; - public: - IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); - virtual void updateReceived(const SKUpdate& up); + int _magCalibration, _accelCalibration; + double _pitch, _roll, _heading; - bool processEvent(const TickEvent &te); - bool processEvent(const ButtonEvent &be); + public: + IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); + virtual void updateReceived(const SKUpdate& up); + bool processEvent(const TickEvent &te); + bool processEvent(const ButtonEvent &be); }; From f2aa14dbd5781fd8ef66b207af193ed81fac8bf9 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:50:39 +0200 Subject: [PATCH 58/81] indent corrected, not used function deleted --- src/host/pages/IMUMonitorPage.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 67850afb..2db64668 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -84,7 +84,7 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ _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 + // calibration below default setting, change color to red if ( ! _imuService.isMagCalibrated() || ! _imuService.isHeelAndPitchCalibrated()) { _hdgTL->setColor(ColorRed); _calTL->setColor(ColorRed); @@ -95,7 +95,3 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ return true; } - -void IMUMonitorPage::updateReceived(const SKUpdate& up) { - // may be needed for something.... -} From 333242ecabe71ea234b45be1a1e8f45e83b782f0 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:50:48 +0200 Subject: [PATCH 59/81] indent corrected, not used function deleted --- src/host/pages/IMUMonitorPage.h | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index 08035bca..2851790e 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -37,17 +37,16 @@ class IMUMonitorPage : public Page, public SKSubscriber { private: - TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; - IMUConfig &_config; - IMUService &_imuService; + TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; + IMUConfig &_config; + IMUService &_imuService; - int _magCalibration, _accelCalibration; - double _pitch, _roll, _heading; + int _magCalibration, _accelCalibration; + double _pitch, _roll, _heading; - public: - IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); - virtual void updateReceived(const SKUpdate& up); + public: + IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); - bool processEvent(const TickEvent &te); - bool processEvent(const ButtonEvent &be); + bool processEvent(const TickEvent &te); + bool processEvent(const ButtonEvent &be); }; From 69007d208ca6b06b2b444316eec2f8a4ee93e15e Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:52:03 +0200 Subject: [PATCH 60/81] IMU calibration values deleted from config --- extras/config/default-config.json | 2 -- 1 file changed, 2 deletions(-) diff --git a/extras/config/default-config.json b/extras/config/default-config.json index 297ce9a5..e6273619 100644 --- a/extras/config/default-config.json +++ b/extras/config/default-config.json @@ -4,8 +4,6 @@ "frequency": 20, "enableHdg": true, "enableHeelPitch": true, - "calHdg": 3, - "calHeelPitch": 2, "mounting": "verticalStarboardHull" }, "barometer": { From 03c048cc0eb589ee6f3314ba28110a1cf4d41f78 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:52:50 +0200 Subject: [PATCH 61/81] IMU calibration values deleted from config --- src/host/config/IMUConfig.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/host/config/IMUConfig.h b/src/host/config/IMUConfig.h index 3b1b4b80..aa138325 100644 --- a/src/host/config/IMUConfig.h +++ b/src/host/config/IMUConfig.h @@ -44,7 +44,5 @@ struct IMUConfig { int frequency; bool enableHdg; bool enableHeelPitch; - int calHdg; - int calHeelPitch; enum IMUMounting mounting = VerticalStbHull; }; From dc36c58fd2f867d2ad3dd20ecc26c65a25d6d5f2 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:53:48 +0200 Subject: [PATCH 62/81] IMU calibration values deleted from config --- src/host/config/KBoxConfigParser.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/host/config/KBoxConfigParser.cpp b/src/host/config/KBoxConfigParser.cpp index 4a75912d..ab611fee 100644 --- a/src/host/config/KBoxConfigParser.cpp +++ b/src/host/config/KBoxConfigParser.cpp @@ -57,8 +57,6 @@ 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.calHdg = 3; // hdg valid, if calibration value greater equal - config.imuConfig.calHeelPitch = 2; // heel (roll) & pitch valid, if calibration value greater equal config.imuConfig.mounting = VerticalStbHull; config.barometerConfig.enabled = true; @@ -83,8 +81,6 @@ void KBoxConfigParser::parseIMUConfig(const JsonObject &json, IMUConfig &config) 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); READ_ENUM_VALUE(mounting, convertIMUMounting); } From 2c87d62613089595a492ca0972fd11a494d0dce6 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 19:54:32 +0200 Subject: [PATCH 63/81] IMU calibration values deleted from config --- src/test/config/KBoxConfigParserTest.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/test/config/KBoxConfigParserTest.cpp b/src/test/config/KBoxConfigParserTest.cpp index 37d4fcf2..cbc13143 100644 --- a/src/test/config/KBoxConfigParserTest.cpp +++ b/src/test/config/KBoxConfigParserTest.cpp @@ -64,8 +64,6 @@ TEST_CASE("KBoxConfigParser") { CHECK( config.imuConfig.enableHdg ); CHECK( config.imuConfig.enableHeelPitch ); - CHECK( config.imuConfig.calHdg == 3 ); - CHECK( config.imuConfig.calHeelPitch == 2 ); } SECTION("basic config") { From 019d7e7678888daa8f87831faf4316670ebe27f3 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 20:00:35 +0200 Subject: [PATCH 64/81] put back updateReceived due to error message.... --- src/host/pages/IMUMonitorPage.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 2db64668..402d6528 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -95,3 +95,7 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ return true; } + +void IMUMonitorPage::updateReceived(const SKUpdate& up) { + // may be needed for something.... +} From 2be63c3a7dab5a83355bb2ca15f8b379f8ba6270 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 20:00:46 +0200 Subject: [PATCH 65/81] put back updateReceived due to error message.... --- src/host/pages/IMUMonitorPage.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index 2851790e..f0f9804d 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -47,6 +47,8 @@ class IMUMonitorPage : public Page, public SKSubscriber { public: IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); + virtual void updateReceived(const SKUpdate& up); + bool processEvent(const TickEvent &te); bool processEvent(const ButtonEvent &be); }; From 2abe954c4c730e20bb0d521d149d452cee5101fc Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 20:06:01 +0200 Subject: [PATCH 66/81] not needed debug msg deleted --- src/host/pages/IMUMonitorPage.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 402d6528..7e5f9051 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -62,9 +62,7 @@ IMUMonitorPage::IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuSer } 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; } From 9b18505dd08798eccab3c58713b257d265f0f905 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Mon, 1 Jan 2018 20:25:57 +0200 Subject: [PATCH 67/81] prevent overflow by setOffset --- src/host/services/IMUService.cpp | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index f6038342..a23f9b09 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -104,14 +104,25 @@ void IMUService::getLastValues(int &accelCalibration, double &pitch, double &rol heading = _heading; } -// Calc offset for making Heel=0 and Pitch=0 (e.g. called with long button press in IMUMonitorPage) +// Calc offset for making Heel=0 and Pitch=0 +// (e.g. called with long button press in IMUMonitorPage) +// TODO: 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::setOffset() { + bool changed = false; // FIXIT: Why is it called at startup of KBox? event longClick coming? if ( millis() > 10000 ) { - _offsetRoll = _roll * (-1); - _offsetPitch = _pitch * (-1); - INFO("Offset changed: Heel -> %.3f | Pitch -> %.3f", SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); - // TODO: angle of heel and pitch must be smaller than 90°, check overflow! + 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)); + } } } From e1c285a1c585f8a7f924654815fbbdfac83fe89b Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Tue, 2 Jan 2018 22:56:39 +0200 Subject: [PATCH 68/81] read/write BNO055 Calibration and heel/pitch offset from/to EEPROM --- src/host/pages/IMUMonitorPage.cpp | 8 +- src/host/pages/IMUMonitorPage.h | 3 +- src/host/services/IMUService.cpp | 131 ++++++++++++++++++---------- src/host/services/IMUService.h | 20 +++-- src/host/util/PersistentStorage.cpp | 36 ++++++++ src/host/util/PersistentStorage.h | 33 ++++++- 6 files changed, 170 insertions(+), 61 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 7e5f9051..2017e81c 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -44,7 +44,7 @@ IMUMonitorPage::IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuSer 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(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)); @@ -67,17 +67,17 @@ bool IMUMonitorPage::processEvent(const ButtonEvent &be){ return false; } if (be.clickType == ButtonEventTypeLongClick) { - _imuService.setOffset(); + _imuService.setHeelPitchOffset(); } return true; } bool IMUMonitorPage::processEvent(const TickEvent &te){ - _imuService.getLastValues(_accelCalibration, _pitch, _roll, _magCalibration, _heading); + _imuService.getLastValues(_sysCalibration, _accelCalibration, _pitch, _roll, _magCalibration, _heading); // TODO: Some damping for the display _hdgTL->setText(String( SKRadToDeg(_heading), 1) + "° "); - _calTL->setText(String( _magCalibration) + "/" + String( _accelCalibration) + " "); + _calTL->setText(String( _sysCalibration) + "/" + String( _magCalibration) + "/" + String( _accelCalibration) + " "); _pitchTL->setText(String( SKRadToDeg(_pitch), 1) + "° "); _rollTL->setText(String( SKRadToDeg(_roll), 1) + "° "); diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index f0f9804d..35694014 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -33,6 +33,7 @@ #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 { @@ -41,7 +42,7 @@ class IMUMonitorPage : public Page, public SKSubscriber { IMUConfig &_config; IMUService &_imuService; - int _magCalibration, _accelCalibration; + int _magCalibration, _accelCalibration, _sysCalibration; double _pitch, _roll, _heading; public: diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index a23f9b09..6d036057 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -26,9 +26,10 @@ #include "common/signalk/SKUpdateStatic.h" #include "common/signalk/SKUnits.h" #include "IMUService.h" -#include +#include "host/util/PersistentStorage.h" void IMUService::setup() { + _timeSinceLastSetOffset = 0; _offsetRoll = 0.0; _offsetPitch = 0.0; // Default mounting position: VerticalStbHull @@ -49,8 +50,9 @@ void IMUService::setup() { DEBUG("Error initializing BNO055"); } else { - if (recallCalibration()) - DEBUG("Success!"); + // read calibration and offset values for heel & pitch from flash + readCalibrationOffsets(); + readHeelPitchOffset(); } } @@ -92,11 +94,18 @@ void IMUService::loop() { //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); } _skHub.publish(update); + + // Save calibration values to EEPROM every 30 Minutes, if BNO055 is fully calibrated + if (bno055.isFullyCalibrated() && _timeSinceLastCalSave > 1800000) { + if (saveCalibrationOffsets()) + _timeSinceLastCalSave = 0; + } } // get the actual values of calibration, heel & pitch (including offset) and heading for display // all angles in radians -void IMUService::getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading){ +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; @@ -106,9 +115,9 @@ void IMUService::getLastValues(int &accelCalibration, double &pitch, double &rol // Calc offset for making Heel=0 and Pitch=0 // (e.g. called with long button press in IMUMonitorPage) -// TODO: If an setOffset will be done a second time within 5 Seconds, +// 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::setOffset() { +void IMUService::setHeelPitchOffset() { bool changed = false; // FIXIT: Why is it called at startup of KBox? event longClick coming? if ( millis() > 10000 ) { @@ -122,58 +131,86 @@ void IMUService::setOffset() { } if ( changed ) { INFO("Offset changed: Heel -> %.3f | Pitch -> %.3f", SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); + if (_timeSinceLastSetOffset < 5000) { + saveHeelPitchOffset(); + } } + _timeSinceLastSetOffset = 0; } } -bool IMUService::saveCalibration() { - int _eeAddress = 0; - long bnoID; - sensor_t sensor; - adafruit_bno055_offsets_t newCalib; - bno055.getSensorOffsets(newCalib); - - DEBUG("Storing calibration data to EEPROM..."); - bno055.getSensor(&sensor); - bnoID = sensor.sensor_id; - - EEPROM.put(_eeAddress, bnoID); +bool IMUService::saveHeelPitchOffset() { + struct PersistentStorage::IMUHeelPitchOffsets o; + o.offsetHeel = (int) _offsetRoll * 1000; + o.offsetPitch = (int) _offsetPitch * 1000; - _eeAddress += sizeof(long); - EEPROM.put(_eeAddress, newCalib); - DEBUG("Data stored to EEPROM."); - delay(500); - return true; + if (PersistentStorage::writeImuHeelPitchOffsets(o)) { + DEBUG("Storing offset data to EEPROM..."); + return true; + } + ERROR("Error saving IMU Offsets for heel and pitch to flash."); + return false; } -bool IMUService::recallCalibration() { - - int _eeAddress = 0; - long bnoID; +bool IMUService::readHeelPitchOffset() { + struct PersistentStorage::IMUHeelPitchOffsets o; - EEPROM.get(_eeAddress, bnoID); - adafruit_bno055_offsets_t calibrationData; - sensor_t sensor; - - /* - * Look for the sensor's unique ID at the beginning oF EEPROM. - * This isn't foolproof, but it's better than nothing. - */ - bno055.getSensor(&sensor); - if (bnoID != sensor.sensor_id) { - DEBUG("No Calibration Data for this sensor exists in EEPROM"); + if (PersistentStorage::writeImuHeelPitchOffsets(o)) { + DEBUG("Recalling offset data from EEPROM..."); + _offsetRoll = o.offsetHeel / 100.0; + _offsetPitch = o.offsetPitch / 100.0; + return true; } - else { - DEBUG("Found Calibration for this sensor in EEPROM."); - _eeAddress += sizeof(long); - EEPROM.get(_eeAddress, calibrationData); + ERROR("Error reading IMU Offsets for heel and pitch from flash."); + return false; +} - DEBUG("Restoring Calibration data to the BNO055..."); - bno055.setSensorOffsets(calibrationData); +bool IMUService::saveCalibrationOffsets() { + adafruit_bno055_offsets_t c; + bno055.getSensorOffsets(c); + + struct PersistentStorage::BNO055CalOffsets o; + o.accel_offset_x = c.accel_offset_x; + o.accel_offset_y = c.accel_offset_y; + o.accel_offset_z = c.accel_offset_z; + o.gyro_offset_x = c.gyro_offset_x; + o.gyro_offset_y = c.gyro_offset_y; + o.gyro_offset_z = c.gyro_offset_z; + o.mag_offset_x = c.mag_offset_x; + o.mag_offset_y = c.mag_offset_y; + o.mag_offset_z = c.mag_offset_z; + o.accel_radius = c.accel_radius; + o.mag_radius = c.mag_radius; + + if (PersistentStorage::writeBNO055CalOffsets(o)) { + DEBUG("BNO055 Calibration Offsets written to flash."); + return true; + } + ERROR("Error saving BNO055 Calibration Offsets to flash."); + return false; +} - DEBUG("Calibration data loaded into BNO055"); - delay(1000); - return true; +bool IMUService::readCalibrationOffsets() { + adafruit_bno055_offsets_t c; + struct PersistentStorage::BNO055CalOffsets o; + + if (PersistentStorage::readBNO055CalOffsets(o)) { + c.accel_offset_x = o.accel_offset_x; + c.accel_offset_y = o.accel_offset_y; + c.accel_offset_z = o.accel_offset_z; + c.gyro_offset_x = o.gyro_offset_x; + c.gyro_offset_y = o.gyro_offset_y; + c.gyro_offset_z = o.gyro_offset_z; + c.mag_offset_x = o.mag_offset_x; + c.mag_offset_y = o.mag_offset_y; + c.mag_offset_z = o.mag_offset_z; + c.accel_radius = o.accel_radius; + c.mag_radius = o.mag_radius; + + bno055.setSensorOffsets(c); + DEBUG("BNO055 Calibration Offsets recalled from flash."); + return true; } + ERROR("Error recalling BNO055 Calibration Offsets from EEPROM"); return false; } diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index f85cf287..7a94b9bf 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -35,6 +35,9 @@ class IMUService : public Task { uint8_t _sysCalib, _gyroCalib, _accelCalib, _magCalib; double _roll, _pitch, _heading; double _offsetRoll, _offsetPitch; + elapsedMillis _timeSinceLastCalSave; + elapsedMillis _timeSinceLastSetOffset; + imu::Vector<3> eulerAngles; public: @@ -43,18 +46,23 @@ class IMUService : public Task { void loop(); void resetIMU(); - bool recallCalibration(); - bool saveCalibration(); - void setOffset(); + bool readCalibrationOffsets(); + bool saveCalibrationOffsets(); + void setHeelPitchOffset(); + bool saveHeelPitchOffset(); + bool readHeelPitchOffset(); + // 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 calibration status jumps to 1 or higher). bool isMagCalibrated() { - return _magCalib == 3; + return _magCalib == 3 && _sysCalib > 0; } bool isHeelAndPitchCalibrated() { - return _accelCalib >= 2 && _gyroCalib >= 2; + return _accelCalib >= 2 && _gyroCalib >= 2 && _sysCalib > 0; } - void getLastValues(int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); + void getLastValues(int &_sysCalibration, int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); }; diff --git a/src/host/util/PersistentStorage.cpp b/src/host/util/PersistentStorage.cpp index 61e1f6bd..0627e2a5 100644 --- a/src/host/util/PersistentStorage.cpp +++ b/src/host/util/PersistentStorage.cpp @@ -108,3 +108,39 @@ bool PersistentStorage::writeNMEA2000Parameters(struct NMEA2000Parameters &p) { return write(nmea2000ParamsAddress, &p, sizeof(NMEA2000Parameters)); } +// reads calibration offset values of BNO055 IMU sensor +bool PersistentStorage::readBNO055CalOffsets(struct BNO055CalOffsets &o) { + if (!isInitialized()) { + return false; + } + + if (!read(bno055CalOffsetsAddress, &o, sizeof(BNO055CalOffsets))) { + return false; + } + return true; +} + +bool PersistentStorage::writeBNO055CalOffsets(struct BNO055CalOffsets &o) { + if (!isInitialized()) { + initialize(); + } + + return write(bno055CalOffsetsAddress, &o, sizeof(BNO055CalOffsets)); +} + +bool PersistentStorage::readImuHeelPitchOffsets(IMUHeelPitchOffsets &o) { + if (!isInitialized()) { + initialize(); + } + + + return false; +} + +bool PersistentStorage::writeImuHeelPitchOffsets(IMUHeelPitchOffsets &o){ + if (!isInitialized()) { + initialize(); + } + + return false; +} diff --git a/src/host/util/PersistentStorage.h b/src/host/util/PersistentStorage.h index dbc02a20..8de82508 100644 --- a/src/host/util/PersistentStorage.h +++ b/src/host/util/PersistentStorage.h @@ -65,15 +65,37 @@ class PersistentStorage { }; }; + struct BNO055CalOffsets { + 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; + }; + + struct IMUHeelPitchOffsets { + int offsetHeel; + int 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 bno055CalOffsetsAddress = 20; // 11 x 2 Byte + static const uint16_t imuHeelPitchOffsetsAddress = 42; // 2 x 2 Byte // Size of flash - static const uint16_t storageUsed = 8 + sizeof(struct NMEA2000Parameters); - static const uint16_t storageSize = 2048; + static const uint16_t storageUsed = 46; + 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 +108,10 @@ class PersistentStorage { public: static bool readNMEA2000Parameters(struct NMEA2000Parameters &p); static bool writeNMEA2000Parameters(struct NMEA2000Parameters &p); -}; + static bool readBNO055CalOffsets(struct BNO055CalOffsets &o); + static bool writeBNO055CalOffsets(struct BNO055CalOffsets &o); + + static bool readImuHeelPitchOffsets(IMUHeelPitchOffsets &o); + static bool writeImuHeelPitchOffsets(IMUHeelPitchOffsets &o); +}; From d36937ea40f8583d85ae3bdf8d478e9d249ee294 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Tue, 2 Jan 2018 23:08:44 +0200 Subject: [PATCH 69/81] correction to 3 digits for heel/pitch values --- src/host/services/IMUService.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 6d036057..4c558d7b 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -157,8 +157,8 @@ bool IMUService::readHeelPitchOffset() { if (PersistentStorage::writeImuHeelPitchOffsets(o)) { DEBUG("Recalling offset data from EEPROM..."); - _offsetRoll = o.offsetHeel / 100.0; - _offsetPitch = o.offsetPitch / 100.0; + _offsetRoll = o.offsetHeel / 1000.0; + _offsetPitch = o.offsetPitch / 1000.0; return true; } ERROR("Error reading IMU Offsets for heel and pitch from flash."); From 179fcb53a3b2459da255d6895eb96db7e27e33b8 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Tue, 2 Jan 2018 23:35:05 +0200 Subject: [PATCH 70/81] indent corrected --- src/host/services/IMUService.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 7a94b9bf..4d644325 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -56,12 +56,12 @@ class IMUService : public Task { // and orientation values will be off The heading will jump to an absolute value // once the BNO finds magnetic north (the system calibration status jumps to 1 or higher). bool isMagCalibrated() { - return _magCalib == 3 && _sysCalib > 0; - } + return _magCalib == 3 && _sysCalib > 0; + } - bool isHeelAndPitchCalibrated() { - return _accelCalib >= 2 && _gyroCalib >= 2 && _sysCalib > 0; - } + bool isHeelAndPitchCalibrated() { + return _accelCalib >= 2 && _gyroCalib >= 2 && _sysCalib > 0; + } void getLastValues(int &_sysCalibration, int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); From b442fc712bfe24b225b1dda2f13443bb6d7c8409 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Tue, 2 Jan 2018 18:41:21 -0800 Subject: [PATCH 71/81] fixing some indent issues --- src/host/pages/IMUMonitorPage.h | 18 +++++++++--------- src/host/services/IMUService.cpp | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index 35694014..bdd86033 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -38,18 +38,18 @@ class IMUMonitorPage : public Page, public SKSubscriber { private: - TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; - IMUConfig &_config; - IMUService &_imuService; + TextLayer *_hdgTL, *_rollTL, *_pitchTL, *_calTL; + IMUConfig &_config; + IMUService &_imuService; - int _magCalibration, _accelCalibration, _sysCalibration; - double _pitch, _roll, _heading; + int _magCalibration, _accelCalibration, _sysCalibration; + double _pitch, _roll, _heading; - public: - IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); + public: + IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService); virtual void updateReceived(const SKUpdate& up); - bool processEvent(const TickEvent &te); - bool processEvent(const ButtonEvent &be); + 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 4c558d7b..4e545bd1 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -89,7 +89,7 @@ void IMUService::loop() { update.setNavigationHeadingMagnetic(_heading); } // update NavigationAttitude if quality (= calibration value) is ok - if (isHeelAndPitchCalibrated()) { + if (isHeelAndPitchCalibrated()) { update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ SKDoubleNAN)); //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); } From 4a3ef35bc5aa7d4981adbbcaf653d6dd083a65d0 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Tue, 2 Jan 2018 18:42:32 -0800 Subject: [PATCH 72/81] Add missing struct keywords --- src/host/util/PersistentStorage.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/host/util/PersistentStorage.h b/src/host/util/PersistentStorage.h index 8de82508..a2f7e258 100644 --- a/src/host/util/PersistentStorage.h +++ b/src/host/util/PersistentStorage.h @@ -112,6 +112,6 @@ class PersistentStorage { static bool readBNO055CalOffsets(struct BNO055CalOffsets &o); static bool writeBNO055CalOffsets(struct BNO055CalOffsets &o); - static bool readImuHeelPitchOffsets(IMUHeelPitchOffsets &o); - static bool writeImuHeelPitchOffsets(IMUHeelPitchOffsets &o); + static bool readImuHeelPitchOffsets(struct IMUHeelPitchOffsets &o); + static bool writeImuHeelPitchOffsets(struct IMUHeelPitchOffsets &o); }; From a786f356e84ec69740ea33eab9bc5560ded4a1e1 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Tue, 2 Jan 2018 18:54:49 -0800 Subject: [PATCH 73/81] Fix phantom button event at startup --- src/host/services/MFD.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/host/services/MFD.cpp b/src/host/services/MFD.cpp index d2d32d12..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() { From 368f6fb8d4e7fc0e01990d0fac97555ffd7bcb9e Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Tue, 2 Jan 2018 18:55:06 -0800 Subject: [PATCH 74/81] Fix name of function called to read heel/pitch offset This function is not implemented yet but anyway ;) Also, some minor cosmetic changes to code style (sorry could not help myself). --- src/host/services/IMUService.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 4e545bd1..0a0936f7 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -148,21 +148,25 @@ bool IMUService::saveHeelPitchOffset() { DEBUG("Storing offset data to EEPROM..."); return true; } - ERROR("Error saving IMU Offsets for heel and pitch to flash."); - return false; + else { + ERROR("Error saving IMU Offsets for heel and pitch to flash."); + return false; + } } bool IMUService::readHeelPitchOffset() { struct PersistentStorage::IMUHeelPitchOffsets o; - if (PersistentStorage::writeImuHeelPitchOffsets(o)) { + if (PersistentStorage::readImuHeelPitchOffsets(o)) { DEBUG("Recalling offset data from EEPROM..."); _offsetRoll = o.offsetHeel / 1000.0; _offsetPitch = o.offsetPitch / 1000.0; return true; } - ERROR("Error reading IMU Offsets for heel and pitch from flash."); - return false; + else { + ERROR("Error reading IMU Offsets for heel and pitch from flash."); + return false; + } } bool IMUService::saveCalibrationOffsets() { @@ -186,8 +190,10 @@ bool IMUService::saveCalibrationOffsets() { DEBUG("BNO055 Calibration Offsets written to flash."); return true; } - ERROR("Error saving BNO055 Calibration Offsets to flash."); - return false; + else { + ERROR("Error saving BNO055 Calibration Offsets to flash."); + return false; + } } bool IMUService::readCalibrationOffsets() { @@ -211,6 +217,8 @@ bool IMUService::readCalibrationOffsets() { DEBUG("BNO055 Calibration Offsets recalled from flash."); return true; } - ERROR("Error recalling BNO055 Calibration Offsets from EEPROM"); - return false; + else { + ERROR("Error recalling BNO055 Calibration Offsets from EEPROM"); + return false; + } } From ca5fe38d60678fedd31186417dad233025dc2f79 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Wed, 3 Jan 2018 11:31:45 +0200 Subject: [PATCH 75/81] KBox default mounting position: vertical, Port Hull --- extras/config/default-config.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extras/config/default-config.json b/extras/config/default-config.json index e6273619..92c9a2bb 100644 --- a/extras/config/default-config.json +++ b/extras/config/default-config.json @@ -4,7 +4,7 @@ "frequency": 20, "enableHdg": true, "enableHeelPitch": true, - "mounting": "verticalStarboardHull" + "mounting": "verticalPortHull" }, "barometer": { "enabled": true, From 61ca6e551a53288f885b1c5f418fb05f03f6b395 Mon Sep 17 00:00:00 2001 From: ronzeiller Date: Wed, 3 Jan 2018 11:47:54 +0200 Subject: [PATCH 76/81] mounting positions switch statements, default position port hull --- src/host/config/IMUConfig.h | 6 +- src/host/config/KBoxConfigParser.cpp | 12 ++-- src/host/services/IMUService.cpp | 89 +++++++++++++++------------- src/host/services/IMUService.h | 1 + 4 files changed, 59 insertions(+), 49 deletions(-) diff --git a/src/host/config/IMUConfig.h b/src/host/config/IMUConfig.h index aa138325..90d7e5ae 100644 --- a/src/host/config/IMUConfig.h +++ b/src/host/config/IMUConfig.h @@ -34,9 +34,9 @@ enum IMUMounting { VerticalStbHull, VerticalPortHull, VerticalTopToBow, - VerticalTopToAft, + //VerticalTopToStern, HorizontalLeftSideToBow, - HorizontalRightSideToBow + //HorizontalRightSideToBow }; struct IMUConfig { @@ -44,5 +44,5 @@ struct IMUConfig { int frequency; bool enableHdg; bool enableHeelPitch; - enum IMUMounting mounting = VerticalStbHull; + enum IMUMounting mounting = VerticalPortHull; }; diff --git a/src/host/config/KBoxConfigParser.cpp b/src/host/config/KBoxConfigParser.cpp index ab611fee..7b4cda66 100644 --- a/src/host/config/KBoxConfigParser.cpp +++ b/src/host/config/KBoxConfigParser.cpp @@ -141,14 +141,18 @@ enum SerialMode KBoxConfigParser::convertSerialMode(const String &s) { } enum IMUMounting KBoxConfigParser::convertIMUMounting(const String &s) { + if (s == "verticalPortHull") { + return VerticalPortHull; + } if (s == "verticalStarboardHull") { - return VerticalStbHull; + return VerticalStbHull; } if (s == "verticalTopToBow") { - return VerticalTopToBow; + return VerticalTopToBow; } if (s == "horizontalLeftSideToBow") { - return HorizontalLeftSideToBow; + return HorizontalLeftSideToBow; } - return VerticalStbHull; + // default + return VerticalPortHull; } diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 0a0936f7..0774bc2d 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -32,21 +32,22 @@ void IMUService::setup() { _timeSinceLastSetOffset = 0; _offsetRoll = 0.0; _offsetPitch = 0.0; - // Default mounting position: VerticalStbHull - uint8_t axisConfig = 0b00001001; - uint8_t signConfig = 0b00000000; - if (_config.mounting == VerticalTopToBow) { - axisConfig = 0b00001001; - signConfig = 0b00000000; - } - if (_config.mounting == HorizontalLeftSideToBow) { - axisConfig = 0b00100100; - signConfig = 0b00000000; + switch (_config.mounting) { + case HorizontalLeftSideToBow: + _axisConfig = 0b00100100; + _signConfig = 0b00000000; + break; + case VerticalTopToBow: + case VerticalStbHull: + case VerticalPortHull: + default: + _axisConfig = 0b00001001; + _signConfig = 0b00000000; } DEBUG("Initing BNO055"); - if (!bno055.begin(bno055.OPERATION_MODE_NDOF, axisConfig, signConfig)) { + if (!bno055.begin(bno055.OPERATION_MODE_NDOF, _axisConfig, _signConfig)) { DEBUG("Error initializing BNO055"); } else { @@ -67,21 +68,27 @@ void IMUService::loop() { // Note: We could calculate yaw as the difference between the Magnetic // Heading and the Course Over Ground Magnetic. - if (_config.mounting == VerticalStbHull) { - _roll = SKDegToRad(eulerAngles.z()); - _pitch = SKDegToRad(eulerAngles.y()); - _heading = SKDegToRad(fmod(eulerAngles.x() + 270, 360)); - } - if (_config.mounting == VerticalTopToBow) { - //TODO: check position for CaptainRon47 - _roll = SKDegToRad(eulerAngles.y()); - _pitch = SKDegToRad(eulerAngles.z()); - _heading = SKDegToRad(fmod(eulerAngles.x() + 180, 360)); - } - if (_config.mounting == HorizontalLeftSideToBow) { - _roll = SKDegToRad(eulerAngles.y()); - _pitch = SKDegToRad(eulerAngles.z()) * (-1); - _heading = SKDegToRad(eulerAngles.x()); + 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; } // update NavigationHeadingMagnetic if quality (= calibration value) is ok @@ -119,24 +126,22 @@ void IMUService::getLastValues(int &sysCalibration, int &accelCalibration, doubl // the values will be stored into EEPROM and loaded at start of KBox void IMUService::setHeelPitchOffset() { bool changed = false; - // FIXIT: Why is it called at startup of KBox? event longClick coming? - if ( millis() > 10000 ) { - 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)); - if (_timeSinceLastSetOffset < 5000) { - saveHeelPitchOffset(); - } + 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)); + if (_timeSinceLastSetOffset < 5000) { + saveHeelPitchOffset(); } - _timeSinceLastSetOffset = 0; } + _timeSinceLastSetOffset = 0; } bool IMUService::saveHeelPitchOffset() { diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 4d644325..5d43fc53 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -33,6 +33,7 @@ class IMUService : public Task { SKHub &_skHub; Adafruit_BNO055 bno055; uint8_t _sysCalib, _gyroCalib, _accelCalib, _magCalib; + uint8_t _axisConfig, _signConfig; double _roll, _pitch, _heading; double _offsetRoll, _offsetPitch; elapsedMillis _timeSinceLastCalSave; From b41921439c269e85c211b35d09962910780c6689 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Wed, 3 Jan 2018 20:17:26 -0800 Subject: [PATCH 77/81] fix my if/else codestyle --- src/host/services/IMUService.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 0774bc2d..76a4f6d1 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -152,8 +152,7 @@ bool IMUService::saveHeelPitchOffset() { if (PersistentStorage::writeImuHeelPitchOffsets(o)) { DEBUG("Storing offset data to EEPROM..."); return true; - } - else { + } else { ERROR("Error saving IMU Offsets for heel and pitch to flash."); return false; } @@ -167,8 +166,7 @@ bool IMUService::readHeelPitchOffset() { _offsetRoll = o.offsetHeel / 1000.0; _offsetPitch = o.offsetPitch / 1000.0; return true; - } - else { + } else { ERROR("Error reading IMU Offsets for heel and pitch from flash."); return false; } @@ -194,8 +192,7 @@ bool IMUService::saveCalibrationOffsets() { if (PersistentStorage::writeBNO055CalOffsets(o)) { DEBUG("BNO055 Calibration Offsets written to flash."); return true; - } - else { + } else { ERROR("Error saving BNO055 Calibration Offsets to flash."); return false; } @@ -221,8 +218,7 @@ bool IMUService::readCalibrationOffsets() { bno055.setSensorOffsets(c); DEBUG("BNO055 Calibration Offsets recalled from flash."); return true; - } - else { + } else { ERROR("Error recalling BNO055 Calibration Offsets from EEPROM"); return false; } From 860f447915bf8bf8a467460a925b953eafacc613 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Wed, 3 Jan 2018 23:17:24 -0800 Subject: [PATCH 78/81] Few changes to IMUService and calibration - Use an array for BNO055 calibration data to avoid having to assign registers one by one. - Save IMUCalibration with offsets since we are very likely to read/write them together. - Also include with calibration the mounting position because the calibration very likely needs to be rest when the mounting position changes. - Use "roll" everywhere instead of pitch --- src/host/pages/IMUMonitorPage.cpp | 15 ++- src/host/services/IMUService.cpp | 156 ++++++++++++---------------- src/host/services/IMUService.h | 35 ++++--- src/host/util/PersistentStorage.cpp | 29 +----- src/host/util/PersistentStorage.h | 38 +++---- 5 files changed, 111 insertions(+), 162 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index 2017e81c..d07d3981 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -28,14 +28,13 @@ THE SOFTWARE. */ -#include -#include "common/signalk/SKUpdate.h" + #include "IMUMonitorPage.h" -#include -#include "signalk/SKUnits.h" -IMUMonitorPage::IMUMonitorPage(IMUConfig &config, SKHub& hub, IMUService &imuService) : _config(config), _imuService(imuService) { +#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; @@ -67,7 +66,7 @@ bool IMUMonitorPage::processEvent(const ButtonEvent &be){ return false; } if (be.clickType == ButtonEventTypeLongClick) { - _imuService.setHeelPitchOffset(); + _imuService.setRollPitchOffset(); } return true; } @@ -82,8 +81,8 @@ bool IMUMonitorPage::processEvent(const TickEvent &te){ _rollTL->setText(String( SKRadToDeg(_roll), 1) + "° "); // Always show Hdg from IMU-sensor, but if the value is not trusted (which means - // calibration below default setting, change color to red - if ( ! _imuService.isMagCalibrated() || ! _imuService.isHeelAndPitchCalibrated()) { + // calibrationData below default setting, change color to red + if ( ! _imuService.isMagCalibrated() || ! _imuService.isRollAndPitchCalibrated()) { _hdgTL->setColor(ColorRed); _calTL->setColor(ColorRed); } else { diff --git a/src/host/services/IMUService.cpp b/src/host/services/IMUService.cpp index 76a4f6d1..6d5c2c85 100644 --- a/src/host/services/IMUService.cpp +++ b/src/host/services/IMUService.cpp @@ -28,11 +28,19 @@ #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() { - _timeSinceLastSetOffset = 0; _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; @@ -41,7 +49,6 @@ void IMUService::setup() { case VerticalTopToBow: case VerticalStbHull: case VerticalPortHull: - default: _axisConfig = 0b00001001; _signConfig = 0b00000000; } @@ -51,23 +58,20 @@ void IMUService::setup() { DEBUG("Error initializing BNO055"); } else { - // read calibration and offset values for heel & pitch from flash - readCalibrationOffsets(); - readHeelPitchOffset(); + // read calibrationData and offset values for heel & pitch from flash + restoreCalibration(); } } void IMUService::loop() { bno055.getCalibration(&_sysCalib, &_gyroCalib, &_accelCalib, &_magCalib); - //DEBUG("Calib Sys: %i Accel: %i Gyro: %i Mag: %i", _sysCalib, _accelCalib, _gyroCalib, _magCalib); - - eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - //DEBUG("Sensor Raw-datas: eulerAngles.z=%.3f eulerAngles.y=%.3f eulerAngles.x=%.3f", eulerAngles.z(), eulerAngles.y(), eulerAngles.x()); + imu::Vector<3> eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); SKUpdateStatic<2> update; - // Note: We could calculate yaw as the difference between the Magnetic - // Heading and the Course Over Ground Magnetic. + // 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()); @@ -91,25 +95,27 @@ void IMUService::loop() { break; } - // update NavigationHeadingMagnetic if quality (= calibration value) is ok if (isMagCalibrated()) { update.setNavigationHeadingMagnetic(_heading); } - // update NavigationAttitude if quality (= calibration value) is ok - if (isHeelAndPitchCalibrated()) { - update.setNavigationAttitude(SKTypeAttitude(/* roll */ _roll + _offsetRoll, /* pitch */ _pitch + _offsetPitch, /* yaw */ SKDoubleNAN)); - //DEBUG("Heel = %.3f | Pitch = %.3f | Heading = %.3f", SKRadToDeg( _roll + _offsetRoll), SKRadToDeg( _pitch + _offsetPitch), SKRadToDeg(_heading)); + + 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); - // Save calibration values to EEPROM every 30 Minutes, if BNO055 is fully calibrated - if (bno055.isFullyCalibrated() && _timeSinceLastCalSave > 1800000) { - if (saveCalibrationOffsets()) - _timeSinceLastCalSave = 0; + // 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 calibration, heel & pitch (including offset) and heading for display +// 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; @@ -124,7 +130,7 @@ void IMUService::getLastValues(int &sysCalibration, int &accelCalibration, doubl // (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::setHeelPitchOffset() { +void IMUService::setRollPitchOffset() { bool changed = false; if (abs(_roll) < M_PI / 2 ) { _offsetRoll = _roll * (-1); @@ -136,90 +142,60 @@ void IMUService::setHeelPitchOffset() { } if ( changed ) { - INFO("Offset changed: Heel -> %.3f | Pitch -> %.3f", SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); - if (_timeSinceLastSetOffset < 5000) { - saveHeelPitchOffset(); - } + INFO("Offset changed: Heel -> %.3f | Pitch -> %.3f", + SKRadToDeg(_offsetRoll), SKRadToDeg(_offsetPitch)); + DEBUG("Offset changed: Heel -> %.3f | Pitch -> %.3f", _offsetRoll, + _offsetPitch); + saveCalibration(); + _timeSinceLastCalSave = 0; } - _timeSinceLastSetOffset = 0; } -bool IMUService::saveHeelPitchOffset() { - struct PersistentStorage::IMUHeelPitchOffsets o; - o.offsetHeel = (int) _offsetRoll * 1000; - o.offsetPitch = (int) _offsetPitch * 1000; - - if (PersistentStorage::writeImuHeelPitchOffsets(o)) { - DEBUG("Storing offset data to EEPROM..."); - return true; - } else { - ERROR("Error saving IMU Offsets for heel and pitch to flash."); - return false; - } -} +bool IMUService::saveCalibration() { + struct PersistentStorage::IMUCalibration calibration; -bool IMUService::readHeelPitchOffset() { - struct PersistentStorage::IMUHeelPitchOffsets o; + // 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."); - if (PersistentStorage::readImuHeelPitchOffsets(o)) { - DEBUG("Recalling offset data from EEPROM..."); - _offsetRoll = o.offsetHeel / 1000.0; - _offsetPitch = o.offsetPitch / 1000.0; - return true; - } else { - ERROR("Error reading IMU Offsets for heel and pitch from flash."); + 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); -bool IMUService::saveCalibrationOffsets() { - adafruit_bno055_offsets_t c; - bno055.getSensorOffsets(c); - - struct PersistentStorage::BNO055CalOffsets o; - o.accel_offset_x = c.accel_offset_x; - o.accel_offset_y = c.accel_offset_y; - o.accel_offset_z = c.accel_offset_z; - o.gyro_offset_x = c.gyro_offset_x; - o.gyro_offset_y = c.gyro_offset_y; - o.gyro_offset_z = c.gyro_offset_z; - o.mag_offset_x = c.mag_offset_x; - o.mag_offset_y = c.mag_offset_y; - o.mag_offset_z = c.mag_offset_z; - o.accel_radius = c.accel_radius; - o.mag_radius = c.mag_radius; - - if (PersistentStorage::writeBNO055CalOffsets(o)) { - DEBUG("BNO055 Calibration Offsets written to flash."); + if (PersistentStorage::writeIMUCalibration(calibration)) { + DEBUG("IMU Calibration written to flash."); return true; } else { - ERROR("Error saving BNO055 Calibration Offsets to flash."); + ERROR("Error saving IMU Calibration to flash."); return false; } } -bool IMUService::readCalibrationOffsets() { - adafruit_bno055_offsets_t c; - struct PersistentStorage::BNO055CalOffsets o; - - if (PersistentStorage::readBNO055CalOffsets(o)) { - c.accel_offset_x = o.accel_offset_x; - c.accel_offset_y = o.accel_offset_y; - c.accel_offset_z = o.accel_offset_z; - c.gyro_offset_x = o.gyro_offset_x; - c.gyro_offset_y = o.gyro_offset_y; - c.gyro_offset_z = o.gyro_offset_z; - c.mag_offset_x = o.mag_offset_x; - c.mag_offset_y = o.mag_offset_y; - c.mag_offset_z = o.mag_offset_z; - c.accel_radius = o.accel_radius; - c.mag_radius = o.mag_radius; - - bno055.setSensorOffsets(c); - DEBUG("BNO055 Calibration Offsets recalled from flash."); +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 BNO055 Calibration Offsets from EEPROM"); + ERROR("Error recalling IMU Calibration from flash"); return false; } } + diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index 5d43fc53..a21a104c 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -21,7 +21,9 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + #pragma once + #include #include "common/os/Task.h" #include "common/signalk/SKHub.h" @@ -29,6 +31,14 @@ 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; @@ -37,32 +47,31 @@ class IMUService : public Task { double _roll, _pitch, _heading; double _offsetRoll, _offsetPitch; elapsedMillis _timeSinceLastCalSave; - elapsedMillis _timeSinceLastSetOffset; - imu::Vector<3> eulerAngles; + bool restoreCalibration(); + bool saveCalibration(); public: - IMUService(IMUConfig &config, SKHub& skHub) : Task("IMU"), _config(config), _skHub(skHub) {}; + IMUService(IMUConfig &config, SKHub& skHub); void setup(); void loop(); - void resetIMU(); - bool readCalibrationOffsets(); - bool saveCalibrationOffsets(); - void setHeelPitchOffset(); - bool saveHeelPitchOffset(); - bool readHeelPitchOffset(); + /** + * 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 calibration status jumps to 1 or higher). + // once the BNO finds magnetic north (the system calibrationData status jumps to 1 or higher). bool isMagCalibrated() { return _magCalib == 3 && _sysCalib > 0; - } + }; - bool isHeelAndPitchCalibrated() { + bool isRollAndPitchCalibrated() { return _accelCalib >= 2 && _gyroCalib >= 2 && _sysCalib > 0; - } + }; void getLastValues(int &_sysCalibration, int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); diff --git a/src/host/util/PersistentStorage.cpp b/src/host/util/PersistentStorage.cpp index 0627e2a5..2f59d508 100644 --- a/src/host/util/PersistentStorage.cpp +++ b/src/host/util/PersistentStorage.cpp @@ -108,39 +108,18 @@ bool PersistentStorage::writeNMEA2000Parameters(struct NMEA2000Parameters &p) { return write(nmea2000ParamsAddress, &p, sizeof(NMEA2000Parameters)); } -// reads calibration offset values of BNO055 IMU sensor -bool PersistentStorage::readBNO055CalOffsets(struct BNO055CalOffsets &o) { +bool PersistentStorage::readIMUCalibration(struct IMUCalibration &o){ if (!isInitialized()) { return false; } - if (!read(bno055CalOffsetsAddress, &o, sizeof(BNO055CalOffsets))) { - return false; - } - return true; -} - -bool PersistentStorage::writeBNO055CalOffsets(struct BNO055CalOffsets &o) { - if (!isInitialized()) { - initialize(); - } - - return write(bno055CalOffsetsAddress, &o, sizeof(BNO055CalOffsets)); -} - -bool PersistentStorage::readImuHeelPitchOffsets(IMUHeelPitchOffsets &o) { - if (!isInitialized()) { - initialize(); - } - - - return false; + return read(imuCalibrationAddress, &o, sizeof(IMUCalibration)); } -bool PersistentStorage::writeImuHeelPitchOffsets(IMUHeelPitchOffsets &o){ +bool PersistentStorage::writeIMUCalibration(struct IMUCalibration &o) { if (!isInitialized()) { initialize(); } - return false; + return write(imuCalibrationAddress, &o, sizeof(IMUCalibration)); } diff --git a/src/host/util/PersistentStorage.h b/src/host/util/PersistentStorage.h index a2f7e258..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,23 +65,11 @@ class PersistentStorage { }; }; - struct BNO055CalOffsets { - 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; - }; - - struct IMUHeelPitchOffsets { - int offsetHeel; - int offsetPitch; + struct IMUCalibration { + uint8_t mountingPosition; + uint8_t calibrationData[22]; // NUM_BNO055_OFFSET_REGISTERS + int16_t offsetRoll; + int16_t offsetPitch; }; private: @@ -89,11 +77,12 @@ class PersistentStorage { static const uint16_t magicAddress = 0; static const uint16_t versionAddress = 4; static const uint16_t nmea2000ParamsAddress = 8; - static const uint16_t bno055CalOffsetsAddress = 20; // 11 x 2 Byte - static const uint16_t imuHeelPitchOffsetsAddress = 42; // 2 x 2 Byte + static const uint16_t imuCalibrationAddress = + nmea2000ParamsAddress + sizeof(NMEA2000Parameters); // Size of flash - static const uint16_t storageUsed = 46; + 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 @@ -109,9 +98,6 @@ class PersistentStorage { static bool readNMEA2000Parameters(struct NMEA2000Parameters &p); static bool writeNMEA2000Parameters(struct NMEA2000Parameters &p); - static bool readBNO055CalOffsets(struct BNO055CalOffsets &o); - static bool writeBNO055CalOffsets(struct BNO055CalOffsets &o); - - static bool readImuHeelPitchOffsets(struct IMUHeelPitchOffsets &o); - static bool writeImuHeelPitchOffsets(struct IMUHeelPitchOffsets &o); + static bool readIMUCalibration(struct IMUCalibration &o); + static bool writeIMUCalibration(struct IMUCalibration &o); }; From 546347207221c7f00228150ea742c346c7c5a19d Mon Sep 17 00:00:00 2001 From: Ronnie Zeiller Date: Fri, 5 Jan 2018 12:32:54 +0200 Subject: [PATCH 79/81] Update IMUService.h --- src/host/services/IMUService.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/host/services/IMUService.h b/src/host/services/IMUService.h index a21a104c..26f0398f 100644 --- a/src/host/services/IMUService.h +++ b/src/host/services/IMUService.h @@ -70,7 +70,7 @@ class IMUService : public Task { }; bool isRollAndPitchCalibrated() { - return _accelCalib >= 2 && _gyroCalib >= 2 && _sysCalib > 0; + return _accelCalib >= 2 && _gyroCalib >= 2; }; void getLastValues(int &_sysCalibration, int &accelCalibration, double &pitch, double &roll, int &magCalibration, double &heading); From 7b4e9a32e4565f56affe373924090f28aedbae3f Mon Sep 17 00:00:00 2001 From: Ronnie Zeiller Date: Fri, 5 Jan 2018 12:33:26 +0200 Subject: [PATCH 80/81] Copyright changed --- src/host/pages/IMUMonitorPage.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/host/pages/IMUMonitorPage.cpp b/src/host/pages/IMUMonitorPage.cpp index d07d3981..4ca86e8b 100644 --- a/src/host/pages/IMUMonitorPage.cpp +++ b/src/host/pages/IMUMonitorPage.cpp @@ -7,7 +7,8 @@ The MIT License - Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net + 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 From 17ce6800642a7694e2b0437a3814d542da4296ed Mon Sep 17 00:00:00 2001 From: Ronnie Zeiller Date: Fri, 5 Jan 2018 12:34:14 +0200 Subject: [PATCH 81/81] Copyright changed --- src/host/pages/IMUMonitorPage.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/host/pages/IMUMonitorPage.h b/src/host/pages/IMUMonitorPage.h index bdd86033..33efa2da 100644 --- a/src/host/pages/IMUMonitorPage.h +++ b/src/host/pages/IMUMonitorPage.h @@ -6,8 +6,9 @@ \/_/\/_/ \/_____/ \/_____/ \/_/\/_/ The MIT License - - Copyright (c) 2017 Thomas Sarlandie thomas@sarlandie.net + + 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