From ea9ace07b53d8257c53024b9e995562414b04863 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 14:20:17 -0400 Subject: [PATCH 01/42] Add files via upload Adding autopilot functionality --- main.cpp | 154 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 main.cpp diff --git a/main.cpp b/main.cpp new file mode 100644 index 00000000..62e30a76 --- /dev/null +++ b/main.cpp @@ -0,0 +1,154 @@ +/* + The MIT License + + Copyright (c) 2016 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 +#include +#include +#include "util/ESPProgrammer.h" + +// Those are included here to force platformio to link to them. +// See issue github.com/platformio/platformio/issues/432 for a better way to do this. +#include + +#include "ClockPage.h" + +KBox kbox; + +void setup() { + // Enable float in printf: + // https://forum.pjrc.com/threads/27827-Float-in-sscanf-on-Teensy-3-1 + asm(".global _printf_float"); + + delay(3000); + + DEBUG_INIT(); + DEBUG("Starting"); + + + digitalWrite(led_pin, 1); + + // Create all the receiver task first + WiFiTask *wifi = new WiFiTask(); + + // Create all the generating tasks and connect them + NMEA2000Task *n2kTask = new NMEA2000Task(); + n2kTask->connectTo(*wifi); + + ADCTask *adcTask = new ADCTask(kbox.getADC()); + + // Convert battery measurement into n2k messages before sending them to wifi. + VoltageN2kConverter *voltageConverter = new VoltageN2kConverter(); + adcTask->connectTo(*voltageConverter); + voltageConverter->connectTo(*wifi); + voltageConverter->connectTo(*n2kTask); + + NMEAReaderTask *reader1 = new NMEAReaderTask(NMEA1_SERIAL); + NMEAReaderTask *reader2 = new NMEAReaderTask(NMEA2_SERIAL); + reader1->connectTo(*wifi); + reader2->connectTo(*wifi); + + IMUTask *imuTask = new IMUTask(); + imuTask->connectTo(*wifi); //RIGM added + imuTask->connectTo(*n2kTask); + + BarometerTask *baroTask = new BarometerTask(); + + BarometerN2kConverter *bn2k = new BarometerN2kConverter(); + bn2k->connectTo(*wifi); + bn2k->connectTo(*n2kTask); + + baroTask->connectTo(*bn2k); + + SDCardTask *sdcardTask = new SDCardTask(); + reader1->connectTo(*sdcardTask); + reader2->connectTo(*sdcardTask); + adcTask->connectTo(*sdcardTask); + n2kTask->connectTo(*sdcardTask); + baroTask->connectTo(*sdcardTask); + imuTask->connectTo(*sdcardTask); + + AutoPilotTask *autoPilotTask = new AutoPilotTask();//RIGM added + autoPilotTask->connectTo(*wifi); + autoPilotTask->connectTo(*n2kTask); + + // Add all the tasks + kbox.addTask(new IntervalTask(new RunningLightTask(), 250)); + kbox.addTask(new IntervalTask(adcTask, 1000)); + kbox.addTask(new IntervalTask(imuTask, 50)); + kbox.addTask(new IntervalTask(baroTask, 1000)); + kbox.addTask(n2kTask); + kbox.addTask(reader1); + kbox.addTask(reader2); + kbox.addTask(wifi); + kbox.addTask(sdcardTask); + kbox.addTask(autoPilotTask); + + NavigationPage *navPage = new NavigationPage(); //RIGM added + imuTask->connectTo(*navPage); + adcTask->connectTo(*navPage); + autoPilotTask->connectTo(*navPage); + kbox.addPage(navPage); + navPage->connectTo(*autoPilotTask); + + BatteryMonitorPage *batPage = new BatteryMonitorPage(); + adcTask->connectTo(*batPage); + kbox.addPage(batPage); + + StatsPage *statsPage = new StatsPage(); + statsPage->setNmea1Task(reader1); + statsPage->setNmea2Task(reader2); + statsPage->setNMEA2000Task(n2kTask); + statsPage->setTaskManager(&(kbox.getTaskManager())); + statsPage->setSDCardTask(sdcardTask); + + kbox.addPage(statsPage); + + kbox.setup(); + + // Reinitialize debug here because in some configurations + // (like logging to nmea2 output), the kbox setup might have messed + // up the debug configuration. + DEBUG_INIT(); + DEBUG("setup done"); + + Serial.setTimeout(0); + Serial1.setTimeout(0); + Serial2.setTimeout(0); + Serial3.setTimeout(0); +} + +void loop() { + if (Serial.baud() == 230400) { + DEBUG("Entering programmer mode"); + ESPProgrammer programmer(kbox.getNeopixels(), Serial, Serial1); + while (programmer.getState() != ESPProgrammer::ProgrammerState::Done) { + programmer.loop(); + } + DEBUG("Exiting programmer mode"); + CPU_RESTART; + } + else { + kbox.loop(); + } +} From 3b0ed7d7b1aa7f7fdcfd3a609a747851f46f0d66 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 14:22:01 -0400 Subject: [PATCH 02/42] Delete main.cpp --- main.cpp | 154 ------------------------------------------------------- 1 file changed, 154 deletions(-) delete mode 100644 main.cpp diff --git a/main.cpp b/main.cpp deleted file mode 100644 index 62e30a76..00000000 --- a/main.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/* - The MIT License - - Copyright (c) 2016 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 -#include -#include -#include "util/ESPProgrammer.h" - -// Those are included here to force platformio to link to them. -// See issue github.com/platformio/platformio/issues/432 for a better way to do this. -#include - -#include "ClockPage.h" - -KBox kbox; - -void setup() { - // Enable float in printf: - // https://forum.pjrc.com/threads/27827-Float-in-sscanf-on-Teensy-3-1 - asm(".global _printf_float"); - - delay(3000); - - DEBUG_INIT(); - DEBUG("Starting"); - - - digitalWrite(led_pin, 1); - - // Create all the receiver task first - WiFiTask *wifi = new WiFiTask(); - - // Create all the generating tasks and connect them - NMEA2000Task *n2kTask = new NMEA2000Task(); - n2kTask->connectTo(*wifi); - - ADCTask *adcTask = new ADCTask(kbox.getADC()); - - // Convert battery measurement into n2k messages before sending them to wifi. - VoltageN2kConverter *voltageConverter = new VoltageN2kConverter(); - adcTask->connectTo(*voltageConverter); - voltageConverter->connectTo(*wifi); - voltageConverter->connectTo(*n2kTask); - - NMEAReaderTask *reader1 = new NMEAReaderTask(NMEA1_SERIAL); - NMEAReaderTask *reader2 = new NMEAReaderTask(NMEA2_SERIAL); - reader1->connectTo(*wifi); - reader2->connectTo(*wifi); - - IMUTask *imuTask = new IMUTask(); - imuTask->connectTo(*wifi); //RIGM added - imuTask->connectTo(*n2kTask); - - BarometerTask *baroTask = new BarometerTask(); - - BarometerN2kConverter *bn2k = new BarometerN2kConverter(); - bn2k->connectTo(*wifi); - bn2k->connectTo(*n2kTask); - - baroTask->connectTo(*bn2k); - - SDCardTask *sdcardTask = new SDCardTask(); - reader1->connectTo(*sdcardTask); - reader2->connectTo(*sdcardTask); - adcTask->connectTo(*sdcardTask); - n2kTask->connectTo(*sdcardTask); - baroTask->connectTo(*sdcardTask); - imuTask->connectTo(*sdcardTask); - - AutoPilotTask *autoPilotTask = new AutoPilotTask();//RIGM added - autoPilotTask->connectTo(*wifi); - autoPilotTask->connectTo(*n2kTask); - - // Add all the tasks - kbox.addTask(new IntervalTask(new RunningLightTask(), 250)); - kbox.addTask(new IntervalTask(adcTask, 1000)); - kbox.addTask(new IntervalTask(imuTask, 50)); - kbox.addTask(new IntervalTask(baroTask, 1000)); - kbox.addTask(n2kTask); - kbox.addTask(reader1); - kbox.addTask(reader2); - kbox.addTask(wifi); - kbox.addTask(sdcardTask); - kbox.addTask(autoPilotTask); - - NavigationPage *navPage = new NavigationPage(); //RIGM added - imuTask->connectTo(*navPage); - adcTask->connectTo(*navPage); - autoPilotTask->connectTo(*navPage); - kbox.addPage(navPage); - navPage->connectTo(*autoPilotTask); - - BatteryMonitorPage *batPage = new BatteryMonitorPage(); - adcTask->connectTo(*batPage); - kbox.addPage(batPage); - - StatsPage *statsPage = new StatsPage(); - statsPage->setNmea1Task(reader1); - statsPage->setNmea2Task(reader2); - statsPage->setNMEA2000Task(n2kTask); - statsPage->setTaskManager(&(kbox.getTaskManager())); - statsPage->setSDCardTask(sdcardTask); - - kbox.addPage(statsPage); - - kbox.setup(); - - // Reinitialize debug here because in some configurations - // (like logging to nmea2 output), the kbox setup might have messed - // up the debug configuration. - DEBUG_INIT(); - DEBUG("setup done"); - - Serial.setTimeout(0); - Serial1.setTimeout(0); - Serial2.setTimeout(0); - Serial3.setTimeout(0); -} - -void loop() { - if (Serial.baud() == 230400) { - DEBUG("Entering programmer mode"); - ESPProgrammer programmer(kbox.getNeopixels(), Serial, Serial1); - while (programmer.getState() != ESPProgrammer::ProgrammerState::Done) { - programmer.loop(); - } - DEBUG("Exiting programmer mode"); - CPU_RESTART; - } - else { - kbox.loop(); - } -} From cef86c256397d333b744b3b0a9e8632635227055 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 14:22:39 -0400 Subject: [PATCH 03/42] Add autopilot functionality --- src/host/main.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/host/main.cpp b/src/host/main.cpp index 268d0378..62e30a76 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -69,6 +69,7 @@ void setup() { reader2->connectTo(*wifi); IMUTask *imuTask = new IMUTask(); + imuTask->connectTo(*wifi); //RIGM added imuTask->connectTo(*n2kTask); BarometerTask *baroTask = new BarometerTask(); @@ -86,6 +87,10 @@ void setup() { n2kTask->connectTo(*sdcardTask); baroTask->connectTo(*sdcardTask); imuTask->connectTo(*sdcardTask); + + AutoPilotTask *autoPilotTask = new AutoPilotTask();//RIGM added + autoPilotTask->connectTo(*wifi); + autoPilotTask->connectTo(*n2kTask); // Add all the tasks kbox.addTask(new IntervalTask(new RunningLightTask(), 250)); @@ -97,6 +102,14 @@ void setup() { kbox.addTask(reader2); kbox.addTask(wifi); kbox.addTask(sdcardTask); + kbox.addTask(autoPilotTask); + + NavigationPage *navPage = new NavigationPage(); //RIGM added + imuTask->connectTo(*navPage); + adcTask->connectTo(*navPage); + autoPilotTask->connectTo(*navPage); + kbox.addPage(navPage); + navPage->connectTo(*autoPilotTask); BatteryMonitorPage *batPage = new BatteryMonitorPage(); adcTask->connectTo(*batPage); From bdae8839d65771627c4a88272eed83b61f97c40a Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:21:37 -0400 Subject: [PATCH 04/42] Adding autopilot functionality --- lib/KBox/src/KBox.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/KBox/src/KBox.h b/lib/KBox/src/KBox.h index 956b1505..dd7be2be 100644 --- a/lib/KBox/src/KBox.h +++ b/lib/KBox/src/KBox.h @@ -43,6 +43,7 @@ #include "tasks/IMUTask.h" #include "tasks/NMEAReaderTask.h" #include "tasks/WiFiTask.h" +#include "tasks/AutoPilotTask.h" //RIGM added /* Converters */ #include "converters/VoltageN2kConverter.h" @@ -51,6 +52,7 @@ /* Pages */ #include "pages/BatteryMonitorPage.h" #include "pages/StatsPage.h" +#include "pages/NavigationPage.h" //RIGM added /* Other dependencies */ #include From 3c3720a3079df63e71f6d7e2b0c4f41dd608f6d9 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:22:54 -0400 Subject: [PATCH 05/42] Adding autopilot functionality --- lib/KBox/src/KMessage.h | 107 +++++++++++++++++++++++++-- lib/KBox/src/KMessageNMEAVisitor.cpp | 41 ++++++++++ 2 files changed, 143 insertions(+), 5 deletions(-) diff --git a/lib/KBox/src/KMessage.h b/lib/KBox/src/KMessage.h index 59651b85..8d2c7986 100644 --- a/lib/KBox/src/KMessage.h +++ b/lib/KBox/src/KMessage.h @@ -1,3 +1,4 @@ + /* The MIT License @@ -34,6 +35,8 @@ class BarometerMeasurement; class VoltageMeasurement; class NMEA2000Message; class IMUMessage; +class NAVMessage; +class APMessage; class KMessage { private: @@ -51,6 +54,9 @@ class KVisitor { virtual void visit(const VoltageMeasurement &) {}; virtual void visit(const NMEA2000Message &) {}; virtual void visit(const IMUMessage &) {}; + virtual void visit(const NAVMessage &) {}; + virtual void visit(const APMessage &) {}; + }; class NMEASentence : public KMessage { @@ -145,41 +151,46 @@ class NMEA2000Message: public KMessage { class IMUMessage: public KMessage { private: + String label; //RIGM added, may not need it int calibration; double course, yaw, pitch, roll; public: static const int IMU_CALIBRATED = 3; - IMUMessage(int c, double course, double yaw, double pitch, double roll) : calibration(c), course(course), yaw(yaw), pitch(pitch), roll(roll) + IMUMessage(String label, int c, float course, float yaw, float pitch, float roll) : calibration(c), course(course), yaw(yaw), pitch(pitch), roll(roll) {}; void accept(KVisitor &v) const { v.visit(*this); }; + + String getLabel() const { + return label; + }; int getCalibration() const { return calibration; }; /* - * Heading in Radians + * Heading in Radians: for necessary precision need to use float, not double for radians (RIGM) */ - double getCourse() const { + float getCourse() const { return course; }; /* * Difference between vessel orientation and course over water in radians. */ - double getYaw() const { + float getYaw() const { return yaw; }; /* * Pitch in radians. Positive when bow rises. */ - double getPitch() const { + float getPitch() const { return pitch; }; @@ -191,6 +202,92 @@ class IMUMessage: public KMessage { }; }; +class NAVMessage: public KMessage { //RIGM added for autopilot functionality +private: + String label; //not sure we need a label + bool apMode, apHeadingMode, apWaypointMode, apDodgeMode; + double currentHeading, targetHeading, courseToWaypoint, rudderSensorPosition, targetRudderPosition, rudderCommandSent; + +public: + NAVMessage(String label, bool apMode, bool apHeadingMode, bool apWaypointMode, bool apDodgeMode, double currentHeading, double targetHeading, double courseToWaypoint, double rudderSensorPosition) : + label(label), apMode(apMode), apHeadingMode(apHeadingMode), apWaypointMode(apWaypointMode), apDodgeMode(apDodgeMode), + currentHeading(currentHeading),targetHeading(targetHeading),courseToWaypoint(courseToWaypoint), + rudderSensorPosition(rudderSensorPosition){}; + + void accept(KVisitor &v) const { + v.visit(*this); + }; + + String getLabel() const { + return label; + }; + + bool getApMode() const { + return apMode; + }; + + bool getApHeadingMode() const { + return apHeadingMode; + }; + + bool getApWaypointMode() const { + return apWaypointMode; + }; + + bool getApDodgeMode() const { + return apDodgeMode; + }; + + /* + * Courses in Degrees, therefore using doubles: + */ + double getCurrentHeading() const { + return currentHeading; + }; + + double getTargetHeading() const { + return targetHeading; + }; + + double getCourseToWaypoint() const { + return courseToWaypoint; + }; + + double getRudderSensorPosition() const { + return rudderSensorPosition; + }; + + +}; + +class APMessage: public KMessage { //RIGM added for autopilot functionality +private: + String label; //not sure we need a label + double targetRudderPosition, rudderCommandSent; + +public: + APMessage(String label, double targetRudderPosition, double rudderCommandSent) : + label(label), targetRudderPosition(targetRudderPosition),rudderCommandSent(rudderCommandSent){}; + + void accept(KVisitor &v) const { + v.visit(*this); + }; + + String getLabel() const { + return label; + }; + + double getTargetRudderPosition() const { + return targetRudderPosition; + }; + + double getRudderCommandSent() const { + return rudderCommandSent; + }; + +}; + + class KReceiver { public: virtual void processMessage(const KMessage&) = 0; diff --git a/lib/KBox/src/KMessageNMEAVisitor.cpp b/lib/KBox/src/KMessageNMEAVisitor.cpp index ae154b14..f709bc05 100644 --- a/lib/KBox/src/KMessageNMEAVisitor.cpp +++ b/lib/KBox/src/KMessageNMEAVisitor.cpp @@ -25,6 +25,12 @@ #include "KMessageNMEAVisitor.h" #include "util/NMEASentenceBuilder.h" #include "util/nmea2000.h" +//RIGM added +#include "tasks/NMEA2000Task.h" +#include "KBoxDebug.h" + +#define IMU_IN_NMEA2K true; //variable to determine whether IMU data sent out over N2K +unsigned int _imuSequence = 1; void KMessageNMEAVisitor::visit(const NMEASentence& s) { nmeaContent += s.getSentence() + "\r\n"; @@ -63,7 +69,40 @@ void KMessageNMEAVisitor::visit(const NMEA2000Message &n2km) { free(s); } + + void KMessageNMEAVisitor::visit(const IMUMessage &imu) { + +#ifdef IMU_IN_NMEA2K //adds IMU to N2K + + tN2kMsg n2kmessage; + tN2kMsg n2kmessage2; + _imuSequence++; + //DEBUG("imuSequence: %i course: %.1f yaw: %.1f roll: %.1f pitch: %.1f",_imuSequence,imu.getCourse(),imu.getYaw(),imu.getRoll(),imu.getPitch()); + //values in Radians from IMUTask + SetN2kPGN127257(n2kmessage, _imuSequence, imu.getYaw(), imu.getPitch(), imu.getRoll()); //sequence consistent with KMessage, NMEA2000 task + if(RadToDeg(imu.getCourse()) < 0){ + SetN2kPGN127250(n2kmessage2, _imuSequence, (imu.getCourse() - 0.261799), /* Deviation */ N2kDoubleNA, /* Variation */ N2kDoubleNA, N2khr_magnetic); + //for some strange reason, when converting RadToDeg for N2K streaming purposes, values over 180 are off by 15 degrees. This does not affect the heading shown on the display, only NMEA viewed on other devices + } else { + SetN2kPGN127250(n2kmessage2, _imuSequence, imu.getCourse(), /* Deviation */ N2kDoubleNA, /* Variation */ N2kDoubleNA, N2khr_magnetic); + } + + char *s = nmea_pcdin_sentence_for_n2kmsg(n2kmessage, millis() / 1000); + //DEBUG("nmeaContent: %s", s); + nmeaContent += String(s) + "\r\n"; + free(s); + + + s = nmea_pcdin_sentence_for_n2kmsg(n2kmessage2, millis() / 1000); + //DEBUG("nmeaContent: %s", s); + nmeaContent += String(s) + "\r\n"; + free(s); + + +#endif + +#ifndef IMU_IN_NMEA2K NMEASentenceBuilder sb("II", "XDR", 16); sb.setField(1, "A"); sb.setField(2, imu.getYaw(), 1); @@ -92,5 +131,7 @@ void KMessageNMEAVisitor::visit(const IMUMessage &imu) { sb2.setField(2, "M"); nmeaContent += sb2.toNMEA() + "\r\n"; +#endif + } From 9a17bd63b314c23e0487b7e02934f6d06adebacf Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:23:47 -0400 Subject: [PATCH 06/42] Adding autopilot functionality --- lib/KBox/src/pages/NavigationPage.cpp | 305 ++++++++++++++++++++++++++ lib/KBox/src/pages/NavigationPage.h | 67 ++++++ 2 files changed, 372 insertions(+) create mode 100644 lib/KBox/src/pages/NavigationPage.cpp create mode 100644 lib/KBox/src/pages/NavigationPage.h diff --git a/lib/KBox/src/pages/NavigationPage.cpp b/lib/KBox/src/pages/NavigationPage.cpp new file mode 100644 index 00000000..8813ce10 --- /dev/null +++ b/lib/KBox/src/pages/NavigationPage.cpp @@ -0,0 +1,305 @@ +/* + The MIT License + + Copyright (c) 2016 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. +*/ + +/* AutoPilot functionality can exist at several levels. In this initial iteration, the design + focused on heading the boat on a defined course that is automatically set when AP functionality + is turned on (referred to as "Heading" mode below. Later on we could introduce an additional + mode when a course to a waypoint is provided by a NMEA-connected device. In addition, we have + "Dodge" mode which suspends rudder actions temporarily in order to manually steer the boat around + an object or to manually put it back on course. Waypoint functionality will be added at a later time. RIGM + */ + +#include "NavigationPage.h" +#include "MFD.h" +#include +#include "KBoxDebug.h" + + +bool apMode = false; +bool apDodgeMode = false; +bool apHeadingMode = false; +bool apWaypointMode = false;//reserved for future use +String apModeString = "APMode: Off"; // Modes: "Off", "Heading" == follow set heading, "Waypoint" = GoTo provided Waypoint, "Dodge" +String apWaypointString = "045 20.2N, 081 50.3W";//placeholder for future use +double courseToWaypoint = 0; + +int navEncoderClicks = 0; +int encoderClicks = 0; + +//String navRudderMovementIndicator = "|"; + +double initialTargetHeading = 0; +double navCurrentDisplayHeading = 0; +double navTargetDisplayHeading = 0; +double rudderCommandForDisplay = 0; +double rudderAngleForDisplay = 0; +double rawHeading = 0; +double rawRudderAngle = 0; +double rawTargetHeading = 0; +bool isTargetSet = false; + +#define P_Param 0.5 //proportional param P-gain, the gain determines how much change the OP will make due to a change in error +#define I_Param 0.0 //integral or I-gain, the the reset determines how much to change the OP over time due to the error +#define D_Param 0.0 //derivative or D-gain, the preact determines how much to change the OP due from a change in direction of the error + + +NavigationPage::NavigationPage() { + static const int col1 = 5; + //static const int col2 = 200; + static const int col2 = 160; + static const int row1 = 20; + static const int row2 = 40; + static const int row3 = 70; + static const int row4 = 152; + static const int row5 = 182; + + + addLayer(new TextLayer(Point(col1, row2), Size(20, 20), "Heading", ColorWhite, ColorBlack, FontDefault)); + addLayer(new TextLayer(Point(col2, row2), Size(20, 20), "Target Heading", ColorWhite, ColorBlack, FontDefault)); + addLayer(new TextLayer(Point(col1, row4), Size(20, 20), "Rudder Position", ColorWhite, ColorBlack, FontDefault)); + addLayer(new TextLayer(Point(col2, row4), Size(20, 20), "Rudder Command", ColorWhite, ColorBlack, FontDefault)); + + apModeDisplay= new TextLayer(Point(col1, row1), Size(20, 20), apModeString, ColorBlue, ColorBlack, FontDefault); + waypointDisplay = new TextLayer(Point(col2, row1), Size(20, 20), apWaypointString, ColorWhite, ColorBlack, FontDefault); //reserved for future use + headingDisplay = new TextLayer(Point(col1, row3), Size(20, 20), "----", ColorWhite, ColorBlack, FontLarge); + targetHeadingDisplay = new TextLayer(Point(col2, row3), Size(20, 20), "----", ColorWhite, ColorBlack, FontLarge); + rudderPositionDisplay = new TextLayer(Point(col1, row5), Size(20, 20), "----", ColorWhite, ColorBlack, FontLarge); + rudderCommandDisplay = new TextLayer(Point(col2, row5), Size(20, 20), "----", ColorWhite, ColorBlack, FontLarge); + + addLayer(apModeDisplay); + addLayer(waypointDisplay); + addLayer(headingDisplay); + addLayer(targetHeadingDisplay); + addLayer(rudderPositionDisplay); + addLayer(rudderCommandDisplay); +} + +Color NavigationPage::colorForRudder(float r) { + if (r == 0) { + return ColorWhite; + } + if (r > 0) { + return ColorGreen; + } + if (r < 0) { + return ColorRed; + } + return ColorWhite; +} +Color NavigationPage::colorForCourse(float c) { + return ColorWhite; +} + +bool NavigationPage::processEvent(const ButtonEvent &be) { + if (be.clickType == ButtonEventTypePressed) { + this->buttonPressed = true; + this->buttonPressedTimer = 0; + //DEBUG("Setting buttonPressed to true"); + } + + if (be.clickType == ButtonEventTypeReleased && this->buttonPressed == true) { + if (this->buttonPressedTimer < 2000) { + // short click - return false to force the MFD to skip to the next page + //DEBUG("short click"); + return false; + } + else if (this->buttonPressedTimer < 5000) { //medium click, if navMode is on, activate dodge mode + //DEBUG("medium click"); + if (apMode == true){ + if (apDodgeMode == true){ + apDodgeMode = false; + if (apHeadingMode == true){ + apModeString = "APMode: Heading"; + } else if (apWaypointMode == true){ + apModeString = "APMode: Waypoint"; + } + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorGreen); + } else { + apDodgeMode = true; + apModeString = "APMode: Dodge ";//extra spaces needed to overwrite old string if longer + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorRed); + } + return true; + } else { + DEBUG("apMode not active so no effect"); + } + } else if (this->buttonPressedTimer > 5000) { // Long click - start and stop the navMode here + DEBUG("long click"); + if (apMode == false){ + apMode = true; + apHeadingMode = true; + apModeString = "APMode: Heading ";//extra spaces needed to overwrite old string if longer + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorGreen); + } else { + apMode = false; + apDodgeMode = false; + apModeString = "APMode: Off "; + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorBlue); + } + return true; + } + } + //DEBUG("APmode is: - %d", apMode); + //DEBUG("APHeadingmode is: - %d", apHeadingMode); + //DEBUG("APDodgemode is: - %d", apDodgeMode); + return true; +} + +bool NavigationPage::processEvent(const EncoderEvent &ee) { + encoderClicks = ee.rotation; + navEncoderClicks = navEncoderClicks + encoderClicks; + encoderClicks = 0; + + //DEBUG("encoderClicks - %d", encoderClicks); + //DEBUG("navEncoderClicks - %d", navEncoderClicks); + return true; +} + + +String NavigationPage::formatMeasurement(float measure, const char *unit) { + // extra spaces at the end needed to clear previous value completely + // (we use a non-fixed width font) + char s[10]; + //snprintf(s, sizeof(s), "%.1f %s ", measure, unit); + snprintf(s, sizeof(s), "%.0f %s ", measure, unit); + + return String(s); +} + + +void NavigationPage::processMessage(const KMessage &message) { + message.accept(*this); +} + +void NavigationPage::visit(const VoltageMeasurement &vm) { + if (vm.getLabel() == "bat3") { + rawRudderAngle = vm.getVoltage(); + //provision for tuning + navRudderSensorPosition = rawRudderAngle; + } +} + + +void NavigationPage::visit(const APMessage &ap) { //this function never called + + navTargetRudderPosition = ap.getTargetRudderPosition(); + navTargetRudderPosition = ap.getRudderCommandSent(); + //DEBUG("testRudderCommandPosition - %.0f",navTargetRudderPosition); + //DEBUG("testRudderCommandSent - %.0f",navRudderCommandSent); + + + +} + +void NavigationPage::visit(const IMUMessage &imu) { + + rawHeading = RadToDeg(imu.getCourse()); + if (rawHeading < 0){ + navCurrentHeading = 720 + rawHeading;//adding 720 for nav math purposes + navCurrentDisplayHeading = 360 + rawHeading; + } else { + navCurrentHeading = 720 + rawHeading; + navCurrentDisplayHeading = rawHeading; + } + headingDisplay->setText(formatMeasurement(navCurrentDisplayHeading, "")); + headingDisplay->setColor(colorForCourse(navCurrentDisplayHeading)); + + if (apMode == true && apHeadingMode == true ){ + if (isTargetSet == false){ + initialTargetHeading = navCurrentHeading; + navTargetHeading = initialTargetHeading; + isTargetSet = true; + } else { + navTargetHeading = initialTargetHeading + navEncoderClicks; + if ((navTargetHeading - 720) < 0){ + navTargetDisplayHeading = 360 + (navTargetHeading - 720) ; + } else { + navTargetDisplayHeading = navTargetHeading - 720; + } + } + targetHeadingDisplay->setText(formatMeasurement(navTargetDisplayHeading, "")); + targetHeadingDisplay->setColor(colorForCourse(navTargetDisplayHeading)); + + + if (navTargetRudderPosition == 33){ // 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right + rudderCommandForDisplay = 0; + } else if (navTargetRudderPosition < 33){ //32 sb 1 so display 33 - command + rudderCommandForDisplay = 33 - navTargetRudderPosition; + } else if (navTargetRudderPosition > 33){ //34 sb -1 so display 66 - + rudderCommandForDisplay = (navTargetRudderPosition - 32) * -1 ; //rounding anomaly to avoid displaying -0 + } + rudderCommandDisplay->setText(formatMeasurement(rudderCommandForDisplay, "")); + rudderCommandDisplay->setColor(colorForRudder(rudderCommandForDisplay)); + + //this should ultimately be done with the rudderCommandSent variable, not the rudderCommandForDisplay + if (rudderCommandForDisplay == navRudderSensorPosition){ //zero angle + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "|")); + } + if (rudderCommandForDisplay >= 0){ + if (navRudderSensorPosition < rudderCommandForDisplay){ + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ">")); + } else { + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "<")); + } + } else { + if (navRudderSensorPosition > rudderCommandForDisplay){ + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "<")); + } else { + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ">")); + } + } + rudderPositionDisplay->setColor(colorForRudder(navRudderSensorPosition)); + + } else if (apMode == true && apWaypointMode == true ){ + + //reserved for future use when waypoint functionality added + + } else { //AP mode is false + isTargetSet = false; + targetHeadingDisplay->setText("---- "); + targetHeadingDisplay->setColor(ColorWhite); + + rudderCommandDisplay->setText("---- "); + rudderCommandDisplay->setColor(ColorWhite); + + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "")); + rudderPositionDisplay->setColor(colorForRudder(navRudderSensorPosition)); + } + + NAVMessage m("test", apMode, apHeadingMode, apWaypointMode, apDodgeMode, navCurrentHeading, navTargetHeading, courseToWaypoint, rawRudderAngle); + sendMessage(m); + +} + + + + + + + + diff --git a/lib/KBox/src/pages/NavigationPage.h b/lib/KBox/src/pages/NavigationPage.h new file mode 100644 index 00000000..923d3e70 --- /dev/null +++ b/lib/KBox/src/pages/NavigationPage.h @@ -0,0 +1,67 @@ +/* + The MIT License + + Copyright (c) 2016 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 "MFD.h" +#include "../tasks/AutoPilotTask.h" +#include "../tasks/NMEA2000Task.h" +#include "KMessage.h" +#include "ui/TextLayer.h" + +/* +extern double navCurrentHeading; +extern double navTargetHeading; +extern double navTargetRudderPosition; +extern double navRudderSensorPosition; +extern double navRudderCommandSent; +extern String navRudderMovementIndicator; + */ + + +class NavigationPage : public Page, public KReceiver, public KVisitor, public KGenerator { + private: + TextLayer *apModeDisplay, *waypointDisplay, *headingDisplay, *targetHeadingDisplay, *rudderPositionDisplay, *rudderCommandDisplay; + + Color colorForRudder(float r); + Color colorForCourse(float c); + String formatMeasurement(float measure, const char *unit); + + bool buttonPressed = false; + elapsedMillis buttonPressedTimer; + + double navCurrentHeading; + double navTargetHeading; + double navRudderSensorPosition; + double navTargetRudderPosition; + double navRudderCommandSent; + + public: + NavigationPage(); + + void processMessage(const KMessage& message); + void visit(const IMUMessage&); + void visit(const APMessage&); + void visit(const VoltageMeasurement&); + bool processEvent(const ButtonEvent &be); + bool processEvent(const EncoderEvent &ee); +}; From 49afcf6949de717dace01fb20921702a2d0c52e3 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:24:36 -0400 Subject: [PATCH 07/42] Adding autopilot functionality --- lib/KBox/src/tasks/AutoPilotTask.cpp | 136 +++++++++++++++++++++++++++ lib/KBox/src/tasks/AutoPilotTask.h | 55 +++++++++++ 2 files changed, 191 insertions(+) create mode 100644 lib/KBox/src/tasks/AutoPilotTask.cpp create mode 100644 lib/KBox/src/tasks/AutoPilotTask.h diff --git a/lib/KBox/src/tasks/AutoPilotTask.cpp b/lib/KBox/src/tasks/AutoPilotTask.cpp new file mode 100644 index 00000000..ae6eebc4 --- /dev/null +++ b/lib/KBox/src/tasks/AutoPilotTask.cpp @@ -0,0 +1,136 @@ +/* + The MIT License + + Copyright (c) 2016 Thomas Sarlandie thomas@sarlandie.net + Added by Rob McLean, March 2017 + + 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. +*/ + +/* AutoPilot functionality can exist at several levels. In this initial iteration, the design + focused on heading the boat on a defined course that is automatically set when AP functionality + is turned on (referred to as "Heading" mode below. Later on we could introduce an additional + mode when a course to a waypoint is provided by a NMEA-connected device. In addition, we have + "Dodge" mode which suspends rudder actions temporarily in order to manually steer the boat around + an object or to manually put it back on course. + + The course calculation approach below is a simplified and adapted version of Robert Huitema's FreeboardPLC_v1_2 code. RIGM + */ + +#include "AutoPilotTask.h" +#include "../pages/NavigationPage.h" +#include "KBoxDebug.h" +#include "../drivers/board.h" +#include "KMessage.h" + +#include +#include + + +double apCurrentHeading = 0; +double apTargetHeading = 0; +double apTargetRudderPosition = 0; + + + +/*pending tuning,following is set in AutoPilotTask.h +#define P_Param 0.5 //proportional param P-gain, the gain determines how much change the OP will make due to a change in error +#define I_Param 0.0 //integral or I-gain, the the reset determines how much to change the OP over time due to the error +#define D_Param 0.0 //derivative or D-gain, the preact determines how much to change the OP due from a change in direction of the error + */ + +PID headingPID(&apCurrentHeading, &apTargetRudderPosition, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int)REVERSE); +bool isSetup = false; //if false sets initial PID variables on bootup + +void AutoPilotTask::processMessage(const KMessage &message) { + message.accept(*this); +} + + +void AutoPilotTask::visit(const NAVMessage &nav) { //this function now working!! + + apCurrentHeading = nav.getCurrentHeading(); + apTargetHeading = nav.getTargetHeading(); + apRudderSensorPosition = nav.getRudderSensorPosition(); + + // DEBUG("apPilotCurrentHeading - %.0f",apCurrentHeading); +} + +void AutoPilotTask::loop() { + + if (isSetup == false){ //tried a separate "setup" for init but could not get it to work + sameLastDirection=true; + apTargetRudderPosition = 33; //centred. For programming purposes rudder is assumed to move through 66 degrees lock to lock + headingPID.SetMode(AUTOMATIC); + headingPID.SetOutputLimits(0.0, 66.0); //output limits 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right) + headingPID.SetSampleTime(250); + DEBUG("Init autopilot complete"); + isSetup = true; + } + + headingPID.Compute(); + + autoPilotOffCourse = apTargetHeading - apCurrentHeading; //variable not currently used for anything + + autoPilotDeadZone = 5; //pending tuning on water + autoPilotSlack = 5; //pending tuning on water + apRudderCommandSent = apTargetRudderPosition; //by default send rudder command to equal target position, then modify it + double testDegs = apTargetRudderPosition - apRudderCommandSent; + + if (abs(testDegs) > autoPilotDeadZone) { + DEBUG("need to move the rudder"); + + //then we move the rudder. + //is it changing movement direction, we need to compensate for slack + if (sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { + //same direction to stbd, no slack + apRudderCommandSent = apTargetRudderPosition; + }else if (sameLastDirection && apTargetRudderPosition < apRudderCommandSent) { + //changed direction to port, subtract slack + sameLastDirection = false; + apRudderCommandSent = apTargetRudderPosition - autoPilotSlack; + }else if (!sameLastDirection && apTargetRudderPosition < apRudderCommandSent) { + //same direction to port + apRudderCommandSent = apTargetRudderPosition; + }else if (!sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { + //changed direction to stbd, add slack + sameLastDirection = true; + apRudderCommandSent = apTargetRudderPosition + autoPilotSlack; + } + + + } + //DEBUG("navTargetRudderPosition - %.0f", navTargetRudderPosition); + //DEBUG("navRudderCommandSent2 - %.0f", navRudderCommandSent); + + APMessage m("test", apTargetRudderPosition, apRudderCommandSent); + sendMessage(m); + + + //Add code here to actually move the rudder based on controller system used + +} + + + + + + + + diff --git a/lib/KBox/src/tasks/AutoPilotTask.h b/lib/KBox/src/tasks/AutoPilotTask.h new file mode 100644 index 00000000..072d2aa6 --- /dev/null +++ b/lib/KBox/src/tasks/AutoPilotTask.h @@ -0,0 +1,55 @@ +/* + The MIT License + + Copyright (c) 2016 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 +#include "TaskManager.h" +#include "KMessage.h" +#include + +//original values 0.45, 0.08, 0.52 + +#define P_Param 0.5 //proportional param P-gain, the gain determines how much change the OP will make due to a change in error +#define I_Param 0.0 //integral or I-gain, the the reset determines how much to change the OP over time due to the error +#define D_Param 0.0 //derivative or D-gain, the preact determines how much to change the OP due from a change in direction of the error + +//extern double autoPilotRudderCommand; + +class AutoPilotTask : public Task, public KReceiver, public KVisitor, public KGenerator { private: + + + double apRudderSensorPosition; + double apRudderCommandSent; + double autoPilotOffCourse; //not currently used + double autoPilotDeadZone; //variable to be tuned based on experience + double autoPilotSlack; //variable to be tuned based on experience + bool sameLastDirection; //last rudder movement direction + + public: + AutoPilotTask() : Task("AutoPilot") {}; + void processMessage(const KMessage& message); + void visit(const NAVMessage&); + void loop(); + +}; From e8275893531de6ca0728e8c35695b6feaa65a162 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:26:02 -0400 Subject: [PATCH 08/42] Added variable to configure KBOX orientation --- lib/KBox/src/tasks/IMUTask.cpp | 29 +++++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/lib/KBox/src/tasks/IMUTask.cpp b/lib/KBox/src/tasks/IMUTask.cpp index 28008e3b..b9f1d30f 100644 --- a/lib/KBox/src/tasks/IMUTask.cpp +++ b/lib/KBox/src/tasks/IMUTask.cpp @@ -24,6 +24,10 @@ #include "IMUTask.h" #include "KBoxDebug.h" +const String kBoxOrientation = "bow"; //Modification to send correct course, pitch, roll data based on where the back of the KBOX is mounted. Options are back of the box to port, to bow, to stbd, to stern + + + void IMUTask::setup() { DEBUG("Initing BNO055"); if (!bno055.begin()) { @@ -47,8 +51,29 @@ void IMUTask::loop() { eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); //DEBUG("Vector Euler x=%f y=%f z=%f", eulerAngles.x(), eulerAngles.y(), eulerAngles.z()); //DEBUG("Course: %.0f MAG Pitch: %.1f Heel: %.1f", eulerAngles.x(), eulerAngles.y(), eulerAngles.z()); - IMUMessage m(sysCalib, eulerAngles.x(), /* yaw?*/ 0, eulerAngles.y(), eulerAngles.z()); - sendMessage(m); + //IMUMessage m(sysCalib, eulerAngles.x(), /* yaw?*/ 0, eulerAngles.y(), eulerAngles.z()); //original + //sendMessage(m); + + + if (kBoxOrientation == "port"){ + IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x() - 90), /* yaw?*/ 0, DegToRad(eulerAngles.y() * -1), DegToRad(eulerAngles.z())); + sendMessage(m); + } + + if (kBoxOrientation == "stbd"){ + IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x() + 90), /* yaw?*/ 0, DegToRad(eulerAngles.y()), DegToRad(eulerAngles.z() * -1)); + sendMessage(m); + } + + if (kBoxOrientation == "bow"){ + IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x() - 180), /* yaw?*/ 0, DegToRad(eulerAngles.y()), DegToRad(eulerAngles.z())); + sendMessage(m); + } + + if (kBoxOrientation == "stern"){ + IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x()), /* yaw?*/ 0, DegToRad(eulerAngles.z() * -1), DegToRad(eulerAngles.y() * -1)); + sendMessage(m); + } } //DEBUG("BNO055 temperature is %i", bno055.getTemp()); From db87e769d49a504969c502a5f35e58641146e0da Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:27:07 -0400 Subject: [PATCH 09/42] Warning when IMU not configured --- lib/KBox/src/tasks/NMEA2000Task.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/KBox/src/tasks/NMEA2000Task.cpp b/lib/KBox/src/tasks/NMEA2000Task.cpp index bad00439..8ff6e2a7 100644 --- a/lib/KBox/src/tasks/NMEA2000Task.cpp +++ b/lib/KBox/src/tasks/NMEA2000Task.cpp @@ -113,6 +113,8 @@ void NMEA2000Task::visit(const IMUMessage &m) { sendN2kMessage(n2kmessage); _imuSequence++; + } else { + DEBUG("IMU is not calibrated: move the unit around to calibrate it"); //debug message added to warn user if IMU not calibrated } } From f74058c762fe9ab0a5d409ded0444040e51df8e2 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:27:59 -0400 Subject: [PATCH 10/42] Rudder sensor input on bat3 --- lib/KBox/src/tasks/ADCTask.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/lib/KBox/src/tasks/ADCTask.cpp b/lib/KBox/src/tasks/ADCTask.cpp index 137d43ca..b4430ad2 100644 --- a/lib/KBox/src/tasks/ADCTask.cpp +++ b/lib/KBox/src/tasks/ADCTask.cpp @@ -33,11 +33,18 @@ void ADCTask::loop() { int bat1_adc = adc.analogRead(bat1_analog, ADC_0); int bat2_adc = adc.analogRead(bat2_analog, ADC_0); int bat3_adc = adc.analogRead(bat3_analog, ADC_0); + + //permits rudder sensor on bat3 input. Eventually should move rudder sensor calcs to a separate task + double maxAngle = 680; //this is the max reading based on the potentiometer I am using at 3.3v. + double midAngle = maxAngle / 2; + int rudderFactor = 10; //680 / 10 is close to the rudder angle range currently defined as 0-66. Factor could require a bit of tuning based on the actual installation in the boat + double rudderAngle = (bat3_adc - midAngle) / rudderFactor; supply = supply_adc * analog_max_voltage / adc.getMaxValue(); bat1 = bat1_adc * analog_max_voltage / adc.getMaxValue(); bat2 = bat2_adc * analog_max_voltage / adc.getMaxValue(); - bat3 = bat3_adc * analog_max_voltage / adc.getMaxValue(); + //bat3 = bat3_adc * analog_max_voltage / adc.getMaxValue(); + bat3 = rudderAngle; //DEBUG("ADC - Supply: %sV Bat1: %sV Bat2: %sV Bat3: %sV", //String(supply, 2).c_str(), String(bat1, 2).c_str(), String(bat2, 2).c_str(), String(bat3, 2).c_str()); From 8d32f7ad83e783334b3c4d868aaeb11e06074bc1 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:32:56 -0400 Subject: [PATCH 11/42] Create test --- lib/Arduino-PID-Library/test | 1 + 1 file changed, 1 insertion(+) create mode 100644 lib/Arduino-PID-Library/test diff --git a/lib/Arduino-PID-Library/test b/lib/Arduino-PID-Library/test new file mode 100644 index 00000000..9daeafb9 --- /dev/null +++ b/lib/Arduino-PID-Library/test @@ -0,0 +1 @@ +test From 37f5edda21e2030972188a5558a11345371afcb4 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:33:13 -0400 Subject: [PATCH 12/42] Delete test --- lib/Arduino-PID-Library/test | 1 - 1 file changed, 1 deletion(-) delete mode 100644 lib/Arduino-PID-Library/test diff --git a/lib/Arduino-PID-Library/test b/lib/Arduino-PID-Library/test deleted file mode 100644 index 9daeafb9..00000000 --- a/lib/Arduino-PID-Library/test +++ /dev/null @@ -1 +0,0 @@ -test From 84e2047fc8da7f4d2fd5646c66a171f67ab87dc6 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:33:44 -0400 Subject: [PATCH 13/42] Create test --- lib/Arduino-PID-Library/test | 1 + 1 file changed, 1 insertion(+) create mode 100644 lib/Arduino-PID-Library/test diff --git a/lib/Arduino-PID-Library/test b/lib/Arduino-PID-Library/test new file mode 100644 index 00000000..9daeafb9 --- /dev/null +++ b/lib/Arduino-PID-Library/test @@ -0,0 +1 @@ +test From 3c0fb544c09390bedc683fc07acf2f4b3df9d15c Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:34:45 -0400 Subject: [PATCH 14/42] Adding PID library --- lib/Arduino-PID-Library/PID_v1.cpp | 195 +++++++++++++++++++++ lib/Arduino-PID-Library/PID_v1.h | 81 +++++++++ lib/Arduino-PID-Library/README.txt | 11 ++ lib/Arduino-PID-Library/keywords.txt | 34 ++++ lib/Arduino-PID-Library/library.json | 19 ++ lib/Arduino-PID-Library/library.properties | 9 + 6 files changed, 349 insertions(+) create mode 100644 lib/Arduino-PID-Library/PID_v1.cpp create mode 100644 lib/Arduino-PID-Library/PID_v1.h create mode 100644 lib/Arduino-PID-Library/README.txt create mode 100644 lib/Arduino-PID-Library/keywords.txt create mode 100644 lib/Arduino-PID-Library/library.json create mode 100644 lib/Arduino-PID-Library/library.properties diff --git a/lib/Arduino-PID-Library/PID_v1.cpp b/lib/Arduino-PID-Library/PID_v1.cpp new file mode 100644 index 00000000..86222c72 --- /dev/null +++ b/lib/Arduino-PID-Library/PID_v1.cpp @@ -0,0 +1,195 @@ +/********************************************************************************************** + * Arduino PID Library - Version 1.1.1 + * by Brett Beauregard brettbeauregard.com + * + * This Library is licensed under a GPLv3 License + **********************************************************************************************/ + +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include + +/*Constructor (...)********************************************************* + * The parameters specified here are those for for which we can't set up + * reliable defaults, so we need to have the user set them. + ***************************************************************************/ +PID::PID(double* Input, double* Output, double* Setpoint, + double Kp, double Ki, double Kd, int ControllerDirection) +{ + + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + PID::SetOutputLimits(0, 255); //default output limit corresponds to + //the arduino pwm limits + + SampleTime = 100; //default Controller Sample Time is 0.1 seconds + + PID::SetControllerDirection(ControllerDirection); + PID::SetTunings(Kp, Ki, Kd); + + lastTime = millis()-SampleTime; +} + + +/* Compute() ********************************************************************** + * This, as they say, is where the magic happens. this function should be called + * every time "void loop()" executes. the function will decide for itself whether a new + * pid Output needs to be computed. returns true when the output is computed, + * false when nothing has been done. + **********************************************************************************/ +bool PID::Compute() +{ + if(!inAuto) return false; + unsigned long now = millis(); + unsigned long timeChange = (now - lastTime); + if(timeChange>=SampleTime) + { + /*Compute all the working error variables*/ + double input = *myInput; + double error = *mySetpoint - input; + ITerm+= (ki * error); + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + double dInput = (input - lastInput); + + /*Compute PID Output*/ + double output = kp * error + ITerm- kd * dInput; + + if(output > outMax) output = outMax; + else if(output < outMin) output = outMin; + *myOutput = output; + + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + return true; + } + else return false; +} + + +/* SetTunings(...)************************************************************* + * This function allows the controller's dynamic performance to be adjusted. + * it's called automatically from the constructor, but tunings can also + * be adjusted on the fly during normal operation + ******************************************************************************/ +void PID::SetTunings(double Kp, double Ki, double Kd) +{ + if (Kp<0 || Ki<0 || Kd<0) return; + + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + double SampleTimeInSec = ((double)SampleTime)/1000; + kp = Kp; + ki = Ki * SampleTimeInSec; + kd = Kd / SampleTimeInSec; + + if(controllerDirection ==REVERSE) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } +} + +/* SetSampleTime(...) ********************************************************* + * sets the period, in Milliseconds, at which the calculation is performed + ******************************************************************************/ +void PID::SetSampleTime(int NewSampleTime) +{ + if (NewSampleTime > 0) + { + double ratio = (double)NewSampleTime + / (double)SampleTime; + ki *= ratio; + kd /= ratio; + SampleTime = (unsigned long)NewSampleTime; + } +} + +/* SetOutputLimits(...)**************************************************** + * This function will be used far more often than SetInputLimits. while + * the input to the controller will generally be in the 0-1023 range (which is + * the default already,) the output will be a little different. maybe they'll + * be doing a time window and will need 0-8000 or something. or maybe they'll + * want to clamp it from 0-125. who knows. at any rate, that can all be done + * here. + **************************************************************************/ +void PID::SetOutputLimits(double Min, double Max) +{ + if(Min >= Max) return; + outMin = Min; + outMax = Max; + + if(inAuto) + { + if(*myOutput > outMax) *myOutput = outMax; + else if(*myOutput < outMin) *myOutput = outMin; + + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + } +} + +/* SetMode(...)**************************************************************** + * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + * when the transition from manual to auto occurs, the controller is + * automatically initialized + ******************************************************************************/ +void PID::SetMode(int Mode) +{ + bool newAuto = (Mode == AUTOMATIC); + if(newAuto && !inAuto) + { /*we just went from manual to auto*/ + PID::Initialize(); + } + inAuto = newAuto; +} + +/* Initialize()**************************************************************** + * does all the things that need to happen to ensure a bumpless transfer + * from manual to automatic mode. + ******************************************************************************/ +void PID::Initialize() +{ + ITerm = *myOutput; + lastInput = *myInput; + if(ITerm > outMax) ITerm = outMax; + else if(ITerm < outMin) ITerm = outMin; +} + +/* SetControllerDirection(...)************************************************* + * The PID will either be connected to a DIRECT acting process (+Output leads + * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + * know which one, because otherwise we may increase the output when we should + * be decreasing. This is called from the constructor. + ******************************************************************************/ +void PID::SetControllerDirection(int Direction) +{ + if(inAuto && Direction !=controllerDirection) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +/* Status Funcions************************************************************* + * Just because you set the Kp=-1 doesn't mean it actually happened. these + * functions query the internal state of the PID. they're here for display + * purposes. this are the functions the PID Front-end uses for example + ******************************************************************************/ +double PID::GetKp(){ return dispKp; } +double PID::GetKi(){ return dispKi;} +double PID::GetKd(){ return dispKd;} +int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} +int PID::GetDirection(){ return controllerDirection;} + diff --git a/lib/Arduino-PID-Library/PID_v1.h b/lib/Arduino-PID-Library/PID_v1.h new file mode 100644 index 00000000..f6b4e4ff --- /dev/null +++ b/lib/Arduino-PID-Library/PID_v1.h @@ -0,0 +1,81 @@ +#ifndef PID_v1_h +#define PID_v1_h +#define LIBRARY_VERSION 1.1.1 + +class PID +{ + + + public: + + //Constants used in some of the functions below + #define AUTOMATIC 1 + #define MANUAL 0 + #define DIRECT 0 + #define REVERSE 1 + + //commonly used functions ************************************************************************** + + PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and + double, double, double, int); // Setpoint. Initial tuning parameters are also set here + + void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) + + bool Compute(); // * performs the PID calculation. it should be + // called every time loop() cycles. ON/OFF and + // calculation frequency can be set using SetMode + // SetSampleTime respectively + + void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but + //it's likely the user will want to change this depending on + //the application + + + + //available but not commonly used functions ******************************************************** + void SetTunings(double, double, // * While most users will set the tunings once in the + double); // constructor, this function gives the user the option + // of changing tunings during runtime for Adaptive control + void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT + // means the output will increase when error is positive. REVERSE + // means the opposite. it's very unlikely that this will be needed + // once it is set in the constructor. + void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which + // the PID calculation is performed. default is 100 + + + + //Display functions **************************************************************** + double GetKp(); // These functions query the pid for interal values. + double GetKi(); // they were created mainly for the pid front-end, + double GetKd(); // where it's important to know what is actually + int GetMode(); // inside the PID. + int GetDirection(); // + + private: + void Initialize(); + + double dispKp; // * we'll hold on to the tuning parameters in user-entered + double dispKi; // format for display purposes + double dispKd; // + + double kp; // * (P)roportional Tuning Parameter + double ki; // * (I)ntegral Tuning Parameter + double kd; // * (D)erivative Tuning Parameter + + int controllerDirection; + + double *myInput; // * Pointers to the Input, Output, and Setpoint variables + double *myOutput; // This creates a hard link between the variables and the + double *mySetpoint; // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. + + unsigned long lastTime; + double ITerm, lastInput; + + unsigned long SampleTime; + double outMin, outMax; + bool inAuto; +}; +#endif + diff --git a/lib/Arduino-PID-Library/README.txt b/lib/Arduino-PID-Library/README.txt new file mode 100644 index 00000000..61f4be25 --- /dev/null +++ b/lib/Arduino-PID-Library/README.txt @@ -0,0 +1,11 @@ +*************************************************************** +* Arduino PID Library - Version 1.1.1 +* by Brett Beauregard brettbeauregard.com +* +* This Library is licensed under a GPLv3 License +*************************************************************** + + - For an ultra-detailed explanation of why the code is the way it is, please visit: + http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ + + - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary diff --git a/lib/Arduino-PID-Library/keywords.txt b/lib/Arduino-PID-Library/keywords.txt new file mode 100644 index 00000000..55969c17 --- /dev/null +++ b/lib/Arduino-PID-Library/keywords.txt @@ -0,0 +1,34 @@ +####################################### +# Syntax Coloring Map For PID Library +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +PID KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +SetMode KEYWORD2 +Compute KEYWORD2 +SetOutputLimits KEYWORD2 +SetTunings KEYWORD2 +SetControllerDirection KEYWORD2 +SetSampleTime KEYWORD2 +GetKp KEYWORD2 +GetKi KEYWORD2 +GetKd KEYWORD2 +GetMode KEYWORD2 +GetDirection KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + +AUTOMATIC LITERAL1 +MANUAL LITERAL1 +DIRECT LITERAL1 +REVERSE LITERAL1 \ No newline at end of file diff --git a/lib/Arduino-PID-Library/library.json b/lib/Arduino-PID-Library/library.json new file mode 100644 index 00000000..cf2510c6 --- /dev/null +++ b/lib/Arduino-PID-Library/library.json @@ -0,0 +1,19 @@ +{ + "name": "PID", + "keywords": "PID, controller, signal", + "description": "A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).", + "url": "http://playground.arduino.cc/Code/PIDLibrary", + "include": "PID_v1", + "authors": + [ + { + "name": "Brett Beauregard" + } + ], + "repository": + { + "type": "git", + "url": "https://github.com/br3ttb/Arduino-PID-Library.git" + }, + "frameworks": "arduino" +} diff --git a/lib/Arduino-PID-Library/library.properties b/lib/Arduino-PID-Library/library.properties new file mode 100644 index 00000000..9a316b0b --- /dev/null +++ b/lib/Arduino-PID-Library/library.properties @@ -0,0 +1,9 @@ +name=PID +version=1.1.1 +author=Brett Beauregard +maintainer=Brett Beauregard +sentence=PID controller +paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). +category=Signal Input/Output +url=http://playground.arduino.cc/Code/PIDLibrary +architectures=* From 4157c50a5df25f1ea2332f45033c7a99dcd249e3 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Tue, 21 Mar 2017 15:35:04 -0400 Subject: [PATCH 15/42] Delete test --- lib/Arduino-PID-Library/test | 1 - 1 file changed, 1 deletion(-) delete mode 100644 lib/Arduino-PID-Library/test diff --git a/lib/Arduino-PID-Library/test b/lib/Arduino-PID-Library/test deleted file mode 100644 index 9daeafb9..00000000 --- a/lib/Arduino-PID-Library/test +++ /dev/null @@ -1 +0,0 @@ -test From a10acb1c4acc983db1f49db44b66ae73aff45af8 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Wed, 22 Mar 2017 06:01:23 -0400 Subject: [PATCH 16/42] Adds autopilot functionality --- lib/KBox/src/tasks/IMUTask.h | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/KBox/src/tasks/IMUTask.h b/lib/KBox/src/tasks/IMUTask.h index 0483e367..97f09cc1 100644 --- a/lib/KBox/src/tasks/IMUTask.h +++ b/lib/KBox/src/tasks/IMUTask.h @@ -24,6 +24,7 @@ #include #include "TaskManager.h" #include "KMessage.h" +#include "NMEA2000Task.h" //RIGM added class IMUTask : public Task, public KGenerator { private: From 222e8080e2c0375ec5d2f800d3d33573e552822d Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:05:24 -0400 Subject: [PATCH 17/42] Add RudderSensor task --- src/host/main.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index 62e30a76..b8b89136 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -69,7 +69,7 @@ void setup() { reader2->connectTo(*wifi); IMUTask *imuTask = new IMUTask(); - imuTask->connectTo(*wifi); //RIGM added + imuTask->connectTo(*wifi); imuTask->connectTo(*n2kTask); BarometerTask *baroTask = new BarometerTask(); @@ -88,9 +88,15 @@ void setup() { baroTask->connectTo(*sdcardTask); imuTask->connectTo(*sdcardTask); - AutoPilotTask *autoPilotTask = new AutoPilotTask();//RIGM added + RudderSensorTask *rudderSensorTask = new RudderSensorTask(kbox.getADC()); + rudderSensorTask->connectTo(*wifi); + rudderSensorTask->connectTo(*n2kTask); + + + AutoPilotTask *autoPilotTask = new AutoPilotTask(); autoPilotTask->connectTo(*wifi); autoPilotTask->connectTo(*n2kTask); + rudderSensorTask->connectTo(*autoPilotTask); // Add all the tasks kbox.addTask(new IntervalTask(new RunningLightTask(), 250)); @@ -103,11 +109,14 @@ void setup() { kbox.addTask(wifi); kbox.addTask(sdcardTask); kbox.addTask(autoPilotTask); + kbox.addTask(new IntervalTask(rudderSensorTask, 1000)); + - NavigationPage *navPage = new NavigationPage(); //RIGM added + NavigationPage *navPage = new NavigationPage(); imuTask->connectTo(*navPage); adcTask->connectTo(*navPage); autoPilotTask->connectTo(*navPage); + rudderSensorTask->connectTo(*navPage); kbox.addPage(navPage); navPage->connectTo(*autoPilotTask); From 534ea02ec5406b91c3450288bf5eb02eb86267e5 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:14:53 -0400 Subject: [PATCH 18/42] Reformat IMU message generation --- lib/KBox/src/tasks/IMUTask.cpp | 49 +++++++++++++++++++--------------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/lib/KBox/src/tasks/IMUTask.cpp b/lib/KBox/src/tasks/IMUTask.cpp index b9f1d30f..e77f72e1 100644 --- a/lib/KBox/src/tasks/IMUTask.cpp +++ b/lib/KBox/src/tasks/IMUTask.cpp @@ -24,9 +24,8 @@ #include "IMUTask.h" #include "KBoxDebug.h" -const String kBoxOrientation = "bow"; //Modification to send correct course, pitch, roll data based on where the back of the KBOX is mounted. Options are back of the box to port, to bow, to stbd, to stern - - +enum kboxOrientation {PORT, STBD, BOW, STERN}; +kboxOrientation state = kboxOrientation::BOW; //Modification to send correct course, pitch, roll data based on where the back of the KBOX is mounted. Options are back of the box to port, to bow, to stbd, to stern. Specify as appropriate for your setup void IMUTask::setup() { DEBUG("Initing BNO055"); @@ -54,26 +53,34 @@ void IMUTask::loop() { //IMUMessage m(sysCalib, eulerAngles.x(), /* yaw?*/ 0, eulerAngles.y(), eulerAngles.z()); //original //sendMessage(m); + //order is heading, yaw, pitch, roll + double eheading, epitch, eroll; + switch(state){ + case PORT: + eheading = eulerAngles.x() - 90; + epitch = eulerAngles.y() * -1; + eroll = eulerAngles.z(); + break; + case STBD: + eheading = eulerAngles.x() + 90; + epitch = eulerAngles.y(); + eroll = eulerAngles.z() * -1; + break; + case BOW: + eheading = eulerAngles.x() - 180; + epitch = eulerAngles.z(); + eroll = eulerAngles.y(); + break; + case STERN: + eheading = eulerAngles.x(); + epitch = eulerAngles.z() * -1; + eroll = eulerAngles.y() * -1; + break; + } - if (kBoxOrientation == "port"){ - IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x() - 90), /* yaw?*/ 0, DegToRad(eulerAngles.y() * -1), DegToRad(eulerAngles.z())); - sendMessage(m); - } - - if (kBoxOrientation == "stbd"){ - IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x() + 90), /* yaw?*/ 0, DegToRad(eulerAngles.y()), DegToRad(eulerAngles.z() * -1)); - sendMessage(m); - } - - if (kBoxOrientation == "bow"){ - IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x() - 180), /* yaw?*/ 0, DegToRad(eulerAngles.y()), DegToRad(eulerAngles.z())); - sendMessage(m); - } + IMUMessage m(sysCalib, DegToRad(eheading), /* yaw?*/ 0, DegToRad(epitch), DegToRad(eroll)); + sendMessage(m); - if (kBoxOrientation == "stern"){ - IMUMessage m("heading",sysCalib, DegToRad(eulerAngles.x()), /* yaw?*/ 0, DegToRad(eulerAngles.z() * -1), DegToRad(eulerAngles.y() * -1)); - sendMessage(m); - } } //DEBUG("BNO055 temperature is %i", bno055.getTemp()); From b14cb2247dff90afa03e732f8155d5842dfa888f Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:16:19 -0400 Subject: [PATCH 19/42] Improve autopilot functionality --- lib/KBox/src/tasks/AutoPilotTask.cpp | 123 +++++++++++++-------------- lib/KBox/src/tasks/AutoPilotTask.h | 18 ++-- 2 files changed, 69 insertions(+), 72 deletions(-) diff --git a/lib/KBox/src/tasks/AutoPilotTask.cpp b/lib/KBox/src/tasks/AutoPilotTask.cpp index ae6eebc4..a275423a 100644 --- a/lib/KBox/src/tasks/AutoPilotTask.cpp +++ b/lib/KBox/src/tasks/AutoPilotTask.cpp @@ -23,14 +23,13 @@ THE SOFTWARE. */ -/* AutoPilot functionality can exist at several levels. In this initial iteration, the design - focused on heading the boat on a defined course that is automatically set when AP functionality - is turned on (referred to as "Heading" mode below. Later on we could introduce an additional - mode when a course to a waypoint is provided by a NMEA-connected device. In addition, we have - "Dodge" mode which suspends rudder actions temporarily in order to manually steer the boat around +/* AutoPilot functionality can exist at several levels of complexity. In this initial iteration, the design + focuses on heading the boat on a defined course that is automatically set when AP "Heading" Mode is activated + Later on we could introduce an additional mode when a course to a waypoint is provided by a NMEA-connected device. + "Dodge" mode suspends rudder actions temporarily to manually steer the boat around an object or to manually put it back on course. - The course calculation approach below is a simplified and adapted version of Robert Huitema's FreeboardPLC_v1_2 code. RIGM + The course calculation approach below is a simplified and adapted version of Robert Huitema's excellent FreeboardPLC_v1_2 code. RIGM */ #include "AutoPilotTask.h" @@ -38,23 +37,13 @@ #include "KBoxDebug.h" #include "../drivers/board.h" #include "KMessage.h" - #include #include - double apCurrentHeading = 0; double apTargetHeading = 0; double apTargetRudderPosition = 0; - - -/*pending tuning,following is set in AutoPilotTask.h -#define P_Param 0.5 //proportional param P-gain, the gain determines how much change the OP will make due to a change in error -#define I_Param 0.0 //integral or I-gain, the the reset determines how much to change the OP over time due to the error -#define D_Param 0.0 //derivative or D-gain, the preact determines how much to change the OP due from a change in direction of the error - */ - PID headingPID(&apCurrentHeading, &apTargetRudderPosition, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int)REVERSE); bool isSetup = false; //if false sets initial PID variables on bootup @@ -62,69 +51,79 @@ void AutoPilotTask::processMessage(const KMessage &message) { message.accept(*this); } +void AutoPilotTask::visit(const NAVMessage &nav) { + apCurrentHeading = nav.getCurrentHeading(); + apTargetHeading = nav.getTargetHeading(); + navMode = nav.getApMode(); + navDodgeMode = nav.getApDodgeMode(); + //DEBUG("navMode - %.d",navMode); +} -void AutoPilotTask::visit(const NAVMessage &nav) { //this function now working!! - - apCurrentHeading = nav.getCurrentHeading(); - apTargetHeading = nav.getTargetHeading(); - apRudderSensorPosition = nav.getRudderSensorPosition(); - - // DEBUG("apPilotCurrentHeading - %.0f",apCurrentHeading); +void AutoPilotTask::visit(const RUDMessage &rm) { + apRudderSensorPosition = rm.getRawRudderAngle(); } void AutoPilotTask::loop() { - if (isSetup == false){ //tried a separate "setup" for init but could not get it to work + if (isSetup == false){ //tried a separate "setup" for init but could not get it to work sameLastDirection=true; apTargetRudderPosition = 33; //centred. For programming purposes rudder is assumed to move through 66 degrees lock to lock headingPID.SetMode(AUTOMATIC); - headingPID.SetOutputLimits(0.0, 66.0); //output limits 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right) + headingPID.SetOutputLimits(0.0, MAXRUDDERSWING); //output limits 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right) headingPID.SetSampleTime(250); DEBUG("Init autopilot complete"); isSetup = true; - } + } - headingPID.Compute(); + headingPID.Compute(); - autoPilotOffCourse = apTargetHeading - apCurrentHeading; //variable not currently used for anything + apRudderCommandSent = apTargetRudderPosition; //by default send rudder command to equal target position, then modify it + double testDegs = apTargetRudderPosition - apRudderCommandSent; - autoPilotDeadZone = 5; //pending tuning on water - autoPilotSlack = 5; //pending tuning on water - apRudderCommandSent = apTargetRudderPosition; //by default send rudder command to equal target position, then modify it - double testDegs = apTargetRudderPosition - apRudderCommandSent; - - if (abs(testDegs) > autoPilotDeadZone) { - DEBUG("need to move the rudder"); - - //then we move the rudder. - //is it changing movement direction, we need to compensate for slack - if (sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { - //same direction to stbd, no slack - apRudderCommandSent = apTargetRudderPosition; - }else if (sameLastDirection && apTargetRudderPosition < apRudderCommandSent) { - //changed direction to port, subtract slack - sameLastDirection = false; - apRudderCommandSent = apTargetRudderPosition - autoPilotSlack; - }else if (!sameLastDirection && apTargetRudderPosition < apRudderCommandSent) { - //same direction to port - apRudderCommandSent = apTargetRudderPosition; - }else if (!sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { - //changed direction to stbd, add slack - sameLastDirection = true; - apRudderCommandSent = apTargetRudderPosition + autoPilotSlack; - } + if (abs(testDegs) > AUTOPILOTDEADZONE) { + DEBUG("need to move the rudder"); + //then we move the rudder. + //is it changing movement direction, we need to compensate for slack + if (sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { + //same direction to stbd, no slack + apRudderCommandSent = apTargetRudderPosition; + } else if (sameLastDirection && apTargetRudderPosition < apRudderCommandSent) { + //changed direction to port, subtract slack + sameLastDirection = false; + apRudderCommandSent = apTargetRudderPosition - AUTOPILOTSLACK; + } else if (!sameLastDirection && apTargetRudderPosition < apRudderCommandSent) { + //same direction to port + apRudderCommandSent = apTargetRudderPosition; + } else if (!sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { + //changed direction to stbd, add slack + sameLastDirection = true; + apRudderCommandSent = apTargetRudderPosition + AUTOPILOTSLACK; + } + } + + APMessage m(apTargetRudderPosition, apRudderCommandSent); + sendMessage(m); + + if (navMode == true && navDodgeMode == false){ + + //DEBUG("apRudderCommandSent - %.0f", apRudderCommandSent); + //DEBUG("apRudderSensorPosition - %.0f",apRudderSensorPosition); + + if (apRudderCommandSent > apRudderSensorPosition){ + //send acuator stop command; + DEBUG("moving rudder left"); + //send acuator left command; + } else if (apRudderCommandSent < apRudderSensorPosition){ + //send acuator stop command; + DEBUG("moving rudder right"); + //send acuator left command; + } else { //if equal + //send acuator stop command; + DEBUG("stop rudder motion"); } - //DEBUG("navTargetRudderPosition - %.0f", navTargetRudderPosition); - //DEBUG("navRudderCommandSent2 - %.0f", navRudderCommandSent); - - APMessage m("test", apTargetRudderPosition, apRudderCommandSent); - sendMessage(m); - - - //Add code here to actually move the rudder based on controller system used - + } } diff --git a/lib/KBox/src/tasks/AutoPilotTask.h b/lib/KBox/src/tasks/AutoPilotTask.h index 072d2aa6..b38a7fea 100644 --- a/lib/KBox/src/tasks/AutoPilotTask.h +++ b/lib/KBox/src/tasks/AutoPilotTask.h @@ -28,28 +28,26 @@ #include "KMessage.h" #include -//original values 0.45, 0.08, 0.52 - +//following variables will need to be tuned based on actual boat performance #define P_Param 0.5 //proportional param P-gain, the gain determines how much change the OP will make due to a change in error #define I_Param 0.0 //integral or I-gain, the the reset determines how much to change the OP over time due to the error #define D_Param 0.0 //derivative or D-gain, the preact determines how much to change the OP due from a change in direction of the error - -//extern double autoPilotRudderCommand; +#define AUTOPILOTDEADZONE 0 //to be adjusted based on boat characteristics +#define AUTOPILOTSLACK 0 //to be adjusted based on boat characteristics +#define MAXRUDDERSWING 66.0 //max swing lock to lock class AutoPilotTask : public Task, public KReceiver, public KVisitor, public KGenerator { private: - - double apRudderSensorPosition; + double apRudderSensorPosition; //populated from RudderSensorTask double apRudderCommandSent; - double autoPilotOffCourse; //not currently used - double autoPilotDeadZone; //variable to be tuned based on experience - double autoPilotSlack; //variable to be tuned based on experience bool sameLastDirection; //last rudder movement direction + bool navMode = false; + bool navDodgeMode = false; public: AutoPilotTask() : Task("AutoPilot") {}; void processMessage(const KMessage& message); void visit(const NAVMessage&); + void visit(const RUDMessage&); void loop(); - }; From 32a19b099aa61f702e70397c2b413ea17964f9ca Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:18:07 -0400 Subject: [PATCH 20/42] Suppressed debug message --- lib/KBox/src/tasks/NMEA2000Task.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/lib/KBox/src/tasks/NMEA2000Task.cpp b/lib/KBox/src/tasks/NMEA2000Task.cpp index 8ff6e2a7..9303cd41 100644 --- a/lib/KBox/src/tasks/NMEA2000Task.cpp +++ b/lib/KBox/src/tasks/NMEA2000Task.cpp @@ -78,13 +78,14 @@ void NMEA2000Task::setup() { void NMEA2000Task::sendN2kMessage(const tN2kMsg& msg) { bool result = NMEA2000.SendMsg(msg); - +/* DEBUG("Sending message on n2k bus - pgn=%i prio=%i src=%i dst=%i len=%i result=%s", msg.PGN, msg.Priority, msg.Source, msg.Destination, msg.DataLen, result ? "success":"fail"); - +*/ + char *pcdin = nmea_pcdin_sentence_for_n2kmsg(msg, 0); - DEBUG("TX: %s", pcdin); + //DEBUG("TX: %s", pcdin); free(pcdin); if (result) { @@ -114,7 +115,7 @@ void NMEA2000Task::visit(const IMUMessage &m) { _imuSequence++; } else { - DEBUG("IMU is not calibrated: move the unit around to calibrate it"); //debug message added to warn user if IMU not calibrated + // DEBUG("IMU is not calibrated: move the unit around to calibrate it"); } } From 4ca6c6815f7000097d5baec3bd1656672ce020ed Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:19:07 -0400 Subject: [PATCH 21/42] Added RudderSensor task --- lib/KBox/src/RudderSensorTask.cpp | 56 +++++++++++++++++++++++++++++++ lib/KBox/src/RudderSensorTask.h | 39 +++++++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 lib/KBox/src/RudderSensorTask.cpp create mode 100644 lib/KBox/src/RudderSensorTask.h diff --git a/lib/KBox/src/RudderSensorTask.cpp b/lib/KBox/src/RudderSensorTask.cpp new file mode 100644 index 00000000..c92c035e --- /dev/null +++ b/lib/KBox/src/RudderSensorTask.cpp @@ -0,0 +1,56 @@ +/* + The MIT License + + Copyright (c) 2016 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 "RudderSensorTask.h" +#include "KBoxDebug.h" +#include "../drivers/board.h" +#include +#include + +void RudderSensorTask::loop() { + + int sensorValue = adc.analogRead(bat3_analog, ADC_0); //rudder sensor on bat3_analog input + + //the max reading based on the potentiometer I am using at 3.3v is 680. The rudderFactor is intended such that this maxValue + //translates to 66 which is the assumed rudder swing from lock to lock. The factor should be adjusted based on the actual max value and the + //actual rudder swing. The calculation + + double maxValue = 680; + double midAngle = maxValue / 2; + double rudderFactor = 10.31; + //use this if max sensorValue is when rudder is fully to port + //double rawRudderAngle = sensorValue / rudderFactor; //rawRudderAngle will range from 66 to port, 0 to starboard + //double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor) * -1; //displayRudderAngle will range from -33 to port, +33 to starboard + + //use this if max sensorValue is when rudder is fully to starboard + double rawRudderAngle = ((sensorValue / rudderFactor) - 66) * -1; //rawRudderAngle will range from 66 to port, 0 to starboard + double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor); //displayRudderAngle will range from -33 to port, +33 to starboard + + //DEBUG("sensorValue - %d",sensorValue); + //DEBUG("rawRudderAngle - %.0f",rawRudderAngle); + //DEBUG("rudderDisplayAngle - %.0f",rudderDisplayAngle); + + RUDMessage rm(rawRudderAngle, rudderDisplayAngle); + sendMessage(rm); +} diff --git a/lib/KBox/src/RudderSensorTask.h b/lib/KBox/src/RudderSensorTask.h new file mode 100644 index 00000000..d9804818 --- /dev/null +++ b/lib/KBox/src/RudderSensorTask.h @@ -0,0 +1,39 @@ +/* + The MIT License + + Copyright (c) 2016 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 +#include "TaskManager.h" +#include "KMessage.h" + +class RudderSensorTask : public Task, public KGenerator { + private: + ADC& adc; + double rudderAngle; + + public: + RudderSensorTask(ADC& adc) : Task("RudderSensor"), adc(adc) {}; + void processMessage(const KMessage& message); + void loop(); +}; From 7f13d340124dbd62c02e87ab521553b4a39dd6b0 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:23:50 -0400 Subject: [PATCH 22/42] Format fixes and add RudderSensor message --- lib/KBox/src/KMessage.h | 71 ++++++++++++++-------------- lib/KBox/src/KMessageNMEAVisitor.cpp | 13 ++--- 2 files changed, 40 insertions(+), 44 deletions(-) diff --git a/lib/KBox/src/KMessage.h b/lib/KBox/src/KMessage.h index 8d2c7986..30a50cf4 100644 --- a/lib/KBox/src/KMessage.h +++ b/lib/KBox/src/KMessage.h @@ -37,6 +37,7 @@ class NMEA2000Message; class IMUMessage; class NAVMessage; class APMessage; +class RUDMessage; class KMessage { private: @@ -56,7 +57,7 @@ class KVisitor { virtual void visit(const IMUMessage &) {}; virtual void visit(const NAVMessage &) {}; virtual void visit(const APMessage &) {}; - + virtual void visit(const RUDMessage &) {}; }; class NMEASentence : public KMessage { @@ -151,46 +152,41 @@ class NMEA2000Message: public KMessage { class IMUMessage: public KMessage { private: - String label; //RIGM added, may not need it int calibration; double course, yaw, pitch, roll; public: static const int IMU_CALIBRATED = 3; - IMUMessage(String label, int c, float course, float yaw, float pitch, float roll) : calibration(c), course(course), yaw(yaw), pitch(pitch), roll(roll) + IMUMessage(int c, double course, double yaw, double pitch, double roll) : calibration(c), course(course), yaw(yaw), pitch(pitch), roll(roll) {}; void accept(KVisitor &v) const { v.visit(*this); }; - String getLabel() const { - return label; - }; - int getCalibration() const { return calibration; }; /* - * Heading in Radians: for necessary precision need to use float, not double for radians (RIGM) + * Heading in Radians: course value are compass (magnetic) values not adjusted for variation */ - float getCourse() const { + double getCourse() const { return course; }; /* - * Difference between vessel orientation and course over water in radians. + * Difference between vessel orientation and course over water in radians. Not currently measured, set to 0 in code */ - float getYaw() const { + double getYaw() const { return yaw; }; /* * Pitch in radians. Positive when bow rises. */ - float getPitch() const { + double getPitch() const { return pitch; }; @@ -202,26 +198,20 @@ class IMUMessage: public KMessage { }; }; -class NAVMessage: public KMessage { //RIGM added for autopilot functionality +class NAVMessage: public KMessage { private: - String label; //not sure we need a label bool apMode, apHeadingMode, apWaypointMode, apDodgeMode; - double currentHeading, targetHeading, courseToWaypoint, rudderSensorPosition, targetRudderPosition, rudderCommandSent; + double currentHeading, targetHeading, courseToWaypoint; public: - NAVMessage(String label, bool apMode, bool apHeadingMode, bool apWaypointMode, bool apDodgeMode, double currentHeading, double targetHeading, double courseToWaypoint, double rudderSensorPosition) : - label(label), apMode(apMode), apHeadingMode(apHeadingMode), apWaypointMode(apWaypointMode), apDodgeMode(apDodgeMode), - currentHeading(currentHeading),targetHeading(targetHeading),courseToWaypoint(courseToWaypoint), - rudderSensorPosition(rudderSensorPosition){}; + NAVMessage(bool apMode, bool apHeadingMode, bool apWaypointMode, bool apDodgeMode, double currentHeading,double targetHeading, double courseToWaypoint): + apMode(apMode), apHeadingMode(apHeadingMode), apWaypointMode(apWaypointMode), apDodgeMode(apDodgeMode), + currentHeading(currentHeading),targetHeading(targetHeading),courseToWaypoint(courseToWaypoint){}; void accept(KVisitor &v) const { v.visit(*this); }; - String getLabel() const { - return label; - }; - bool getApMode() const { return apMode; }; @@ -253,30 +243,19 @@ class NAVMessage: public KMessage { //RIGM added for autopilot functionality return courseToWaypoint; }; - double getRudderSensorPosition() const { - return rudderSensorPosition; - }; - - }; class APMessage: public KMessage { //RIGM added for autopilot functionality private: - String label; //not sure we need a label double targetRudderPosition, rudderCommandSent; public: - APMessage(String label, double targetRudderPosition, double rudderCommandSent) : - label(label), targetRudderPosition(targetRudderPosition),rudderCommandSent(rudderCommandSent){}; + APMessage(double targetRudderPosition, double rudderCommandSent) : targetRudderPosition(targetRudderPosition),rudderCommandSent(rudderCommandSent){}; void accept(KVisitor &v) const { v.visit(*this); }; - String getLabel() const { - return label; - }; - double getTargetRudderPosition() const { return targetRudderPosition; }; @@ -287,6 +266,28 @@ class APMessage: public KMessage { //RIGM added for autopilot functionality }; +class RUDMessage: public KMessage { //RIGM added for autopilot functionality +private: + double rawRudderAngle, rudderDisplayAngle; + +public: + RUDMessage(double rawRudderAngle, double rudderDisplayAngle) : rawRudderAngle(rawRudderAngle), rudderDisplayAngle(rudderDisplayAngle){}; + + void accept(KVisitor &v) const { + v.visit(*this); + }; + + double getRawRudderAngle() const { + return rawRudderAngle; + }; + + double getRudderDisplayAngle() const { + return rudderDisplayAngle; + }; + + +}; + class KReceiver { public: diff --git a/lib/KBox/src/KMessageNMEAVisitor.cpp b/lib/KBox/src/KMessageNMEAVisitor.cpp index f709bc05..518c6ee7 100644 --- a/lib/KBox/src/KMessageNMEAVisitor.cpp +++ b/lib/KBox/src/KMessageNMEAVisitor.cpp @@ -29,7 +29,7 @@ #include "tasks/NMEA2000Task.h" #include "KBoxDebug.h" -#define IMU_IN_NMEA2K true; //variable to determine whether IMU data sent out over N2K +#define IMU_IN_NMEA2K true //variable to determine whether IMU data sent out over N2K unsigned int _imuSequence = 1; void KMessageNMEAVisitor::visit(const NMEASentence& s) { @@ -69,8 +69,6 @@ void KMessageNMEAVisitor::visit(const NMEA2000Message &n2km) { free(s); } - - void KMessageNMEAVisitor::visit(const IMUMessage &imu) { #ifdef IMU_IN_NMEA2K //adds IMU to N2K @@ -79,11 +77,11 @@ void KMessageNMEAVisitor::visit(const IMUMessage &imu) { tN2kMsg n2kmessage2; _imuSequence++; //DEBUG("imuSequence: %i course: %.1f yaw: %.1f roll: %.1f pitch: %.1f",_imuSequence,imu.getCourse(),imu.getYaw(),imu.getRoll(),imu.getPitch()); - //values in Radians from IMUTask + //values already in Radians from IMUTask SetN2kPGN127257(n2kmessage, _imuSequence, imu.getYaw(), imu.getPitch(), imu.getRoll()); //sequence consistent with KMessage, NMEA2000 task if(RadToDeg(imu.getCourse()) < 0){ SetN2kPGN127250(n2kmessage2, _imuSequence, (imu.getCourse() - 0.261799), /* Deviation */ N2kDoubleNA, /* Variation */ N2kDoubleNA, N2khr_magnetic); - //for some strange reason, when converting RadToDeg for N2K streaming purposes, values over 180 are off by 15 degrees. This does not affect the heading shown on the display, only NMEA viewed on other devices + //for some strange reason, when converting RadToDeg for N2K streaming purposes, values over 180 are off by 15 degrees. This does not affect the heading shown on the display, only NMEA output viewed on other devices. Needs further exploration to find the cause } else { SetN2kPGN127250(n2kmessage2, _imuSequence, imu.getCourse(), /* Deviation */ N2kDoubleNA, /* Variation */ N2kDoubleNA, N2khr_magnetic); } @@ -93,16 +91,13 @@ void KMessageNMEAVisitor::visit(const IMUMessage &imu) { nmeaContent += String(s) + "\r\n"; free(s); - s = nmea_pcdin_sentence_for_n2kmsg(n2kmessage2, millis() / 1000); //DEBUG("nmeaContent: %s", s); nmeaContent += String(s) + "\r\n"; free(s); +#else -#endif - -#ifndef IMU_IN_NMEA2K NMEASentenceBuilder sb("II", "XDR", 16); sb.setField(1, "A"); sb.setField(2, imu.getYaw(), 1); From 3759b36e1b4c1a14d85cf38ce4f511d9a8189587 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:25:12 -0400 Subject: [PATCH 23/42] Add RudderSensor task --- lib/KBox/src/KBox.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lib/KBox/src/KBox.h b/lib/KBox/src/KBox.h index dd7be2be..1f2379a1 100644 --- a/lib/KBox/src/KBox.h +++ b/lib/KBox/src/KBox.h @@ -43,7 +43,8 @@ #include "tasks/IMUTask.h" #include "tasks/NMEAReaderTask.h" #include "tasks/WiFiTask.h" -#include "tasks/AutoPilotTask.h" //RIGM added +#include "tasks/AutoPilotTask.h" +#include "tasks/RudderSensorTask.h" /* Converters */ #include "converters/VoltageN2kConverter.h" @@ -52,7 +53,7 @@ /* Pages */ #include "pages/BatteryMonitorPage.h" #include "pages/StatsPage.h" -#include "pages/NavigationPage.h" //RIGM added +#include "pages/NavigationPage.h" /* Other dependencies */ #include From af382de59fd51c1b5073c627334f25e3cab0f80e Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:34:30 -0400 Subject: [PATCH 24/42] Add RudderSensor task --- lib/KBox/src/tasks/RudderSensorTask.cpp | 56 +++++++++++++++++++++++++ lib/KBox/src/tasks/RudderSensorTask.h | 39 +++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 lib/KBox/src/tasks/RudderSensorTask.cpp create mode 100644 lib/KBox/src/tasks/RudderSensorTask.h diff --git a/lib/KBox/src/tasks/RudderSensorTask.cpp b/lib/KBox/src/tasks/RudderSensorTask.cpp new file mode 100644 index 00000000..c92c035e --- /dev/null +++ b/lib/KBox/src/tasks/RudderSensorTask.cpp @@ -0,0 +1,56 @@ +/* + The MIT License + + Copyright (c) 2016 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 "RudderSensorTask.h" +#include "KBoxDebug.h" +#include "../drivers/board.h" +#include +#include + +void RudderSensorTask::loop() { + + int sensorValue = adc.analogRead(bat3_analog, ADC_0); //rudder sensor on bat3_analog input + + //the max reading based on the potentiometer I am using at 3.3v is 680. The rudderFactor is intended such that this maxValue + //translates to 66 which is the assumed rudder swing from lock to lock. The factor should be adjusted based on the actual max value and the + //actual rudder swing. The calculation + + double maxValue = 680; + double midAngle = maxValue / 2; + double rudderFactor = 10.31; + //use this if max sensorValue is when rudder is fully to port + //double rawRudderAngle = sensorValue / rudderFactor; //rawRudderAngle will range from 66 to port, 0 to starboard + //double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor) * -1; //displayRudderAngle will range from -33 to port, +33 to starboard + + //use this if max sensorValue is when rudder is fully to starboard + double rawRudderAngle = ((sensorValue / rudderFactor) - 66) * -1; //rawRudderAngle will range from 66 to port, 0 to starboard + double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor); //displayRudderAngle will range from -33 to port, +33 to starboard + + //DEBUG("sensorValue - %d",sensorValue); + //DEBUG("rawRudderAngle - %.0f",rawRudderAngle); + //DEBUG("rudderDisplayAngle - %.0f",rudderDisplayAngle); + + RUDMessage rm(rawRudderAngle, rudderDisplayAngle); + sendMessage(rm); +} diff --git a/lib/KBox/src/tasks/RudderSensorTask.h b/lib/KBox/src/tasks/RudderSensorTask.h new file mode 100644 index 00000000..d9804818 --- /dev/null +++ b/lib/KBox/src/tasks/RudderSensorTask.h @@ -0,0 +1,39 @@ +/* + The MIT License + + Copyright (c) 2016 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 +#include "TaskManager.h" +#include "KMessage.h" + +class RudderSensorTask : public Task, public KGenerator { + private: + ADC& adc; + double rudderAngle; + + public: + RudderSensorTask(ADC& adc) : Task("RudderSensor"), adc(adc) {}; + void processMessage(const KMessage& message); + void loop(); +}; From f13b9f4b77d9f55b6cc0f110b9174ca15d50d1de Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:43:14 -0400 Subject: [PATCH 25/42] Wrong location --- lib/KBox/src/RudderSensorTask.cpp | 56 ------------------------------- 1 file changed, 56 deletions(-) delete mode 100644 lib/KBox/src/RudderSensorTask.cpp diff --git a/lib/KBox/src/RudderSensorTask.cpp b/lib/KBox/src/RudderSensorTask.cpp deleted file mode 100644 index c92c035e..00000000 --- a/lib/KBox/src/RudderSensorTask.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - The MIT License - - Copyright (c) 2016 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 "RudderSensorTask.h" -#include "KBoxDebug.h" -#include "../drivers/board.h" -#include -#include - -void RudderSensorTask::loop() { - - int sensorValue = adc.analogRead(bat3_analog, ADC_0); //rudder sensor on bat3_analog input - - //the max reading based on the potentiometer I am using at 3.3v is 680. The rudderFactor is intended such that this maxValue - //translates to 66 which is the assumed rudder swing from lock to lock. The factor should be adjusted based on the actual max value and the - //actual rudder swing. The calculation - - double maxValue = 680; - double midAngle = maxValue / 2; - double rudderFactor = 10.31; - //use this if max sensorValue is when rudder is fully to port - //double rawRudderAngle = sensorValue / rudderFactor; //rawRudderAngle will range from 66 to port, 0 to starboard - //double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor) * -1; //displayRudderAngle will range from -33 to port, +33 to starboard - - //use this if max sensorValue is when rudder is fully to starboard - double rawRudderAngle = ((sensorValue / rudderFactor) - 66) * -1; //rawRudderAngle will range from 66 to port, 0 to starboard - double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor); //displayRudderAngle will range from -33 to port, +33 to starboard - - //DEBUG("sensorValue - %d",sensorValue); - //DEBUG("rawRudderAngle - %.0f",rawRudderAngle); - //DEBUG("rudderDisplayAngle - %.0f",rudderDisplayAngle); - - RUDMessage rm(rawRudderAngle, rudderDisplayAngle); - sendMessage(rm); -} From 4a7b4ce7ad379ccb3809f65e6264614c870555b1 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:43:32 -0400 Subject: [PATCH 26/42] File in wrong location --- lib/KBox/src/RudderSensorTask.h | 39 --------------------------------- 1 file changed, 39 deletions(-) delete mode 100644 lib/KBox/src/RudderSensorTask.h diff --git a/lib/KBox/src/RudderSensorTask.h b/lib/KBox/src/RudderSensorTask.h deleted file mode 100644 index d9804818..00000000 --- a/lib/KBox/src/RudderSensorTask.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - The MIT License - - Copyright (c) 2016 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 -#include "TaskManager.h" -#include "KMessage.h" - -class RudderSensorTask : public Task, public KGenerator { - private: - ADC& adc; - double rudderAngle; - - public: - RudderSensorTask(ADC& adc) : Task("RudderSensor"), adc(adc) {}; - void processMessage(const KMessage& message); - void loop(); -}; From 300273899a2c8b47f3694311f71d9d5bad39c886 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 23 Mar 2017 18:44:22 -0400 Subject: [PATCH 27/42] Update Navigation page --- lib/KBox/src/pages/NavigationPage.cpp | 151 +++++++++++--------------- lib/KBox/src/pages/NavigationPage.h | 44 +++++--- 2 files changed, 90 insertions(+), 105 deletions(-) diff --git a/lib/KBox/src/pages/NavigationPage.cpp b/lib/KBox/src/pages/NavigationPage.cpp index 8813ce10..d4bfab6a 100644 --- a/lib/KBox/src/pages/NavigationPage.cpp +++ b/lib/KBox/src/pages/NavigationPage.cpp @@ -36,37 +36,8 @@ #include "KBoxDebug.h" -bool apMode = false; -bool apDodgeMode = false; -bool apHeadingMode = false; -bool apWaypointMode = false;//reserved for future use -String apModeString = "APMode: Off"; // Modes: "Off", "Heading" == follow set heading, "Waypoint" = GoTo provided Waypoint, "Dodge" -String apWaypointString = "045 20.2N, 081 50.3W";//placeholder for future use -double courseToWaypoint = 0; - -int navEncoderClicks = 0; -int encoderClicks = 0; - -//String navRudderMovementIndicator = "|"; - -double initialTargetHeading = 0; -double navCurrentDisplayHeading = 0; -double navTargetDisplayHeading = 0; -double rudderCommandForDisplay = 0; -double rudderAngleForDisplay = 0; -double rawHeading = 0; -double rawRudderAngle = 0; -double rawTargetHeading = 0; -bool isTargetSet = false; - -#define P_Param 0.5 //proportional param P-gain, the gain determines how much change the OP will make due to a change in error -#define I_Param 0.0 //integral or I-gain, the the reset determines how much to change the OP over time due to the error -#define D_Param 0.0 //derivative or D-gain, the preact determines how much to change the OP due from a change in direction of the error - - NavigationPage::NavigationPage() { static const int col1 = 5; - //static const int col2 = 200; static const int col2 = 160; static const int row1 = 20; static const int row2 = 40; @@ -74,7 +45,6 @@ NavigationPage::NavigationPage() { static const int row4 = 152; static const int row5 = 182; - addLayer(new TextLayer(Point(col1, row2), Size(20, 20), "Heading", ColorWhite, ColorBlack, FontDefault)); addLayer(new TextLayer(Point(col2, row2), Size(20, 20), "Target Heading", ColorWhite, ColorBlack, FontDefault)); addLayer(new TextLayer(Point(col1, row4), Size(20, 20), "Rudder Position", ColorWhite, ColorBlack, FontDefault)); @@ -148,18 +118,20 @@ bool NavigationPage::processEvent(const ButtonEvent &be) { } } else if (this->buttonPressedTimer > 5000) { // Long click - start and stop the navMode here DEBUG("long click"); - if (apMode == false){ - apMode = true; - apHeadingMode = true; - apModeString = "APMode: Heading ";//extra spaces needed to overwrite old string if longer - apModeDisplay->setText(apModeString); - apModeDisplay->setColor(ColorGreen); - } else { - apMode = false; - apDodgeMode = false; - apModeString = "APMode: Off "; - apModeDisplay->setText(apModeString); - apModeDisplay->setColor(ColorBlue); + if (imuCalibrated == true){ + if (apMode == false){ + apMode = true; + apHeadingMode = true; + apModeString = "APMode: Heading ";//extra spaces needed to overwrite old string if longer + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorGreen); + } else { + apMode = false; + apDodgeMode = false; + apModeString = "APMode: Off "; + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorBlue); + } } return true; } @@ -171,23 +143,17 @@ bool NavigationPage::processEvent(const ButtonEvent &be) { } bool NavigationPage::processEvent(const EncoderEvent &ee) { - encoderClicks = ee.rotation; - navEncoderClicks = navEncoderClicks + encoderClicks; - encoderClicks = 0; - - //DEBUG("encoderClicks - %d", encoderClicks); - //DEBUG("navEncoderClicks - %d", navEncoderClicks); + encoderClicks = ee.rotation; + navEncoderClicks = navEncoderClicks + encoderClicks; + encoderClicks = 0; return true; } - String NavigationPage::formatMeasurement(float measure, const char *unit) { // extra spaces at the end needed to clear previous value completely // (we use a non-fixed width font) char s[10]; - //snprintf(s, sizeof(s), "%.1f %s ", measure, unit); - snprintf(s, sizeof(s), "%.0f %s ", measure, unit); - + snprintf(s, sizeof(s), "%.0f %s ", measure, unit); //do not display decimals for headings return String(s); } @@ -196,27 +162,30 @@ void NavigationPage::processMessage(const KMessage &message) { message.accept(*this); } -void NavigationPage::visit(const VoltageMeasurement &vm) { - if (vm.getLabel() == "bat3") { - rawRudderAngle = vm.getVoltage(); - //provision for tuning - navRudderSensorPosition = rawRudderAngle; - } -} - - -void NavigationPage::visit(const APMessage &ap) { //this function never called - +void NavigationPage::visit(const APMessage &ap) { navTargetRudderPosition = ap.getTargetRudderPosition(); - navTargetRudderPosition = ap.getRudderCommandSent(); - //DEBUG("testRudderCommandPosition - %.0f",navTargetRudderPosition); - //DEBUG("testRudderCommandSent - %.0f",navRudderCommandSent); - - + navRudderCommandSent = ap.getRudderCommandSent(); +} +void NavigationPage::visit(const RUDMessage &rm) { + navRudderSensorPosition = rm.getRudderDisplayAngle(); } void NavigationPage::visit(const IMUMessage &imu) { + if (imu.getCalibration() == 3){ + imuCalibrated = true; + } + if (imuCalibrated){ + if (!apMode){ + apModeString = "APMode: Off "; + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorGreen); + } + } else { + apModeString = "Calibrating "; + apModeDisplay->setText(apModeString); + apModeDisplay->setColor(ColorRed); + } rawHeading = RadToDeg(imu.getCourse()); if (rawHeading < 0){ @@ -244,34 +213,40 @@ void NavigationPage::visit(const IMUMessage &imu) { } targetHeadingDisplay->setText(formatMeasurement(navTargetDisplayHeading, "")); targetHeadingDisplay->setColor(colorForCourse(navTargetDisplayHeading)); + + //following code recalculates navCurrentHeading based on "offcourse" value to avoid non-linear heading numbers across 180 degrees + navOffCourse = navCurrentDisplayHeading - navTargetDisplayHeading; + navOffCourse += (navOffCourse > 180) ? -360 : (navOffCourse < -180) ? 360 : 0; + navCurrentHeading = navTargetHeading + navOffCourse; - if (navTargetRudderPosition == 33){ // 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right rudderCommandForDisplay = 0; } else if (navTargetRudderPosition < 33){ //32 sb 1 so display 33 - command rudderCommandForDisplay = 33 - navTargetRudderPosition; - } else if (navTargetRudderPosition > 33){ //34 sb -1 so display 66 - + } else if (navTargetRudderPosition > 33){ //34 sb -1 so display command - 32 as negative number rudderCommandForDisplay = (navTargetRudderPosition - 32) * -1 ; //rounding anomaly to avoid displaying -0 } rudderCommandDisplay->setText(formatMeasurement(rudderCommandForDisplay, "")); rudderCommandDisplay->setColor(colorForRudder(rudderCommandForDisplay)); - - //this should ultimately be done with the rudderCommandSent variable, not the rudderCommandForDisplay - if (rudderCommandForDisplay == navRudderSensorPosition){ //zero angle - rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "|")); - } - if (rudderCommandForDisplay >= 0){ - if (navRudderSensorPosition < rudderCommandForDisplay){ - rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ">")); + + if (!apDodgeMode){ //if not in dodgeMode show rudder movement indicator + if (rudderCommandForDisplay > 0){ + if (navRudderSensorPosition < rudderCommandForDisplay){ + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ">")); + } else { + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "<")); + } + } else if (rudderCommandForDisplay < 0) { + if (navRudderSensorPosition > rudderCommandForDisplay){ + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "<")); + } else { + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ">")); + } } else { - rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "<")); + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "|")); } } else { - if (navRudderSensorPosition > rudderCommandForDisplay){ - rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "<")); - } else { - rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ">")); - } + rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "")); } rudderPositionDisplay->setColor(colorForRudder(navRudderSensorPosition)); @@ -280,7 +255,9 @@ void NavigationPage::visit(const IMUMessage &imu) { //reserved for future use when waypoint functionality added } else { //AP mode is false - isTargetSet = false; + isTargetSet = false; //reset variables + navEncoderClicks = 0; + targetHeadingDisplay->setText("---- "); targetHeadingDisplay->setColor(ColorWhite); @@ -290,10 +267,8 @@ void NavigationPage::visit(const IMUMessage &imu) { rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, "")); rudderPositionDisplay->setColor(colorForRudder(navRudderSensorPosition)); } - - NAVMessage m("test", apMode, apHeadingMode, apWaypointMode, apDodgeMode, navCurrentHeading, navTargetHeading, courseToWaypoint, rawRudderAngle); + NAVMessage m(apMode, apHeadingMode, apWaypointMode, apDodgeMode, navCurrentHeading, navTargetHeading, courseToWaypoint); sendMessage(m); - } diff --git a/lib/KBox/src/pages/NavigationPage.h b/lib/KBox/src/pages/NavigationPage.h index 923d3e70..8d37be57 100644 --- a/lib/KBox/src/pages/NavigationPage.h +++ b/lib/KBox/src/pages/NavigationPage.h @@ -28,16 +28,6 @@ #include "KMessage.h" #include "ui/TextLayer.h" -/* -extern double navCurrentHeading; -extern double navTargetHeading; -extern double navTargetRudderPosition; -extern double navRudderSensorPosition; -extern double navRudderCommandSent; -extern String navRudderMovementIndicator; - */ - - class NavigationPage : public Page, public KReceiver, public KVisitor, public KGenerator { private: TextLayer *apModeDisplay, *waypointDisplay, *headingDisplay, *targetHeadingDisplay, *rudderPositionDisplay, *rudderCommandDisplay; @@ -49,19 +39,39 @@ class NavigationPage : public Page, public KReceiver, public KVisitor, public KG bool buttonPressed = false; elapsedMillis buttonPressedTimer; - double navCurrentHeading; - double navTargetHeading; - double navRudderSensorPosition; - double navTargetRudderPosition; - double navRudderCommandSent; + bool imuCalibrated = false; + bool apMode = false; + bool apDodgeMode = false; + bool apHeadingMode = false; + bool apWaypointMode = false; + String apModeString = "APMode: Off"; + + int navEncoderClicks = 0; + int encoderClicks = 0; + + String apWaypointString;//for future use + double courseToWaypoint;//for future use + + double rawHeading = 0; + double navCurrentHeading = 0; + double navTargetHeading = 0; + bool isTargetSet = false; + double navRudderSensorPosition = 0; + double navTargetRudderPosition = 0; + double navRudderCommandSent = 0; + double initialTargetHeading = 0; + double navCurrentDisplayHeading = 0; + double navTargetDisplayHeading = 0; + double rudderCommandForDisplay = 0; + double rudderAngleForDisplay = 0; + double navOffCourse = 0; public: NavigationPage(); - void processMessage(const KMessage& message); void visit(const IMUMessage&); void visit(const APMessage&); - void visit(const VoltageMeasurement&); + void visit(const RUDMessage&); bool processEvent(const ButtonEvent &be); bool processEvent(const EncoderEvent &ee); }; From dd59141f7c53c2ea3f508455627d37a94d854506 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 10:55:10 -0400 Subject: [PATCH 28/42] Removed RudderSensor task --- src/host/main.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/src/host/main.cpp b/src/host/main.cpp index b8b89136..f9818dc7 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -88,15 +88,10 @@ void setup() { baroTask->connectTo(*sdcardTask); imuTask->connectTo(*sdcardTask); - RudderSensorTask *rudderSensorTask = new RudderSensorTask(kbox.getADC()); - rudderSensorTask->connectTo(*wifi); - rudderSensorTask->connectTo(*n2kTask); - - AutoPilotTask *autoPilotTask = new AutoPilotTask(); autoPilotTask->connectTo(*wifi); autoPilotTask->connectTo(*n2kTask); - rudderSensorTask->connectTo(*autoPilotTask); + adcTask->connectTo(*autoPilotTask); // Add all the tasks kbox.addTask(new IntervalTask(new RunningLightTask(), 250)); @@ -109,14 +104,11 @@ void setup() { kbox.addTask(wifi); kbox.addTask(sdcardTask); kbox.addTask(autoPilotTask); - kbox.addTask(new IntervalTask(rudderSensorTask, 1000)); - - + NavigationPage *navPage = new NavigationPage(); imuTask->connectTo(*navPage); adcTask->connectTo(*navPage); autoPilotTask->connectTo(*navPage); - rudderSensorTask->connectTo(*navPage); kbox.addPage(navPage); navPage->connectTo(*autoPilotTask); From d11ed629129a0f6cf1a4feb033d4f3da895b9ced Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:23:45 -0400 Subject: [PATCH 29/42] Removed RudderSensorTask (relocated to ADCTask) --- lib/KBox/src/KBox.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/lib/KBox/src/KBox.h b/lib/KBox/src/KBox.h index 1f2379a1..53b88ed3 100644 --- a/lib/KBox/src/KBox.h +++ b/lib/KBox/src/KBox.h @@ -44,16 +44,18 @@ #include "tasks/NMEAReaderTask.h" #include "tasks/WiFiTask.h" #include "tasks/AutoPilotTask.h" -#include "tasks/RudderSensorTask.h" +#include "tasks/MFDTask.h" /* Converters */ #include "converters/VoltageN2kConverter.h" #include "converters/BarometerN2kConverter.h" /* Pages */ +#include "pages/SplashPage.h" +#include "pages/MFDPage.h" +#include "pages/NavigationPage.h" #include "pages/BatteryMonitorPage.h" #include "pages/StatsPage.h" -#include "pages/NavigationPage.h" /* Other dependencies */ #include From 5fce3c386908a05c6e8db72c5a2ff0f5a4448512 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:27:27 -0400 Subject: [PATCH 30/42] Reload correct file --- lib/KBox/src/KBox.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/lib/KBox/src/KBox.h b/lib/KBox/src/KBox.h index 53b88ed3..35219c81 100644 --- a/lib/KBox/src/KBox.h +++ b/lib/KBox/src/KBox.h @@ -44,18 +44,15 @@ #include "tasks/NMEAReaderTask.h" #include "tasks/WiFiTask.h" #include "tasks/AutoPilotTask.h" -#include "tasks/MFDTask.h" /* Converters */ #include "converters/VoltageN2kConverter.h" #include "converters/BarometerN2kConverter.h" /* Pages */ -#include "pages/SplashPage.h" -#include "pages/MFDPage.h" -#include "pages/NavigationPage.h" #include "pages/BatteryMonitorPage.h" #include "pages/StatsPage.h" +#include "pages/NavigationPage.h" /* Other dependencies */ #include From b6935c84b1ce45e8e66a818ca25f47d0677f8b11 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:28:34 -0400 Subject: [PATCH 31/42] Various minor edits --- lib/KBox/src/KMessage.h | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/lib/KBox/src/KMessage.h b/lib/KBox/src/KMessage.h index 30a50cf4..beb06697 100644 --- a/lib/KBox/src/KMessage.h +++ b/lib/KBox/src/KMessage.h @@ -47,7 +47,6 @@ class KMessage { virtual void accept(KVisitor& v) const {}; }; - class KVisitor { public: virtual void visit(const NMEASentence &) {}; @@ -158,8 +157,7 @@ class IMUMessage: public KMessage { public: static const int IMU_CALIBRATED = 3; - IMUMessage(int c, double course, double yaw, double pitch, double roll) : calibration(c), course(course), yaw(yaw), pitch(pitch), roll(roll) - {}; + IMUMessage(int c, double course, double yaw, double pitch, double roll) : calibration(c), course(course), yaw(yaw), pitch(pitch), roll(roll) {}; void accept(KVisitor &v) const { v.visit(*this); @@ -229,7 +227,7 @@ class NAVMessage: public KMessage { }; /* - * Courses in Degrees, therefore using doubles: + * Headings in Degrees for messages passed back and forth between the Autopilot task and the Nav page */ double getCurrentHeading() const { return currentHeading; @@ -242,10 +240,9 @@ class NAVMessage: public KMessage { double getCourseToWaypoint() const { return courseToWaypoint; }; - }; -class APMessage: public KMessage { //RIGM added for autopilot functionality +class APMessage: public KMessage { private: double targetRudderPosition, rudderCommandSent; @@ -266,7 +263,7 @@ class APMessage: public KMessage { //RIGM added for autopilot functionality }; -class RUDMessage: public KMessage { //RIGM added for autopilot functionality +class RUDMessage: public KMessage { private: double rawRudderAngle, rudderDisplayAngle; @@ -284,8 +281,6 @@ class RUDMessage: public KMessage { //RIGM added for autopilot functionality double getRudderDisplayAngle() const { return rudderDisplayAngle; }; - - }; From 15585735afa7dfcc031788e6b96048f0e7c335c7 Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:29:55 -0400 Subject: [PATCH 32/42] Move IMUSequence inside function --- lib/KBox/src/KMessageNMEAVisitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/KBox/src/KMessageNMEAVisitor.cpp b/lib/KBox/src/KMessageNMEAVisitor.cpp index 518c6ee7..afaca3b9 100644 --- a/lib/KBox/src/KMessageNMEAVisitor.cpp +++ b/lib/KBox/src/KMessageNMEAVisitor.cpp @@ -30,7 +30,6 @@ #include "KBoxDebug.h" #define IMU_IN_NMEA2K true //variable to determine whether IMU data sent out over N2K -unsigned int _imuSequence = 1; void KMessageNMEAVisitor::visit(const NMEASentence& s) { nmeaContent += s.getSentence() + "\r\n"; @@ -70,6 +69,7 @@ void KMessageNMEAVisitor::visit(const NMEA2000Message &n2km) { } void KMessageNMEAVisitor::visit(const IMUMessage &imu) { + unsigned int _imuSequence = 1; #ifdef IMU_IN_NMEA2K //adds IMU to N2K From 839101eef2aa9931bbac04f4c13471a7439304ad Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:33:39 -0400 Subject: [PATCH 33/42] Add RudderSensor functionality to ADCTask --- lib/KBox/src/tasks/ADCTask.cpp | 60 +++++++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 16 deletions(-) diff --git a/lib/KBox/src/tasks/ADCTask.cpp b/lib/KBox/src/tasks/ADCTask.cpp index b4430ad2..076f3a3c 100644 --- a/lib/KBox/src/tasks/ADCTask.cpp +++ b/lib/KBox/src/tasks/ADCTask.cpp @@ -21,6 +21,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + #include "ADCTask.h" #include "KBoxDebug.h" #include "../drivers/board.h" @@ -28,34 +29,61 @@ #include #include +bool useRudderSensor = true; //varaible used to determine whether bat3 input used for RudderSensor or battery input + void ADCTask::loop() { int supply_adc = adc.analogRead(supply_analog, ADC_0); int bat1_adc = adc.analogRead(bat1_analog, ADC_0); int bat2_adc = adc.analogRead(bat2_analog, ADC_0); - int bat3_adc = adc.analogRead(bat3_analog, ADC_0); + //int bat3_adc = adc.analogRead(bat3_analog, ADC_0); - //permits rudder sensor on bat3 input. Eventually should move rudder sensor calcs to a separate task - double maxAngle = 680; //this is the max reading based on the potentiometer I am using at 3.3v. - double midAngle = maxAngle / 2; - int rudderFactor = 10; //680 / 10 is close to the rudder angle range currently defined as 0-66. Factor could require a bit of tuning based on the actual installation in the boat - double rudderAngle = (bat3_adc - midAngle) / rudderFactor; - supply = supply_adc * analog_max_voltage / adc.getMaxValue(); bat1 = bat1_adc * analog_max_voltage / adc.getMaxValue(); bat2 = bat2_adc * analog_max_voltage / adc.getMaxValue(); - //bat3 = bat3_adc * analog_max_voltage / adc.getMaxValue(); - bat3 = rudderAngle; - - //DEBUG("ADC - Supply: %sV Bat1: %sV Bat2: %sV Bat3: %sV", - //String(supply, 2).c_str(), String(bat1, 2).c_str(), String(bat2, 2).c_str(), String(bat3, 2).c_str()); - + VoltageMeasurement m1(0, "house", bat1); VoltageMeasurement m2(1, "starter", bat2); VoltageMeasurement mSupply(3, "supply", supply); - VoltageMeasurement m3(4, "bat3", bat3); - + sendMessage(m1); sendMessage(m2); - sendMessage(m3); sendMessage(mSupply); + + if (useRudderSensor == true){ + DEBUG("useRudderSensor is true"); + + int sensorValue = adc.analogRead(bat3_analog, ADC_0); //rudder sensor on bat3_analog input + + //the max reading based on the potentiometer I am using at 3.3v is 680. The rudderFactor is intended such that this maxValue + //translates to 66 which is the assumed rudder swing from lock to lock. The factor should be adjusted based on the actual max value and the + //actual rudder swing. The calculation + + double maxValue = 680; + double midAngle = maxValue / 2; + double rudderFactor = 10.31; + //use this if max sensorValue is when rudder is fully to port + //double rawRudderAngle = sensorValue / rudderFactor; //rawRudderAngle will range from 66 to port, 0 to starboard + //double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor) * -1; //displayRudderAngle will range from -33 to port, +33 to starboard + + //use this if max sensorValue is when rudder is fully to starboard + double rawRudderAngle = ((sensorValue / rudderFactor) - 66) * -1; //rawRudderAngle will range from 66 to port, 0 to starboard + double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor); //displayRudderAngle will range from -33 to port, +33 to starboard + + //DEBUG("sensorValue - %d",sensorValue); + //DEBUG("rawRudderAngle - %.0f",rawRudderAngle); + //DEBUG("rudderDisplayAngle - %.0f",rudderDisplayAngle); + + RUDMessage rm(rawRudderAngle, rudderDisplayAngle); + sendMessage(rm); + + } else { + + int bat3_adc = adc.analogRead(bat3_analog, ADC_0); + VoltageMeasurement m3(4, "bat3", bat3); + sendMessage(m3); + } + + //DEBUG("ADC - Supply: %sV Bat1: %sV Bat2: %sV Bat3: %sV", + //String(supply, 2).c_str(), String(bat1, 2).c_str(), String(bat2, 2).c_str(), String(bat3, 2).c_str()); + } From 0cb29fd9520ca8bee91540a6e062e09e8469f4ed Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:34:36 -0400 Subject: [PATCH 34/42] Delete RudderSensorTask.cpp --- lib/KBox/src/tasks/RudderSensorTask.cpp | 56 ------------------------- 1 file changed, 56 deletions(-) delete mode 100644 lib/KBox/src/tasks/RudderSensorTask.cpp diff --git a/lib/KBox/src/tasks/RudderSensorTask.cpp b/lib/KBox/src/tasks/RudderSensorTask.cpp deleted file mode 100644 index c92c035e..00000000 --- a/lib/KBox/src/tasks/RudderSensorTask.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - The MIT License - - Copyright (c) 2016 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 "RudderSensorTask.h" -#include "KBoxDebug.h" -#include "../drivers/board.h" -#include -#include - -void RudderSensorTask::loop() { - - int sensorValue = adc.analogRead(bat3_analog, ADC_0); //rudder sensor on bat3_analog input - - //the max reading based on the potentiometer I am using at 3.3v is 680. The rudderFactor is intended such that this maxValue - //translates to 66 which is the assumed rudder swing from lock to lock. The factor should be adjusted based on the actual max value and the - //actual rudder swing. The calculation - - double maxValue = 680; - double midAngle = maxValue / 2; - double rudderFactor = 10.31; - //use this if max sensorValue is when rudder is fully to port - //double rawRudderAngle = sensorValue / rudderFactor; //rawRudderAngle will range from 66 to port, 0 to starboard - //double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor) * -1; //displayRudderAngle will range from -33 to port, +33 to starboard - - //use this if max sensorValue is when rudder is fully to starboard - double rawRudderAngle = ((sensorValue / rudderFactor) - 66) * -1; //rawRudderAngle will range from 66 to port, 0 to starboard - double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor); //displayRudderAngle will range from -33 to port, +33 to starboard - - //DEBUG("sensorValue - %d",sensorValue); - //DEBUG("rawRudderAngle - %.0f",rawRudderAngle); - //DEBUG("rudderDisplayAngle - %.0f",rudderDisplayAngle); - - RUDMessage rm(rawRudderAngle, rudderDisplayAngle); - sendMessage(rm); -} From 11e8794684644c78ea65b0be6c906d698f32615d Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:34:47 -0400 Subject: [PATCH 35/42] Delete RudderSensorTask.h --- lib/KBox/src/tasks/RudderSensorTask.h | 39 --------------------------- 1 file changed, 39 deletions(-) delete mode 100644 lib/KBox/src/tasks/RudderSensorTask.h diff --git a/lib/KBox/src/tasks/RudderSensorTask.h b/lib/KBox/src/tasks/RudderSensorTask.h deleted file mode 100644 index d9804818..00000000 --- a/lib/KBox/src/tasks/RudderSensorTask.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - The MIT License - - Copyright (c) 2016 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 -#include "TaskManager.h" -#include "KMessage.h" - -class RudderSensorTask : public Task, public KGenerator { - private: - ADC& adc; - double rudderAngle; - - public: - RudderSensorTask(ADC& adc) : Task("RudderSensor"), adc(adc) {}; - void processMessage(const KMessage& message); - void loop(); -}; From d55348d61ca482e15fb3d312c7700690f65ebbdd Mon Sep 17 00:00:00 2001 From: matrixlinks Date: Thu, 30 Mar 2017 11:39:16 -0400 Subject: [PATCH 36/42] Use MAXRUDDERSWING to set default rudder position --- lib/KBox/src/tasks/AutoPilotTask.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lib/KBox/src/tasks/AutoPilotTask.cpp b/lib/KBox/src/tasks/AutoPilotTask.cpp index a275423a..cbc37029 100644 --- a/lib/KBox/src/tasks/AutoPilotTask.cpp +++ b/lib/KBox/src/tasks/AutoPilotTask.cpp @@ -47,6 +47,8 @@ double apTargetRudderPosition = 0; PID headingPID(&apCurrentHeading, &apTargetRudderPosition, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int)REVERSE); bool isSetup = false; //if false sets initial PID variables on bootup +//AutoPilotTask::AutoPilotTask() : apCurrentHeading(0), apTargetHeading(0), apTargetRudderPosition(0), headingPID(&apCurrentHeading, &apTargetRudderPostion, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int) REVERSE){}; + void AutoPilotTask::processMessage(const KMessage &message) { message.accept(*this); } @@ -67,7 +69,7 @@ void AutoPilotTask::loop() { if (isSetup == false){ //tried a separate "setup" for init but could not get it to work sameLastDirection=true; - apTargetRudderPosition = 33; //centred. For programming purposes rudder is assumed to move through 66 degrees lock to lock + apTargetRudderPosition = MAXRUDDERSWING / 2; //centred. For programming purposes rudder is assumed to move through 66 degrees lock to lock headingPID.SetMode(AUTOMATIC); headingPID.SetOutputLimits(0.0, MAXRUDDERSWING); //output limits 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right) headingPID.SetSampleTime(250); From 52a7115aff6e285b392588a2b355e8dae3a55590 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Sat, 1 Apr 2017 00:09:13 -0700 Subject: [PATCH 37/42] Make ADCTask and IMUTask use const class variables for configuration You can configure the orientation of KBox (IMU) and whether to use bat3 as a rudder input (adctask). --- lib/KBox/src/tasks/ADCTask.cpp | 41 ++++++++---------- lib/KBox/src/tasks/ADCTask.h | 3 ++ lib/KBox/src/tasks/IMUTask.cpp | 78 ++++++++++------------------------ lib/KBox/src/tasks/IMUTask.h | 5 +++ 4 files changed, 48 insertions(+), 79 deletions(-) diff --git a/lib/KBox/src/tasks/ADCTask.cpp b/lib/KBox/src/tasks/ADCTask.cpp index 076f3a3c..fbc10491 100644 --- a/lib/KBox/src/tasks/ADCTask.cpp +++ b/lib/KBox/src/tasks/ADCTask.cpp @@ -29,61 +29,54 @@ #include #include -bool useRudderSensor = true; //varaible used to determine whether bat3 input used for RudderSensor or battery input - void ADCTask::loop() { int supply_adc = adc.analogRead(supply_analog, ADC_0); int bat1_adc = adc.analogRead(bat1_analog, ADC_0); int bat2_adc = adc.analogRead(bat2_analog, ADC_0); //int bat3_adc = adc.analogRead(bat3_analog, ADC_0); - + supply = supply_adc * analog_max_voltage / adc.getMaxValue(); bat1 = bat1_adc * analog_max_voltage / adc.getMaxValue(); bat2 = bat2_adc * analog_max_voltage / adc.getMaxValue(); - + VoltageMeasurement m1(0, "house", bat1); VoltageMeasurement m2(1, "starter", bat2); VoltageMeasurement mSupply(3, "supply", supply); - + sendMessage(m1); sendMessage(m2); sendMessage(mSupply); - - if (useRudderSensor == true){ - DEBUG("useRudderSensor is true"); - + + if (!useRudderSensor) { + int bat3_adc = adc.analogRead(bat3_analog, ADC_0); + bat3 = bat3_adc * analog_max_voltage / adc.getMaxValue(); + + VoltageMeasurement m3(4, "bat3", bat3); + sendMessage(m3); + } + else { int sensorValue = adc.analogRead(bat3_analog, ADC_0); //rudder sensor on bat3_analog input - + //the max reading based on the potentiometer I am using at 3.3v is 680. The rudderFactor is intended such that this maxValue //translates to 66 which is the assumed rudder swing from lock to lock. The factor should be adjusted based on the actual max value and the //actual rudder swing. The calculation - + double maxValue = 680; double midAngle = maxValue / 2; double rudderFactor = 10.31; //use this if max sensorValue is when rudder is fully to port //double rawRudderAngle = sensorValue / rudderFactor; //rawRudderAngle will range from 66 to port, 0 to starboard //double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor) * -1; //displayRudderAngle will range from -33 to port, +33 to starboard - + //use this if max sensorValue is when rudder is fully to starboard double rawRudderAngle = ((sensorValue / rudderFactor) - 66) * -1; //rawRudderAngle will range from 66 to port, 0 to starboard double rudderDisplayAngle = ((sensorValue - midAngle) / rudderFactor); //displayRudderAngle will range from -33 to port, +33 to starboard - + //DEBUG("sensorValue - %d",sensorValue); //DEBUG("rawRudderAngle - %.0f",rawRudderAngle); //DEBUG("rudderDisplayAngle - %.0f",rudderDisplayAngle); - + RUDMessage rm(rawRudderAngle, rudderDisplayAngle); sendMessage(rm); - - } else { - - int bat3_adc = adc.analogRead(bat3_analog, ADC_0); - VoltageMeasurement m3(4, "bat3", bat3); - sendMessage(m3); } - - //DEBUG("ADC - Supply: %sV Bat1: %sV Bat2: %sV Bat3: %sV", - //String(supply, 2).c_str(), String(bat1, 2).c_str(), String(bat2, 2).c_str(), String(bat3, 2).c_str()); - } diff --git a/lib/KBox/src/tasks/ADCTask.h b/lib/KBox/src/tasks/ADCTask.h index 079f27c3..59315f66 100644 --- a/lib/KBox/src/tasks/ADCTask.h +++ b/lib/KBox/src/tasks/ADCTask.h @@ -32,6 +32,9 @@ class ADCTask : public Task, public KGenerator { ADC& adc; float bat1, bat2, bat3, supply; + // Set to true to use bat3 input as a rudder sensor + static const bool useRudderSensor = true; + public: ADCTask(ADC& adc) : Task("ADC"), adc(adc) {}; void loop(); diff --git a/lib/KBox/src/tasks/IMUTask.cpp b/lib/KBox/src/tasks/IMUTask.cpp index e77f72e1..0bea95f6 100644 --- a/lib/KBox/src/tasks/IMUTask.cpp +++ b/lib/KBox/src/tasks/IMUTask.cpp @@ -24,9 +24,6 @@ #include "IMUTask.h" #include "KBoxDebug.h" -enum kboxOrientation {PORT, STBD, BOW, STERN}; -kboxOrientation state = kboxOrientation::BOW; //Modification to send correct course, pitch, roll data based on where the back of the KBOX is mounted. Options are back of the box to port, to bow, to stbd, to stern. Specify as appropriate for your setup - void IMUTask::setup() { DEBUG("Initing BNO055"); if (!bno055.begin()) { @@ -45,60 +42,31 @@ void IMUTask::loop() { bno055.getCalibration(&sysCalib, &gyroCalib, &accelCalib, &magCalib); //DEBUG("BNO055 Calibration - Sys:%i Gyro:%i Accel:%i Mag:%i", sysCalib, gyroCalib, accelCalib, magCalib); + eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - { - eulerAngles = bno055.getVector(Adafruit_BNO055::VECTOR_EULER); - //DEBUG("Vector Euler x=%f y=%f z=%f", eulerAngles.x(), eulerAngles.y(), eulerAngles.z()); - //DEBUG("Course: %.0f MAG Pitch: %.1f Heel: %.1f", eulerAngles.x(), eulerAngles.y(), eulerAngles.z()); - //IMUMessage m(sysCalib, eulerAngles.x(), /* yaw?*/ 0, eulerAngles.y(), eulerAngles.z()); //original - //sendMessage(m); - - //order is heading, yaw, pitch, roll - double eheading, epitch, eroll; - switch(state){ - case PORT: - eheading = eulerAngles.x() - 90; - epitch = eulerAngles.y() * -1; - eroll = eulerAngles.z(); - break; - case STBD: - eheading = eulerAngles.x() + 90; - epitch = eulerAngles.y(); - eroll = eulerAngles.z() * -1; - break; - case BOW: - eheading = eulerAngles.x() - 180; - epitch = eulerAngles.z(); - eroll = eulerAngles.y(); - break; - case STERN: - eheading = eulerAngles.x(); - epitch = eulerAngles.z() * -1; - eroll = eulerAngles.y() * -1; + double eheading, epitch, eroll; + switch(state){ + case PORT: + eheading = eulerAngles.x() - 90; + epitch = eulerAngles.y() * -1; + eroll = eulerAngles.z(); + break; + case STBD: + eheading = eulerAngles.x() + 90; + epitch = eulerAngles.y(); + eroll = eulerAngles.z() * -1; + break; + case BOW: + eheading = eulerAngles.x() - 180; + epitch = eulerAngles.z(); + eroll = eulerAngles.y(); + break; + case STERN: + eheading = eulerAngles.x(); + epitch = eulerAngles.z() * -1; + eroll = eulerAngles.y() * -1; break; - } - + } IMUMessage m(sysCalib, DegToRad(eheading), /* yaw?*/ 0, DegToRad(epitch), DegToRad(eroll)); sendMessage(m); - - } - - //DEBUG("BNO055 temperature is %i", bno055.getTemp()); - - //{ - //imu::Vector<3> vector = bno055.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); - //DEBUG("Vector Magnetometer x=%f y=%f z=%f", vector.x(), vector.y(), vector.z()); - //} - //{ - //imu::Vector<3> vector = bno055.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - //DEBUG("Vector Gyro x=%f y=%f z=%f", vector.x(), vector.y(), vector.z()); - //} - //{ - //imu::Vector<3> vector = bno055.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); - //DEBUG("Vector Accel x=%f y=%f z=%f", vector.x(), vector.y(), vector.z()); - //} - //{ - //imu::Vector<3> vector = bno055.getVector(Adafruit_BNO055::VECTOR_GRAVITY); - //DEBUG("Vector Gravity x=%f y=%f z=%f", vector.x(), vector.y(), vector.z()); - //} } diff --git a/lib/KBox/src/tasks/IMUTask.h b/lib/KBox/src/tasks/IMUTask.h index 97f09cc1..fc76a8e3 100644 --- a/lib/KBox/src/tasks/IMUTask.h +++ b/lib/KBox/src/tasks/IMUTask.h @@ -32,6 +32,11 @@ class IMUTask : public Task, public KGenerator { uint8_t sysCalib, gyroCalib, accelCalib, magCalib; imu::Vector<3> eulerAngles; + enum kboxOrientation {PORT, STBD, BOW, STERN}; + // Modification to send correct course, pitch, roll data based on where the back of the KBOX is mounted. + // Options are back of the box to port, to bow, to stbd, to stern. Specify as appropriate for your setup + static const kboxOrientation state = kboxOrientation::BOW; + public: IMUTask() : Task("IMU") {}; void setup(); From 9aa8f6d03dcf50893a1ad9e8ae21647ba491bcc8 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Sat, 1 Apr 2017 00:47:23 -0700 Subject: [PATCH 38/42] Removing Arduino-PID-Library to re-add it as a subtree --- lib/Arduino-PID-Library/PID_v1.cpp | 195 --------------------- lib/Arduino-PID-Library/PID_v1.h | 81 --------- lib/Arduino-PID-Library/README.txt | 11 -- lib/Arduino-PID-Library/keywords.txt | 34 ---- lib/Arduino-PID-Library/library.json | 19 -- lib/Arduino-PID-Library/library.properties | 9 - 6 files changed, 349 deletions(-) delete mode 100644 lib/Arduino-PID-Library/PID_v1.cpp delete mode 100644 lib/Arduino-PID-Library/PID_v1.h delete mode 100644 lib/Arduino-PID-Library/README.txt delete mode 100644 lib/Arduino-PID-Library/keywords.txt delete mode 100644 lib/Arduino-PID-Library/library.json delete mode 100644 lib/Arduino-PID-Library/library.properties diff --git a/lib/Arduino-PID-Library/PID_v1.cpp b/lib/Arduino-PID-Library/PID_v1.cpp deleted file mode 100644 index 86222c72..00000000 --- a/lib/Arduino-PID-Library/PID_v1.cpp +++ /dev/null @@ -1,195 +0,0 @@ -/********************************************************************************************** - * Arduino PID Library - Version 1.1.1 - * by Brett Beauregard brettbeauregard.com - * - * This Library is licensed under a GPLv3 License - **********************************************************************************************/ - -#if ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -#include - -/*Constructor (...)********************************************************* - * The parameters specified here are those for for which we can't set up - * reliable defaults, so we need to have the user set them. - ***************************************************************************/ -PID::PID(double* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int ControllerDirection) -{ - - myOutput = Output; - myInput = Input; - mySetpoint = Setpoint; - inAuto = false; - - PID::SetOutputLimits(0, 255); //default output limit corresponds to - //the arduino pwm limits - - SampleTime = 100; //default Controller Sample Time is 0.1 seconds - - PID::SetControllerDirection(ControllerDirection); - PID::SetTunings(Kp, Ki, Kd); - - lastTime = millis()-SampleTime; -} - - -/* Compute() ********************************************************************** - * This, as they say, is where the magic happens. this function should be called - * every time "void loop()" executes. the function will decide for itself whether a new - * pid Output needs to be computed. returns true when the output is computed, - * false when nothing has been done. - **********************************************************************************/ -bool PID::Compute() -{ - if(!inAuto) return false; - unsigned long now = millis(); - unsigned long timeChange = (now - lastTime); - if(timeChange>=SampleTime) - { - /*Compute all the working error variables*/ - double input = *myInput; - double error = *mySetpoint - input; - ITerm+= (ki * error); - if(ITerm > outMax) ITerm= outMax; - else if(ITerm < outMin) ITerm= outMin; - double dInput = (input - lastInput); - - /*Compute PID Output*/ - double output = kp * error + ITerm- kd * dInput; - - if(output > outMax) output = outMax; - else if(output < outMin) output = outMin; - *myOutput = output; - - /*Remember some variables for next time*/ - lastInput = input; - lastTime = now; - return true; - } - else return false; -} - - -/* SetTunings(...)************************************************************* - * This function allows the controller's dynamic performance to be adjusted. - * it's called automatically from the constructor, but tunings can also - * be adjusted on the fly during normal operation - ******************************************************************************/ -void PID::SetTunings(double Kp, double Ki, double Kd) -{ - if (Kp<0 || Ki<0 || Kd<0) return; - - dispKp = Kp; dispKi = Ki; dispKd = Kd; - - double SampleTimeInSec = ((double)SampleTime)/1000; - kp = Kp; - ki = Ki * SampleTimeInSec; - kd = Kd / SampleTimeInSec; - - if(controllerDirection ==REVERSE) - { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } -} - -/* SetSampleTime(...) ********************************************************* - * sets the period, in Milliseconds, at which the calculation is performed - ******************************************************************************/ -void PID::SetSampleTime(int NewSampleTime) -{ - if (NewSampleTime > 0) - { - double ratio = (double)NewSampleTime - / (double)SampleTime; - ki *= ratio; - kd /= ratio; - SampleTime = (unsigned long)NewSampleTime; - } -} - -/* SetOutputLimits(...)**************************************************** - * This function will be used far more often than SetInputLimits. while - * the input to the controller will generally be in the 0-1023 range (which is - * the default already,) the output will be a little different. maybe they'll - * be doing a time window and will need 0-8000 or something. or maybe they'll - * want to clamp it from 0-125. who knows. at any rate, that can all be done - * here. - **************************************************************************/ -void PID::SetOutputLimits(double Min, double Max) -{ - if(Min >= Max) return; - outMin = Min; - outMax = Max; - - if(inAuto) - { - if(*myOutput > outMax) *myOutput = outMax; - else if(*myOutput < outMin) *myOutput = outMin; - - if(ITerm > outMax) ITerm= outMax; - else if(ITerm < outMin) ITerm= outMin; - } -} - -/* SetMode(...)**************************************************************** - * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) - * when the transition from manual to auto occurs, the controller is - * automatically initialized - ******************************************************************************/ -void PID::SetMode(int Mode) -{ - bool newAuto = (Mode == AUTOMATIC); - if(newAuto && !inAuto) - { /*we just went from manual to auto*/ - PID::Initialize(); - } - inAuto = newAuto; -} - -/* Initialize()**************************************************************** - * does all the things that need to happen to ensure a bumpless transfer - * from manual to automatic mode. - ******************************************************************************/ -void PID::Initialize() -{ - ITerm = *myOutput; - lastInput = *myInput; - if(ITerm > outMax) ITerm = outMax; - else if(ITerm < outMin) ITerm = outMin; -} - -/* SetControllerDirection(...)************************************************* - * The PID will either be connected to a DIRECT acting process (+Output leads - * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to - * know which one, because otherwise we may increase the output when we should - * be decreasing. This is called from the constructor. - ******************************************************************************/ -void PID::SetControllerDirection(int Direction) -{ - if(inAuto && Direction !=controllerDirection) - { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } - controllerDirection = Direction; -} - -/* Status Funcions************************************************************* - * Just because you set the Kp=-1 doesn't mean it actually happened. these - * functions query the internal state of the PID. they're here for display - * purposes. this are the functions the PID Front-end uses for example - ******************************************************************************/ -double PID::GetKp(){ return dispKp; } -double PID::GetKi(){ return dispKi;} -double PID::GetKd(){ return dispKd;} -int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} -int PID::GetDirection(){ return controllerDirection;} - diff --git a/lib/Arduino-PID-Library/PID_v1.h b/lib/Arduino-PID-Library/PID_v1.h deleted file mode 100644 index f6b4e4ff..00000000 --- a/lib/Arduino-PID-Library/PID_v1.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef PID_v1_h -#define PID_v1_h -#define LIBRARY_VERSION 1.1.1 - -class PID -{ - - - public: - - //Constants used in some of the functions below - #define AUTOMATIC 1 - #define MANUAL 0 - #define DIRECT 0 - #define REVERSE 1 - - //commonly used functions ************************************************************************** - - PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and - double, double, double, int); // Setpoint. Initial tuning parameters are also set here - - void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) - - bool Compute(); // * performs the PID calculation. it should be - // called every time loop() cycles. ON/OFF and - // calculation frequency can be set using SetMode - // SetSampleTime respectively - - void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but - //it's likely the user will want to change this depending on - //the application - - - - //available but not commonly used functions ******************************************************** - void SetTunings(double, double, // * While most users will set the tunings once in the - double); // constructor, this function gives the user the option - // of changing tunings during runtime for Adaptive control - void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT - // means the output will increase when error is positive. REVERSE - // means the opposite. it's very unlikely that this will be needed - // once it is set in the constructor. - void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which - // the PID calculation is performed. default is 100 - - - - //Display functions **************************************************************** - double GetKp(); // These functions query the pid for interal values. - double GetKi(); // they were created mainly for the pid front-end, - double GetKd(); // where it's important to know what is actually - int GetMode(); // inside the PID. - int GetDirection(); // - - private: - void Initialize(); - - double dispKp; // * we'll hold on to the tuning parameters in user-entered - double dispKi; // format for display purposes - double dispKd; // - - double kp; // * (P)roportional Tuning Parameter - double ki; // * (I)ntegral Tuning Parameter - double kd; // * (D)erivative Tuning Parameter - - int controllerDirection; - - double *myInput; // * Pointers to the Input, Output, and Setpoint variables - double *myOutput; // This creates a hard link between the variables and the - double *mySetpoint; // PID, freeing the user from having to constantly tell us - // what these values are. with pointers we'll just know. - - unsigned long lastTime; - double ITerm, lastInput; - - unsigned long SampleTime; - double outMin, outMax; - bool inAuto; -}; -#endif - diff --git a/lib/Arduino-PID-Library/README.txt b/lib/Arduino-PID-Library/README.txt deleted file mode 100644 index 61f4be25..00000000 --- a/lib/Arduino-PID-Library/README.txt +++ /dev/null @@ -1,11 +0,0 @@ -*************************************************************** -* Arduino PID Library - Version 1.1.1 -* by Brett Beauregard brettbeauregard.com -* -* This Library is licensed under a GPLv3 License -*************************************************************** - - - For an ultra-detailed explanation of why the code is the way it is, please visit: - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - - - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary diff --git a/lib/Arduino-PID-Library/keywords.txt b/lib/Arduino-PID-Library/keywords.txt deleted file mode 100644 index 55969c17..00000000 --- a/lib/Arduino-PID-Library/keywords.txt +++ /dev/null @@ -1,34 +0,0 @@ -####################################### -# Syntax Coloring Map For PID Library -####################################### - -####################################### -# Datatypes (KEYWORD1) -####################################### - -PID KEYWORD1 - -####################################### -# Methods and Functions (KEYWORD2) -####################################### - -SetMode KEYWORD2 -Compute KEYWORD2 -SetOutputLimits KEYWORD2 -SetTunings KEYWORD2 -SetControllerDirection KEYWORD2 -SetSampleTime KEYWORD2 -GetKp KEYWORD2 -GetKi KEYWORD2 -GetKd KEYWORD2 -GetMode KEYWORD2 -GetDirection KEYWORD2 - -####################################### -# Constants (LITERAL1) -####################################### - -AUTOMATIC LITERAL1 -MANUAL LITERAL1 -DIRECT LITERAL1 -REVERSE LITERAL1 \ No newline at end of file diff --git a/lib/Arduino-PID-Library/library.json b/lib/Arduino-PID-Library/library.json deleted file mode 100644 index cf2510c6..00000000 --- a/lib/Arduino-PID-Library/library.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "name": "PID", - "keywords": "PID, controller, signal", - "description": "A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).", - "url": "http://playground.arduino.cc/Code/PIDLibrary", - "include": "PID_v1", - "authors": - [ - { - "name": "Brett Beauregard" - } - ], - "repository": - { - "type": "git", - "url": "https://github.com/br3ttb/Arduino-PID-Library.git" - }, - "frameworks": "arduino" -} diff --git a/lib/Arduino-PID-Library/library.properties b/lib/Arduino-PID-Library/library.properties deleted file mode 100644 index 9a316b0b..00000000 --- a/lib/Arduino-PID-Library/library.properties +++ /dev/null @@ -1,9 +0,0 @@ -name=PID -version=1.1.1 -author=Brett Beauregard -maintainer=Brett Beauregard -sentence=PID controller -paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). -category=Signal Input/Output -url=http://playground.arduino.cc/Code/PIDLibrary -architectures=* From 785ed147204832fecd5a5405f7d2d0b4f68b796e Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Sat, 1 Apr 2017 00:48:05 -0700 Subject: [PATCH 39/42] Squashed 'lib/Arduino-PID-Library/' content from commit 5adeed5 git-subtree-dir: lib/Arduino-PID-Library git-subtree-split: 5adeed52b04d4413002d8371ec1525040a143cf3 --- PID_v1.cpp | 195 ++++++++++++++++++ PID_v1.h | 80 +++++++ README.txt | 11 + .../PID_AdaptiveTunings.ino | 56 +++++ examples/PID_Basic/PID_Basic.ino | 35 ++++ examples/PID_RelayOutput/PID_RelayOutput.ino | 64 ++++++ keywords.txt | 34 +++ library.json | 19 ++ library.properties | 9 + 9 files changed, 503 insertions(+) create mode 100644 PID_v1.cpp create mode 100644 PID_v1.h create mode 100644 README.txt create mode 100644 examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino create mode 100644 examples/PID_Basic/PID_Basic.ino create mode 100644 examples/PID_RelayOutput/PID_RelayOutput.ino create mode 100644 keywords.txt create mode 100644 library.json create mode 100644 library.properties diff --git a/PID_v1.cpp b/PID_v1.cpp new file mode 100644 index 00000000..86222c72 --- /dev/null +++ b/PID_v1.cpp @@ -0,0 +1,195 @@ +/********************************************************************************************** + * Arduino PID Library - Version 1.1.1 + * by Brett Beauregard brettbeauregard.com + * + * This Library is licensed under a GPLv3 License + **********************************************************************************************/ + +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include + +/*Constructor (...)********************************************************* + * The parameters specified here are those for for which we can't set up + * reliable defaults, so we need to have the user set them. + ***************************************************************************/ +PID::PID(double* Input, double* Output, double* Setpoint, + double Kp, double Ki, double Kd, int ControllerDirection) +{ + + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + PID::SetOutputLimits(0, 255); //default output limit corresponds to + //the arduino pwm limits + + SampleTime = 100; //default Controller Sample Time is 0.1 seconds + + PID::SetControllerDirection(ControllerDirection); + PID::SetTunings(Kp, Ki, Kd); + + lastTime = millis()-SampleTime; +} + + +/* Compute() ********************************************************************** + * This, as they say, is where the magic happens. this function should be called + * every time "void loop()" executes. the function will decide for itself whether a new + * pid Output needs to be computed. returns true when the output is computed, + * false when nothing has been done. + **********************************************************************************/ +bool PID::Compute() +{ + if(!inAuto) return false; + unsigned long now = millis(); + unsigned long timeChange = (now - lastTime); + if(timeChange>=SampleTime) + { + /*Compute all the working error variables*/ + double input = *myInput; + double error = *mySetpoint - input; + ITerm+= (ki * error); + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + double dInput = (input - lastInput); + + /*Compute PID Output*/ + double output = kp * error + ITerm- kd * dInput; + + if(output > outMax) output = outMax; + else if(output < outMin) output = outMin; + *myOutput = output; + + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + return true; + } + else return false; +} + + +/* SetTunings(...)************************************************************* + * This function allows the controller's dynamic performance to be adjusted. + * it's called automatically from the constructor, but tunings can also + * be adjusted on the fly during normal operation + ******************************************************************************/ +void PID::SetTunings(double Kp, double Ki, double Kd) +{ + if (Kp<0 || Ki<0 || Kd<0) return; + + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + double SampleTimeInSec = ((double)SampleTime)/1000; + kp = Kp; + ki = Ki * SampleTimeInSec; + kd = Kd / SampleTimeInSec; + + if(controllerDirection ==REVERSE) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } +} + +/* SetSampleTime(...) ********************************************************* + * sets the period, in Milliseconds, at which the calculation is performed + ******************************************************************************/ +void PID::SetSampleTime(int NewSampleTime) +{ + if (NewSampleTime > 0) + { + double ratio = (double)NewSampleTime + / (double)SampleTime; + ki *= ratio; + kd /= ratio; + SampleTime = (unsigned long)NewSampleTime; + } +} + +/* SetOutputLimits(...)**************************************************** + * This function will be used far more often than SetInputLimits. while + * the input to the controller will generally be in the 0-1023 range (which is + * the default already,) the output will be a little different. maybe they'll + * be doing a time window and will need 0-8000 or something. or maybe they'll + * want to clamp it from 0-125. who knows. at any rate, that can all be done + * here. + **************************************************************************/ +void PID::SetOutputLimits(double Min, double Max) +{ + if(Min >= Max) return; + outMin = Min; + outMax = Max; + + if(inAuto) + { + if(*myOutput > outMax) *myOutput = outMax; + else if(*myOutput < outMin) *myOutput = outMin; + + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + } +} + +/* SetMode(...)**************************************************************** + * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + * when the transition from manual to auto occurs, the controller is + * automatically initialized + ******************************************************************************/ +void PID::SetMode(int Mode) +{ + bool newAuto = (Mode == AUTOMATIC); + if(newAuto && !inAuto) + { /*we just went from manual to auto*/ + PID::Initialize(); + } + inAuto = newAuto; +} + +/* Initialize()**************************************************************** + * does all the things that need to happen to ensure a bumpless transfer + * from manual to automatic mode. + ******************************************************************************/ +void PID::Initialize() +{ + ITerm = *myOutput; + lastInput = *myInput; + if(ITerm > outMax) ITerm = outMax; + else if(ITerm < outMin) ITerm = outMin; +} + +/* SetControllerDirection(...)************************************************* + * The PID will either be connected to a DIRECT acting process (+Output leads + * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + * know which one, because otherwise we may increase the output when we should + * be decreasing. This is called from the constructor. + ******************************************************************************/ +void PID::SetControllerDirection(int Direction) +{ + if(inAuto && Direction !=controllerDirection) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +/* Status Funcions************************************************************* + * Just because you set the Kp=-1 doesn't mean it actually happened. these + * functions query the internal state of the PID. they're here for display + * purposes. this are the functions the PID Front-end uses for example + ******************************************************************************/ +double PID::GetKp(){ return dispKp; } +double PID::GetKi(){ return dispKi;} +double PID::GetKd(){ return dispKd;} +int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} +int PID::GetDirection(){ return controllerDirection;} + diff --git a/PID_v1.h b/PID_v1.h new file mode 100644 index 00000000..77b3e4b5 --- /dev/null +++ b/PID_v1.h @@ -0,0 +1,80 @@ +#ifndef PID_v1_h +#define PID_v1_h +#define LIBRARY_VERSION 1.1.1 + +class PID +{ + + + public: + + //Constants used in some of the functions below + #define AUTOMATIC 1 + #define MANUAL 0 + #define DIRECT 0 + #define REVERSE 1 + + //commonly used functions ************************************************************************** + PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and + double, double, double, int); // Setpoint. Initial tuning parameters are also set here + + void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) + + bool Compute(); // * performs the PID calculation. it should be + // called every time loop() cycles. ON/OFF and + // calculation frequency can be set using SetMode + // SetSampleTime respectively + + void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but + //it's likely the user will want to change this depending on + //the application + + + + //available but not commonly used functions ******************************************************** + void SetTunings(double, double, // * While most users will set the tunings once in the + double); // constructor, this function gives the user the option + // of changing tunings during runtime for Adaptive control + void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT + // means the output will increase when error is positive. REVERSE + // means the opposite. it's very unlikely that this will be needed + // once it is set in the constructor. + void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which + // the PID calculation is performed. default is 100 + + + + //Display functions **************************************************************** + double GetKp(); // These functions query the pid for interal values. + double GetKi(); // they were created mainly for the pid front-end, + double GetKd(); // where it's important to know what is actually + int GetMode(); // inside the PID. + int GetDirection(); // + + private: + void Initialize(); + + double dispKp; // * we'll hold on to the tuning parameters in user-entered + double dispKi; // format for display purposes + double dispKd; // + + double kp; // * (P)roportional Tuning Parameter + double ki; // * (I)ntegral Tuning Parameter + double kd; // * (D)erivative Tuning Parameter + + int controllerDirection; + + double *myInput; // * Pointers to the Input, Output, and Setpoint variables + double *myOutput; // This creates a hard link between the variables and the + double *mySetpoint; // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. + + unsigned long lastTime; + double ITerm, lastInput; + + unsigned long SampleTime; + double outMin, outMax; + bool inAuto; +}; +#endif + diff --git a/README.txt b/README.txt new file mode 100644 index 00000000..61f4be25 --- /dev/null +++ b/README.txt @@ -0,0 +1,11 @@ +*************************************************************** +* Arduino PID Library - Version 1.1.1 +* by Brett Beauregard brettbeauregard.com +* +* This Library is licensed under a GPLv3 License +*************************************************************** + + - For an ultra-detailed explanation of why the code is the way it is, please visit: + http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ + + - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino new file mode 100644 index 00000000..94f3faf5 --- /dev/null +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -0,0 +1,56 @@ +/******************************************************** + * PID Adaptive Tuning Example + * One of the benefits of the PID library is that you can + * change the tuning parameters at any time. this can be + * helpful if we want the controller to be agressive at some + * times, and conservative at others. in the example below + * we set the controller to use Conservative Tuning Parameters + * when we're near setpoint and more agressive Tuning + * Parameters when we're farther away. + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 + +//Define Variables we'll be connecting to +double Setpoint, Input, Output; + +//Define the aggressive and conservative Tuning Parameters +double aggKp=4, aggKi=0.2, aggKd=1; +double consKp=1, consKi=0.05, consKd=0.25; + +//Specify the links and initial tuning parameters +PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT); + +void setup() +{ + //initialize the variables we're linked to + Input = analogRead(PIN_INPUT); + Setpoint = 100; + + //turn the PID on + myPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = analogRead(PIN_INPUT); + + double gap = abs(Setpoint-Input); //distance away from setpoint + if (gap < 10) + { //we're close to setpoint, use conservative tuning parameters + myPID.SetTunings(consKp, consKi, consKd); + } + else + { + //we're far from setpoint, use aggressive tuning parameters + myPID.SetTunings(aggKp, aggKi, aggKd); + } + + myPID.Compute(); + analogWrite(PIN_OUTPUT, Output); +} + + diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino new file mode 100644 index 00000000..8028b588 --- /dev/null +++ b/examples/PID_Basic/PID_Basic.ino @@ -0,0 +1,35 @@ +/******************************************************** + * PID Basic Example + * Reading analog input 0 to control analog PWM output 3 + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 + +//Define Variables we'll be connecting to +double Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +double Kp=2, Ki=5, Kd=1; +PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); + +void setup() +{ + //initialize the variables we're linked to + Input = analogRead(PIN_INPUT); + Setpoint = 100; + + //turn the PID on + myPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = analogRead(PIN_INPUT); + myPID.Compute(); + analogWrite(PIN_OUTPUT, Output); +} + + diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino new file mode 100644 index 00000000..17fbe1a3 --- /dev/null +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -0,0 +1,64 @@ +/******************************************************** + * PID RelayOutput Example + * Same as basic example, except that this time, the output + * is going to a digital pin which (we presume) is controlling + * a relay. the pid is designed to Output an analog value, + * but the relay can only be On/Off. + * + * to connect them together we use "time proportioning + * control" it's essentially a really slow version of PWM. + * first we decide on a window size (5000mS say.) we then + * set the pid to adjust its output between 0 and that window + * size. lastly, we add some logic that translates the PID + * output into "Relay On Time" with the remainder of the + * window being "Relay Off Time" + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define RELAY_PIN 6 + +//Define Variables we'll be connecting to +double Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +double Kp=2, Ki=5, Kd=1; +PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); + +int WindowSize = 5000; +unsigned long windowStartTime; + +void setup() +{ + windowStartTime = millis(); + + //initialize the variables we're linked to + Setpoint = 100; + + //tell the PID to range between 0 and the full window size + myPID.SetOutputLimits(0, WindowSize); + + //turn the PID on + myPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = analogRead(PIN_INPUT); + myPID.Compute(); + + /************************************************ + * turn the output pin on/off based on pid output + ************************************************/ + if (millis() - windowStartTime > WindowSize) + { //time to shift the Relay Window + windowStartTime += WindowSize; + } + if (Output < millis() - windowStartTime) digitalWrite(RELAY_PIN, HIGH); + else digitalWrite(RELAY_PIN, LOW); + +} + + + diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 00000000..55969c17 --- /dev/null +++ b/keywords.txt @@ -0,0 +1,34 @@ +####################################### +# Syntax Coloring Map For PID Library +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +PID KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +SetMode KEYWORD2 +Compute KEYWORD2 +SetOutputLimits KEYWORD2 +SetTunings KEYWORD2 +SetControllerDirection KEYWORD2 +SetSampleTime KEYWORD2 +GetKp KEYWORD2 +GetKi KEYWORD2 +GetKd KEYWORD2 +GetMode KEYWORD2 +GetDirection KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + +AUTOMATIC LITERAL1 +MANUAL LITERAL1 +DIRECT LITERAL1 +REVERSE LITERAL1 \ No newline at end of file diff --git a/library.json b/library.json new file mode 100644 index 00000000..cf2510c6 --- /dev/null +++ b/library.json @@ -0,0 +1,19 @@ +{ + "name": "PID", + "keywords": "PID, controller, signal", + "description": "A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).", + "url": "http://playground.arduino.cc/Code/PIDLibrary", + "include": "PID_v1", + "authors": + [ + { + "name": "Brett Beauregard" + } + ], + "repository": + { + "type": "git", + "url": "https://github.com/br3ttb/Arduino-PID-Library.git" + }, + "frameworks": "arduino" +} diff --git a/library.properties b/library.properties new file mode 100644 index 00000000..9a316b0b --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=PID +version=1.1.1 +author=Brett Beauregard +maintainer=Brett Beauregard +sentence=PID controller +paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). +category=Signal Input/Output +url=http://playground.arduino.cc/Code/PIDLibrary +architectures=* From 039985bc4a6c1dc24e41f233c9bbc434a189b23e Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Sat, 1 Apr 2017 00:50:16 -0700 Subject: [PATCH 40/42] Finalizing adding the Arduino PID Lib as a subtree --- README.md | 2 ++ lib/README.md | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index c5855270..116e148c 100644 --- a/README.md +++ b/README.md @@ -153,6 +153,8 @@ separately. - [Adafruit BMP280](https://github.com/adafruit/Adafruit_BMP280_Library) is under the BSD license - [Adafruit BNO055](https://github.com/adafruit/Adafruit_BNO055/) is under the MIT license - [ADC library](https://github.com/pedvide/ADC) is a permissive custom license + - [Arduino PID Library](https://github.com/br3ttb/Arduino-PID-Library/) is + under the GPLv3 - [FlexCAN](https://github.com/teachop/FlexCAN_Library) does not seem to have a license [yet](https://github.com/teachop/FlexCAN_Library/issues/12) - [i2c_t3](https://github.com/nox771/i2c_t3) is under the LGPL diff --git a/lib/README.md b/lib/README.md index c52f0a2b..41eba0ec 100644 --- a/lib/README.md +++ b/lib/README.md @@ -14,10 +14,11 @@ To pull or push to the subtrees, start by defining remotes for all the origins: git remote add paul-time git@github.com:PaulStoffregen/Time.git git remote add espasynctcp git@github.com:me-no-dev/ESPAsyncTCP.git git remote add adafruit-ina219 git@github.com:adafruit/Adafruit_INA219.git + git remote add br3ttb-arduino-pid-library https://github.com/br3ttb/Arduino-PID-Library.git To add a new subtree: - git subtree add --prefix=lib/ESPAsyncTCP espasynctcp/master + git subtree add --squash --prefix=lib/ESPAsyncTCP espasynctcp/master To pull changes from a subtree: From aeba2ff426d0d24dc73894d49c237a9ad86e2f76 Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Sat, 1 Apr 2017 01:26:19 -0700 Subject: [PATCH 41/42] Move headingPID inside AutoPilotTask - do not run autopilot continuously but only every 250ms - and also fix some indentation issues --- lib/KBox/src/tasks/AutoPilotTask.cpp | 67 +++++++++++++--------------- lib/KBox/src/tasks/AutoPilotTask.h | 18 +++++--- src/host/main.cpp | 8 ++-- 3 files changed, 48 insertions(+), 45 deletions(-) diff --git a/lib/KBox/src/tasks/AutoPilotTask.cpp b/lib/KBox/src/tasks/AutoPilotTask.cpp index cbc37029..1eee3092 100644 --- a/lib/KBox/src/tasks/AutoPilotTask.cpp +++ b/lib/KBox/src/tasks/AutoPilotTask.cpp @@ -25,10 +25,10 @@ /* AutoPilot functionality can exist at several levels of complexity. In this initial iteration, the design focuses on heading the boat on a defined course that is automatically set when AP "Heading" Mode is activated - Later on we could introduce an additional mode when a course to a waypoint is provided by a NMEA-connected device. + Later on we could introduce an additional mode when a course to a waypoint is provided by a NMEA-connected device. "Dodge" mode suspends rudder actions temporarily to manually steer the boat around - an object or to manually put it back on course. - + an object or to manually put it back on course. + The course calculation approach below is a simplified and adapted version of Robert Huitema's excellent FreeboardPLC_v1_2 code. RIGM */ @@ -40,51 +40,46 @@ #include #include -double apCurrentHeading = 0; -double apTargetHeading = 0; -double apTargetRudderPosition = 0; - -PID headingPID(&apCurrentHeading, &apTargetRudderPosition, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int)REVERSE); -bool isSetup = false; //if false sets initial PID variables on bootup +AutoPilotTask::AutoPilotTask() : Task("AutoPilotTask"), + apCurrentHeading(0), apTargetHeading(0), apTargetRudderPosition(0), + headingPID(&apCurrentHeading, &apTargetRudderPosition, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int) REVERSE) +{ -//AutoPilotTask::AutoPilotTask() : apCurrentHeading(0), apTargetHeading(0), apTargetRudderPosition(0), headingPID(&apCurrentHeading, &apTargetRudderPostion, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int) REVERSE){}; +} void AutoPilotTask::processMessage(const KMessage &message) { - message.accept(*this); + message.accept(*this); } void AutoPilotTask::visit(const NAVMessage &nav) { - apCurrentHeading = nav.getCurrentHeading(); - apTargetHeading = nav.getTargetHeading(); - navMode = nav.getApMode(); - navDodgeMode = nav.getApDodgeMode(); - //DEBUG("navMode - %.d",navMode); + apCurrentHeading = nav.getCurrentHeading(); + apTargetHeading = nav.getTargetHeading(); + navMode = nav.getApMode(); + navDodgeMode = nav.getApDodgeMode(); } void AutoPilotTask::visit(const RUDMessage &rm) { - apRudderSensorPosition = rm.getRawRudderAngle(); + apRudderSensorPosition = rm.getRawRudderAngle(); +} + +void AutoPilotTask::setup() { + sameLastDirection=true; + apTargetRudderPosition = MAXRUDDERSWING / 2; //centred. For programming purposes rudder is assumed to move through 66 degrees lock to lock + headingPID.SetMode(AUTOMATIC); + headingPID.SetOutputLimits(0.0, MAXRUDDERSWING); //output limits 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right) + headingPID.SetSampleTime(AUTOPILOT_SAMPLE_TIME); + DEBUG("Init autopilot complete"); } void AutoPilotTask::loop() { - - if (isSetup == false){ //tried a separate "setup" for init but could not get it to work - sameLastDirection=true; - apTargetRudderPosition = MAXRUDDERSWING / 2; //centred. For programming purposes rudder is assumed to move through 66 degrees lock to lock - headingPID.SetMode(AUTOMATIC); - headingPID.SetOutputLimits(0.0, MAXRUDDERSWING); //output limits 0 = full starboard rudder (trailing edge of rudder to the right, bow moves to right) - headingPID.SetSampleTime(250); - DEBUG("Init autopilot complete"); - isSetup = true; - } - headingPID.Compute(); - + apRudderCommandSent = apTargetRudderPosition; //by default send rudder command to equal target position, then modify it double testDegs = apTargetRudderPosition - apRudderCommandSent; - + if (abs(testDegs) > AUTOPILOTDEADZONE) { DEBUG("need to move the rudder"); - + //then we move the rudder. //is it changing movement direction, we need to compensate for slack if (sameLastDirection && apTargetRudderPosition > apRudderCommandSent) { @@ -102,17 +97,17 @@ void AutoPilotTask::loop() { sameLastDirection = true; apRudderCommandSent = apTargetRudderPosition + AUTOPILOTSLACK; } - + } APMessage m(apTargetRudderPosition, apRudderCommandSent); sendMessage(m); if (navMode == true && navDodgeMode == false){ - - //DEBUG("apRudderCommandSent - %.0f", apRudderCommandSent); - //DEBUG("apRudderSensorPosition - %.0f",apRudderSensorPosition); - + + //DEBUG("apRudderCommandSent - %.0f", apRudderCommandSent); + //DEBUG("apRudderSensorPosition - %.0f",apRudderSensorPosition); + if (apRudderCommandSent > apRudderSensorPosition){ //send acuator stop command; DEBUG("moving rudder left"); diff --git a/lib/KBox/src/tasks/AutoPilotTask.h b/lib/KBox/src/tasks/AutoPilotTask.h index b38a7fea..a457618a 100644 --- a/lib/KBox/src/tasks/AutoPilotTask.h +++ b/lib/KBox/src/tasks/AutoPilotTask.h @@ -23,7 +23,6 @@ */ #pragma once -//#include #include "TaskManager.h" #include "KMessage.h" #include @@ -35,19 +34,28 @@ #define AUTOPILOTDEADZONE 0 //to be adjusted based on boat characteristics #define AUTOPILOTSLACK 0 //to be adjusted based on boat characteristics #define MAXRUDDERSWING 66.0 //max swing lock to lock +// How often should the autopilot computations run +#define AUTOPILOT_SAMPLE_TIME 250 -class AutoPilotTask : public Task, public KReceiver, public KVisitor, public KGenerator { private: - +class AutoPilotTask : public Task, public KReceiver, public KVisitor, public KGenerator { + private: double apRudderSensorPosition; //populated from RudderSensorTask double apRudderCommandSent; bool sameLastDirection; //last rudder movement direction bool navMode = false; bool navDodgeMode = false; - + + double apCurrentHeading = 0; + double apTargetHeading = 0; + double apTargetRudderPosition = 0; + + PID headingPID; + public: - AutoPilotTask() : Task("AutoPilot") {}; + AutoPilotTask(); void processMessage(const KMessage& message); void visit(const NAVMessage&); void visit(const RUDMessage&); + void setup(); void loop(); }; diff --git a/src/host/main.cpp b/src/host/main.cpp index f9818dc7..b8dbb30b 100644 --- a/src/host/main.cpp +++ b/src/host/main.cpp @@ -87,7 +87,7 @@ void setup() { n2kTask->connectTo(*sdcardTask); baroTask->connectTo(*sdcardTask); imuTask->connectTo(*sdcardTask); - + AutoPilotTask *autoPilotTask = new AutoPilotTask(); autoPilotTask->connectTo(*wifi); autoPilotTask->connectTo(*n2kTask); @@ -103,8 +103,8 @@ void setup() { kbox.addTask(reader2); kbox.addTask(wifi); kbox.addTask(sdcardTask); - kbox.addTask(autoPilotTask); - + kbox.addTask(new IntervalTask(autoPilotTask, AUTOPILOT_SAMPLE_TIME)); + NavigationPage *navPage = new NavigationPage(); imuTask->connectTo(*navPage); adcTask->connectTo(*navPage); @@ -127,7 +127,7 @@ void setup() { kbox.setup(); - // Reinitialize debug here because in some configurations + // Reinitialize debug here because in some configurations // (like logging to nmea2 output), the kbox setup might have messed // up the debug configuration. DEBUG_INIT(); From ce0b4b9e97ffb7e61753e07bb0e45dbc3689298b Mon Sep 17 00:00:00 2001 From: Thomas Sarlandie Date: Sat, 1 Apr 2017 01:33:21 -0700 Subject: [PATCH 42/42] fix indentation --- lib/KBox/src/KMessage.h | 78 ++++++++++++++++++++--------------------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/lib/KBox/src/KMessage.h b/lib/KBox/src/KMessage.h index beb06697..1686033e 100644 --- a/lib/KBox/src/KMessage.h +++ b/lib/KBox/src/KMessage.h @@ -162,7 +162,7 @@ class IMUMessage: public KMessage { void accept(KVisitor &v) const { v.visit(*this); }; - + int getCalibration() const { return calibration; }; @@ -197,89 +197,89 @@ class IMUMessage: public KMessage { }; class NAVMessage: public KMessage { -private: + private: bool apMode, apHeadingMode, apWaypointMode, apDodgeMode; double currentHeading, targetHeading, courseToWaypoint; - -public: + + public: NAVMessage(bool apMode, bool apHeadingMode, bool apWaypointMode, bool apDodgeMode, double currentHeading,double targetHeading, double courseToWaypoint): apMode(apMode), apHeadingMode(apHeadingMode), apWaypointMode(apWaypointMode), apDodgeMode(apDodgeMode), currentHeading(currentHeading),targetHeading(targetHeading),courseToWaypoint(courseToWaypoint){}; - + void accept(KVisitor &v) const { - v.visit(*this); + v.visit(*this); }; - + bool getApMode() const { - return apMode; + return apMode; }; - + bool getApHeadingMode() const { - return apHeadingMode; + return apHeadingMode; }; - + bool getApWaypointMode() const { - return apWaypointMode; + return apWaypointMode; }; - + bool getApDodgeMode() const { - return apDodgeMode; + return apDodgeMode; }; - + /* * Headings in Degrees for messages passed back and forth between the Autopilot task and the Nav page */ double getCurrentHeading() const { - return currentHeading; + return currentHeading; }; - + double getTargetHeading() const { - return targetHeading; + return targetHeading; }; - + double getCourseToWaypoint() const { - return courseToWaypoint; + return courseToWaypoint; }; }; class APMessage: public KMessage { -private: + private: double targetRudderPosition, rudderCommandSent; - -public: + + public: APMessage(double targetRudderPosition, double rudderCommandSent) : targetRudderPosition(targetRudderPosition),rudderCommandSent(rudderCommandSent){}; - + void accept(KVisitor &v) const { - v.visit(*this); + v.visit(*this); }; - + double getTargetRudderPosition() const { - return targetRudderPosition; + return targetRudderPosition; }; - + double getRudderCommandSent() const { - return rudderCommandSent; + return rudderCommandSent; }; - + }; class RUDMessage: public KMessage { -private: + private: double rawRudderAngle, rudderDisplayAngle; - -public: + + public: RUDMessage(double rawRudderAngle, double rudderDisplayAngle) : rawRudderAngle(rawRudderAngle), rudderDisplayAngle(rudderDisplayAngle){}; - + void accept(KVisitor &v) const { - v.visit(*this); + v.visit(*this); }; - + double getRawRudderAngle() const { - return rawRudderAngle; + return rawRudderAngle; }; - + double getRudderDisplayAngle() const { - return rudderDisplayAngle; + return rudderDisplayAngle; }; };