Add navigation page and autopilot task #28

matrixlinks · 2017-03-21T19:41:33Z

Change to enable configuring Kbox orientation and autopilot functionality

Adding autopilot functionality

sarfata

Wow! For a first external pull-request to kbox-firmware you have set the bar super high!

Thank you so much for all of this. I have lots of comments but that is totally expected :) Let me know if you have any question - and feel free to disagree with me too!

sarfata · 2017-03-22T04:06:36Z

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


You do not need the "RIGM added" comments. It's pretty obvious in the pull request what you changed and the code will get pretty polluted in the future if we add names to each line. Can you remove those?

Will remove.

now removed throughout

sarfata · 2017-03-22T04:06:57Z

lib/KBox/src/KMessage.h

@@ -145,41 +151,46 @@ class NMEA2000Message: public KMessage {

 class IMUMessage: public KMessage {
  private:
+    String label; //RIGM added, may not need it


Do you need it? If yes, what is this for? If not, can you remove it?

Will remove

now removed

sarfata · 2017-03-22T04:07:33Z

lib/KBox/src/KMessage.h


    int getCalibration() const {
      return calibration;
    };

    /*
-     * Heading in Radians
+     * Heading in Radians: for necessary precision need to use float, not double for radians (RIGM)


Doubles are more precise than float. Can you explain why you changed from double to float?

I misunderstood the precise, will change back to double

reverted to doubles

sarfata · 2017-03-22T04:08:16Z

lib/KBox/src/KMessage.h

@@ -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


Please remove the label if you do not need it. Strings take up a lot of memory and we need to be careful to not waste RAM.

Will remove

now removed

sarfata · 2017-03-22T04:09:34Z

lib/KBox/src/KMessage.h

+    };
+
+    /*
+     * Courses in Degrees, therefore using doubles:


To avoid confusion and future errors I think we should use the same units everywhere. This is a good principle that SignalK is also pushing hard on.

Radians are the SI unit for angles and so I think we should use radian everywhere.

However, it's not clear to me reading the code if your angles are magnetic or true and we should document this here so that it's explicit.

Will note that headings are magnetic.
With respect to the data passed back and forth between the nav page and the autopilot task, yes we could have the Kmessage heading-related variables in radians, but assuming we are doing the autopilot math in degrees, that would mean converting degrees to radians in the navPage and then immediately converting degrees to radians in the autopilot task. Seems that this would add calculation overhead for no benefit. On the other hand, the IMU Message is in radians, and this makes it easier to transmit directly to N2K.

Pending discussion about whether to do conversions on messages passed back and forth between navPage and autopilot task.

I tried changing the NAVMessage to store values as radians and adding the DegToRad and RadToDeg conversions on messages, but the PID code will no longer compile:
PID headingPID(RadToDeg(&apCurrentHeading), &apTargetRudderPosition, (RadToDeg)&apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int)REVERSE); error: cannot convert 'double*' to 'double' for argument '1' to 'double RadToDeg(double)'
I'm probably not doing this right. For now I will leave it the way it was, which works.

sarfata · 2017-03-22T15:12:46Z

lib/KBox/src/tasks/AutoPilotTask.cpp

+
+    autoPilotOffCourse = apTargetHeading - apCurrentHeading; //variable not currently used for anything
+
+    autoPilotDeadZone = 5; //pending tuning on water


We should put all the tuning parameters in one place, ideally as #define or static const variables in the header file. This way it's easy to see what values can be tuned.

Also, I think we should do all variable initializations in setup()

OK will move tuning variables as #defines and initialize in setup

now set up as defines

sarfata · 2017-03-22T15:14:39Z

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


Can you make that an enum instead of a string?

Sure. I'd do some research on enum ...

now set up as enum

sarfata · 2017-03-22T15:16:44Z

lib/KBox/src/tasks/IMUTask.cpp

-    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);


I think things would be a bit easier to read if you first assigned values to double variables for yaw, heel and pitch and then applied the transformation, and at the end built the message and sent it.

OK, I think I understand what you are getting at

Looks awesome. Thanks a lot!

sarfata · 2017-03-22T15:17:18Z

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


That is going to be a lot of debugging messages, no? Maybe we should remove this one and instead add some UI to show the user whether the IMU is calibrated.

We could show a message like "Calibrating" in place of "APMode: Off" on the nav page until IMU is calibrated. Could also prevent trying to use the AP until calibration is OK

navpage now shows "calibrating" message until imu is calibrated

sarfata · 2017-03-22T15:19:57Z

lib/KBox/src/tasks/AutoPilotTask.h

+
+    double apRudderSensorPosition;
+    double apRudderCommandSent;
+    double autoPilotOffCourse; //not currently used


Please remove things that are not currently used. It will make it a lot easier to read and understand your code :)

OK, but I've now just realized why that is there: we will actually be using it. But I get the general point.

A more general process newbie question: once I've made these changes in my source code, what happens next. Do I re-upload the respective files to the branch? Do I attach the respective files to the pull request? Do I generate another pull request based on up-loading the files?

In th course of making these changes I implemented quite a few improvements in how the ap code works. So now wondering if I should upload modified and corrected files and generate a new pull request.

sarfata · 2017-03-23T15:20:09Z

The beauty of a code request is that it can be updated :) Just commit your new changes to your branch and click the sync button, this page will update with your updated code.

…

On Thu, Mar 23, 2017 at 8:11 AM matrixlinks ***@***.***> wrote: ***@***.**** commented on this pull request. ------------------------------ In lib/KBox/src/tasks/AutoPilotTask.h <#28 (comment)>: > +#include <PID_v1.h> + +//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 In th course of making these changes I implemented quite a few improvements in how the ap code works. So now wondering if I should upload modified and corrected files and generate a new pull request. — You are receiving this because you commented. Reply to this email directly, view it on GitHub <#28 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAvi_Zy-0kRgT47YH8BHQv130ShbnwFPks5root6gaJpZM4MkTpv> .

matrixlinks · 2017-03-23T23:02:43Z

Hi Thomas: I have uploaded all the changes made so far. However, so far have not been able to find a “sync” button, but I see that there are references in the pull request to my recent uploads. If there is some other button I should be clicking, let me know. The two open issues are: • Whether to do DegToRad / RadToDeg on all messages exchanged between the NavPage and the AutoPilot Task: since these are not going out to N2K, it seems to me that doing conversions on all those messages could be adding unnecessary calculation overhead: but you have a better idea than I of the implications of that. • I need some help with the syntax for declaring the PID object in AutoPilotTask.h so that it is accessible in AutoPilotTask.cpp without being declared it as a global variable. I tried various formats without success: this is where my understanding of c++-style code falls short. Other than those two issues, I think everything else is cleaned up. A bientot, Rob

…

On Mar 23, 2017, at 11:20, Thomas Sarlandie ***@***.***> wrote: The beauty of a code request is that it can be updated :) Just commit your new changes to your branch and click the sync button, this page will update with your updated code. On Thu, Mar 23, 2017 at 8:11 AM matrixlinks ***@***.***> wrote: > ***@***.**** commented on this pull request. > ------------------------------ > > In lib/KBox/src/tasks/AutoPilotTask.h > <#28 (comment)>: > > > +#include <PID_v1.h> > + > +//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 > > In th course of making these changes I implemented quite a few > improvements in how the ap code works. So now wondering if I should upload > modified and corrected files and generate a new pull request. > > — > You are receiving this because you commented. > Reply to this email directly, view it on GitHub > <#28 (comment)>, > or mute the thread > <https://github.com/notifications/unsubscribe-auth/AAvi_Zy-0kRgT47YH8BHQv130ShbnwFPks5root6gaJpZM4MkTpv> > . > — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub, or mute the thread.

sarfata · 2017-03-30T03:30:19Z

Rob,

Looks good to me. Sorry for the additional changes. If you don't have time, we can also go ahead and merge things as they are now and I will them.

I have answered on how to declare the headingPID, let me know if it does not work and I will do it for you and make a pull-request against your project ;)

Also, I do like using RadToDeg and DegToRad, I think it's worth it in the long run.

I think we could simplify the way that the AutopilotPage communicates with the AutopilotTask a bit by having the page keep a reference to the task but I am not 100% sure of which variables I would pass by message and which I would just access directly so let's stick to what you have right now.

sarfata · 2017-03-30T03:12:52Z

lib/KBox/src/pages/NavigationPage.cpp

+        apModeDisplay->setColor(ColorRed);
+    }
+
+    rawHeading = RadToDeg(imu.getCourse());


I think it would make sense to fix the IMU to always return something between 0 and 360. A heading of -30 does not make a lot of sense I think.

I think making changes to the IMU code at this stage would interfere with how it sends N2K data. I could be wrong about this but my impression from testing this with other N2K apps is that N2K sends heading data as positive values between 0 and 180 degrees and negative values (relative to 0) between 180 and 360. So 30 degree compass is +30 (actually the radians equivalent), but 330 degrees compass is -30 (in radians equivalent), and the N2K software on the other end does the conversion to 360-30 = 330. Am I off base here?

Sorry - Yes you are right, I went with the NMEA2000 convention here and we should stick to it.

sarfata · 2017-03-30T03:14:09Z

lib/KBox/src/KMessageNMEAVisitor.cpp

+#include "KBoxDebug.h"
+
+#define IMU_IN_NMEA2K true  //variable to determine whether IMU data sent out over N2K
+unsigned int _imuSequence = 1;


This is a bit of a hack and we could hide it better by making this variable a static variable in the function where it is used. We do not need anywhere else.

Moved it inside the KMessageNMEAVisitor::visit function

sarfata · 2017-03-30T03:14:34Z

lib/KBox/src/KMessage.h

+
+};
+
+class RUDMessage: public KMessage {  //RIGM added for autopilot functionality


Please remove the //RIGM, here and above.

Removed (missed these ones the last time through)

sarfata · 2017-03-30T03:15:03Z

lib/KBox/src/KMessage.h

@@ -157,20 +164,20 @@ class IMUMessage: public KMessage {
    void accept(KVisitor &v) const {
      v.visit(*this);
    };
-
+    


I am really being nitpicky here but please remove those extra spaces here.

Not really certain what you are getting at here but have tried to make spacing consistent for everything on this page.

What I meant is that there are a few white spaces at the beginning of an empty line (github highlights them). Not a big deal, I will take care of that.

sarfata · 2017-03-30T03:17:47Z

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


I think an even better solution would be to keep ADCTask and have a config option in it to replace BAT3 by a Rudder sensor.

If the option is not set, ADCTask would send four VoltageMeasurement like it used to.
If the option is set, ADCTask would send three VoltageMeasurement and one RudderAngle.

sarfata · 2017-03-30T03:22:13Z

lib/KBox/src/tasks/AutoPilotTask.cpp

+double apTargetHeading = 0;
+double apTargetRudderPosition = 0;
+
+PID headingPID(&apCurrentHeading, &apTargetRudderPosition, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int)REVERSE);


What you need here is to declare the headingPID and the current and target variables in the class definition:

class AutoPilotTask { private: double apCurrentHeading, apTargetHeading, apTargetRudderPosition; PID headingPID; // ... };

And initialize it in the constructor:

AutoPilotTask::AutoPilotTask() : apCurrentHeading(0), apTargetHeading(0), apTargetRudderPosition(0), headingPID(&apCurrentHeading, &apTargetRudderPostion, &apTargetHeading, (double)P_Param, (double)I_Param, (double)D_Param, (int) REVERSE) { }

Tried that: won't compile with "error: no matching function for call to 'PID::PID()". Evidently I'm not doing this right. If you could take a crack at it that would be helpful.

In particular, I'm also not sure how I would use this code. Does this constructor create the HeadingPid object that can be accessed in other functions on AutoPilotTask.cpp?

One of the issues I've encountered here and elsewhere is that I can only send KMessage objects from the "loop" function in any task. If I try to do "sendMessage" from any other function nothing happens. So I'm stuck on how to create a headingPID object, and access that object in both setup() (assuming we would like a distinct setup function) and the loop.

I'm having the same issue with the MFDTask code: when the MFDTask::monitorN2kMessage gets an N2K message, I can extract the data, but if I try to create a MONMessage (the KMessage I'm using to send position, label and value to the display), I can't send it from within the monitorN2kMessage function. I can only send it from the loop function in MFDTask. Nor can I figure out how to create a MONMessage object inside the monitorN2kMessage function and then pass the object to the loop function to send the data to the display page. Obviously there are some very different ways that c++ deals with objects than what I am used to! Any insights or practical solutions you can offer would be great. The easiest solution here would be if I could send the Kmessage from within the function. There's probably a switch somewhere in the visitor code structure that could let me do that.

I have done the following:

moved setup code to setup, removed isSetup

moved headingPID inside the AutoPilotTask so now it is available in loop() and in setup()

Regarding your question about the MFD: the MFD class should have variables that it updates when it receives nmea messages. And then whenever it runs the loop, it can update the display. You can look at the BatteryMonitorPage for an example, it's very similar. Sorry if I am not understanding the question.

sarfata · 2017-03-30T03:23:12Z

lib/KBox/src/tasks/AutoPilotTask.cpp

+
+  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


You have defined a constant MAXRUDDERSWING so you should probably do MAXRUDDERSWING/2 here, no?

sarfata · 2017-03-30T03:24:15Z

lib/KBox/src/tasks/IMUTask.cpp

-    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);


Looks awesome. Thanks a lot!

matrixlinks · 2017-03-30T15:39:58Z

Hi Thomas: Subject to any further minor edits, I think I've done what I can with this, and have uploaded the latest edits.

As a java guy I'm gradually starting to get my head about some of the big differences with c++, but I'm still stymied on how to actually make the PID object declaration work. Looking forward to your insights as to how to best move forward.

This is interrelated with the RadToDeg and DegToRad conversions between the AutoPilotTask and the NavigationPage. Not sure how to implement the conversion in relation to the "&variable" references in the headingPID.

In addition, I'm not sure what happens next with respect to creating a pull request for the MFD functionality once we're finished (for now) with the autopilot. Do I create another branch? Or can I use the same branch to create another pull request once the autpilot request has been processed and I've uploaded the new MFD code?

Thanks for your patience as I try to figure all this out!

Rob

You can configure the orientation of KBox (IMU) and whether to use bat3 as a rudder input (adctask).

git-subtree-dir: lib/Arduino-PID-Library git-subtree-split: 5adeed52b04d4413002d8371ec1525040a143cf3

…no-PID-Library'

- do not run autopilot continuously but only every 250ms - and also fix some indentation issues

sarfata · 2017-04-01T08:31:13Z

Hey Rob, so I have made some changes that I have pushed. If you look at this pull request in github, you can see each of my commits and see what I have changed. Feel free to ask me questions about anything.

There is still a few things that I think we can improve to make the code easier to read and maintain in the future. I will add more comments, please tell me what you think.

sarfata

Hi Rob, I have added more comments now that I understand the code better and have tested it.

Let me know if you have questions or if you want me to do some of that.

sarfata · 2017-04-01T08:39:11Z

lib/KBox/src/KMessage.h

+class NAVMessage: public KMessage {
+  private:
+    bool apMode, apHeadingMode, apWaypointMode, apDodgeMode;
+    double currentHeading, targetHeading, courseToWaypoint;


I don't think the AutopilotTask should rely on currentHeading being provided by the NAVMessage. If the currently displayed page on KBox is not the navigation page, then those messages will not be sent and the autopilot will not get updates to the heading.

I think that instead, the autopilot should listen to IMUMessage and update its currentHeading based on IMUMessages.

Do you agree? If yes, I can do it - or you can do it. Tell me what you prefer.

Also, I don't think there is a difference between targetHeading and courseToWaypoint. I think they are both the same. As far as I can tell, courseToWaypoint is not used now and we should remove it so it's easier to read and understand the code.

Finally, I think your code would greatly benefit if you replaced your 4 mode variables with one enum that lists all the possible states of the autopilot. There are so many variables now that it is hard to verify that we are not missing a case in the if statements that do logic on them.

I can see your point on having the AutoPilotTask getting the current heading directly from the IMU message. I'll tackle that.

We can take out courseToWaypoint if you like. It was there as a placeholder for the next stage in the AP which would be adding the ability to navigate to a waypoint provided from a plotter. Which raises a whole other conversation ... probably should have it elsewhere, so I'll put it on the discussion list ... which is can we get a message back over WiFI from a mobile device running nav software ?

With respect to replacing the mode variables with an ENUM, I thought about that when you suggested it before and started to do it, but then I realized there were issues with that. Let's say we have 4 ENUMs that are mutually exclusive: OFF, HEADINGMODE, DODGEMODE, WAYPOINTMODE. Let's say we're in WAYPOINTMODE and we go into DODGEMODE to avoid a windsurfer. When we come out of DODGEMODE, how does KBOX know what mode to go into then? Does it go back to HEADINGMODE or WAYPOINTMODE? We wouldn't know unless we track another various like previousMode. Also, it means that when we go into DODGEMODE, KBOX can keep the nav calculations going for either HEADINGMODE or WAYPOINTMODE in the background and resume giving appropriate steering directions when dropping out of DODGEMODE. Given these scenarios, having thought about it, I decided it would be cleaner to have independent boolean variables than going with your ENUM suggestion. I'm happy with that decision, but would be interested if you think there is a cleaner solution now that you understand more about why I made the choice I did.

We can take out courseToWaypoint if you like. It was there as a placeholder for the next stage in the AP which would be adding the ability to navigate to a waypoint provided from a plotter.

I understand but I think it's best to keep the code as simple as possible. It will be easy enough to add it when we add support for waypoint. Right now, I think it just makes things more complicated to read and understand.

Which raises a whole other conversation ... probably should have it elsewhere, so I'll put it on the discussion list ... which is can we get a message back over WiFI from a mobile device running nav software ?

Not yet but I have been working on a big branch that has a ton of changes and that is one of the very important ones. Wifi and host will be able to exchange different type of messages (status of the connection, incoming frames in signalk or nmea format, etc).

With respect to replacing the mode variables with an ENUM, I thought about that when you suggested it before and started to do it, but then I realized there were issues with that. Let's say we have 4 ENUMs that are mutually exclusive: OFF, HEADINGMODE, DODGEMODE, WAYPOINTMODE.

What you are building here is a state machine. This is a perfect fit for what you are doing here but the risk when writing them is bugs in the transition from one state to the other (for example, forgetting to update one of the field on the display).
I will take a stab at rewriting this in a slightly different way and hopefully I can show you that it makes things easier to read and understand.

Can you help me understand what is DODGE MODE?

Is it a mode where we stop controlling the motors for X seconds so that the helmsman can dodge something else and then quickly get back to the previous setting without having to re-set the right heading?

Yes, its common on Autopilots. A seadoo is on a collision course and though you are tempted to hit it, you decide not to, go into dodge mode, manually steer for a few seconds to avoid, then go back out of dodge mode and resume the previous heading. Or a series of waves pushes you off course and you decide to give the AP a break, go into dodge mode, steer back onto the proper heading, then go back out of dodge mode to resume normal AP operation. Before going into Dodge mode we could have been in Heading Mode or Waypoint mode. In Dodge mode, we stop the AP controlling the rudder, although the device continues to calculate where the rudder should be.

I did some research on how autopilot are defined by other protocols.

In NMEA0183 the key sentence is APB which is sent by a GPS or a control unit and tells the pilot which heading to follow

Or RMB which is very similar. [Apparently that is the one OpenCPN uses](.

SignalK defines 8 possible "states": "auto", "standby", "alarm", "noDrift", "wind", "depthContour", "route", "directControl"

SignalK defines 3 possible "target" which are headingMagnetic, headingTrue, windAngleApparent

This SignalK plugin has NMEA2000 commands to control a Raymarine autopilot. The four supported states are auto/route/wind/standby.

There are a few things I draw from this research:

Most current autopilots do not do navigation calculation to a waypoint. They rely on an external device (GPS unit, PC, iPad, etc) to tell them what is the heading to destination and they follow it.

It would be really cool to display this information on KBox so that whether you have a human pilot or an autopilot, you could see on KBox how well you are doing towards your waypoint, whether you are on course and how far you are.

The way to "dodge" with a traditional autopilot is to press -10/+10 (see this discussion). This could be done by just turning the rotary button on KBox.

We could have an extra "follow waypoint" feature where KBox would be smart enough to be both the autopilot control head and the navigation system. We would have to include also detecting when you get in the arrival circle or the perpendicular of the waypoint which I think they do by keeping the original heading to target. If we have this feature, dodge makes a lot of sense.

So coming back to our current pull request :) All this research helped me realize that what we are doing here is "just" the autopilot head control. Not the navigation part. I agree we want to do the navigation part at some point, but to do that we will need to be able to parse incoming messages to get current position and position of a target waypoint. Let's build things one step at a time.

So this is what I suggest and will push in a different branch for you to review:

Rename NavigationPage to AutopilotControlPage, it will be dedicated to controlling an Autopilot from KBox.

Remove reference to waypoint for now, focus on having a targetMagneticHeading and building an autopilot that can follow it.

I can guarantee from my operational experience with autopilots that having a dodgemode that just changes the targetheading but maintains control of the steering would be a bad idea. Some autopilots have a "powersteering" mode that behaves similar to how you describe press -10/+10. I would not use an autopilot that did not give me a way to temporarily deactivate the steering commands.

And yes, just maintaining a set heading was my initial intention for version 1. I thought it might he helpful to others and a hint to ourselves to leave some breadcrumbs in the code for future intended features like getting a heading to a waypoint, but that does not seem to be your preference. The name of the page is immaterial.

sarfata · 2017-04-01T08:41:02Z

lib/KBox/src/pages/NavigationPage.cpp

+/*
+  The MIT License
+
+  Copyright (c) 2016 Thomas Sarlandie [email protected]


You should put your name here ;)

sarfata · 2017-04-01T08:45:42Z

lib/KBox/src/pages/NavigationPage.cpp

+
+    rawHeading = RadToDeg(imu.getCourse());
+    if (rawHeading < 0){
+        navCurrentHeading = 720 + rawHeading;//adding 720 for nav math purposes


This part of the code is pretty hard to understand and will be hard to debug.

Some ideas to make things easier here:

Write a little utility function formatAngle() that takes an angle in radian and returns a value in degrees between 0 and 360. Then reuse this everywhere you want to display an angle. You could have two if you want one that returns something between 0 and 360 and another one that returns values between -180 and +180.

Don't do navigation math in the navigation page. If the navigation page is not displayed then this code will not get executed. The autopilot needs to do all the navigation math. (If you follow my advice above to remove navCurrentHeading from NAVMessage then you will not need that math here and this function will just have to worry about displaying things.

Use only radians for the navigation math. You should only convert radians to degrees when displaying to the user.

I'll take a look at that approach. As you know this is all new to me, but I'm feeling like it would have been a bit more constructive to get design suggestions like this a bit earlier in the review process: I'm now starting to feel like I'm going backward.

So have thought a bit more about this. I can see the logic for moving the currentHeading / offCourse calculations into the autoPilotTask. We still need to determine targetHeading in the navPage because it depends on encoder events that are only detected in the navPage. So we still need to do the targetHeading math in the Nav page.

I'm not sold on doing all the math in radians. Everybody using this code is going to need to do some customization for their particular boat in determining what adjustments to use for MAXRUDDERSWING, deadzone, slack and the PID variables. I don't think anybody will want to try and figure out what their MAXRUDDERSWING is in radians, or try and tune the AP performance based on radians. I think that would be counterintuitive for just about everybody. So I don't see any practical advantage to doing all the calculations in radians. Am I missing something?

I'll go ahead and move the currentHeading calcs into the AutoPilot, but will hold off on doing everything in radians, pending a persuasive argument why that has practical, rather than purely theoretical benefits.

sarfata · 2017-04-01T08:47:18Z

lib/KBox/src/pages/NavigationPage.cpp

+    headingDisplay->setText(formatMeasurement(navCurrentDisplayHeading, ""));
+    headingDisplay->setColor(colorForCourse(navCurrentDisplayHeading));
+
+    if (apMode == true && apHeadingMode == true ){


If you make apMode an enum then you can replace all those if() by one switch (apMode) statement. It will be much easier to read.

Have already explained my logic for the independent booleans

sarfata · 2017-04-01T08:48:25Z

lib/KBox/src/pages/NavigationPage.cpp

+        targetHeadingDisplay->setColor(colorForCourse(navTargetDisplayHeading));
+
+        //following code recalculates navCurrentHeading based on "offcourse" value to avoid non-linear heading numbers across 180 degrees
+        navOffCourse = navCurrentDisplayHeading - navTargetDisplayHeading;


It looks like navOffCourse is used only in this function so it does not need to be declared in the class. You can just declare it in this function.

sarfata · 2017-04-01T08:53:05Z

Oh and for the MFDPage you can work on another branch. Make sure you start it from master (so you will not have the autopilot for now) and we can start discussing it in parallel.

matrixlinks · 2017-04-01T14:18:57Z

Thanks for the clarification on how to move the MFD stuff forward, I have a couple of technical challenges I don't yet know now to solve. Once I've made the next round of AP updates, I'll create a separate branch, clean up the MFD code, and then do a pull request and inside the code I will have some comments to clarify the issues I'm struggling with.

Thanks!

matrixlinks · 2017-04-01T20:47:33Z

Hi Thomas: Yes, its common on Autopilots. A seadoo is on a collision course and though you are tempted to hit it, you decide not to, go into dodge mode, manually steer for a few seconds to avoid, then go back out of dodge mode and resume the previous heading. Or a series of waves pushes you off course and you decide to give the AP a break, go into dodge mode, steer back onto the proper heading, then go back out of dodge mode to resume normal AP operation. Before going into Dodge mode we could have been in Heading Mode or Waypoint mode. In Dodge mode, we stop the AP controlling the rudder, although the device continues to calculate where the rudder should be.

…

On Apr 1, 2017, at 16:41, Thomas Sarlandie ***@***.***> wrote: @sarfata commented on this pull request. In lib/KBox/src/KMessage.h: > @@ -191,6 +196,94 @@ class IMUMessage: public KMessage { }; }; +class NAVMessage: public KMessage { + private: + bool apMode, apHeadingMode, apWaypointMode, apDodgeMode; + double currentHeading, targetHeading, courseToWaypoint; Can you help me understand what is DODGE MODE? Is it a mode where we stop controlling the motors for X seconds so that the helmsman can dodge something else and then quickly get back to the previous setting without having to re-set the right heading? — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub, or mute the thread.

sarfata · 2017-04-03T00:41:47Z

lib/KBox/src/tasks/AutoPilotTask.cpp

+  headingPID.Compute();
+
+  apRudderCommandSent = apTargetRudderPosition; //by default send rudder command to equal target position, then modify it
+  double testDegs = apTargetRudderPosition - apRudderCommandSent;


I don't understand these two lines. The way they are right now, it seems that apRudderCommandSent will always be equal to apTargetRudderPosition, and so apTargetRudderPosition - apRudderCommandSent will always be 0 and the following if block is never used.

I am guessing you meant something else here.

I think line 77 was originally intended to be in the setup function. I moved it there and everything still functions properly.

sarfata · 2017-04-03T00:49:47Z

lib/KBox/src/tasks/AutoPilotTask.cpp

+}
+
+void AutoPilotTask::loop() {
+  headingPID.Compute();


Do you think the PID is smart enough to understand that to go from 358 to 005, it needs to go right and not left? I think this will require some special attention in the PID algorithm, or maybe some pre-processing of the inputs.

You should know from testing the AP code on KBOX that it works! That's the magic of the math approach that Robert Huitema developed. Circular compass math is converted to to linear math by adding 720 and defining the heading data that feeds the PID function as targetheading plus offcourse. If you set DEBUG on the numbers being fed into the PID you'll see how this works. In effect, the inputs are already "preprocessed" as you suggest.

Have taken a brief closer look at moving the math functions from the current Nav page into the AP task. That's going to take more time than I have available for the next few days. I'll look again later in the week. I'm not entirely sure at this point whether the practical benefits of doing so are worth the effort, particularly as I don't think we can entirely eliminate the math in the Nav page in any event.

6663acd2 fix PDB typos 1e2e147b fix ADC_NUM e79cc582 don't use defines for PDB dab72841 add average test to adc_test 65b60963 add test_compare to adc_test bcaef855 test ADC library 25f2a270 Merge pull request #28 from pedvide/namespace_ADC_Error a5d8c39e fix resetError b1e01afd move error functions into namespace 8d7d5aba Merge pull request #27 from pedvide/simple_ADCK_speeds fc4bce18 added ADC_CFG1_MED_SPEED 74dab20c simplify speed definitions db500416 delete old defines 672e6119 calulate the ADCK speeds automatically for F_BUS 751d9e2d add 30MHz speed for FBUS=60Mhz 3de86672 fix documentation for ADC_ERROR enum 933e11aa Merge remote-tracking branch 'refs/remotes/origin/ADC_ERROR_enums' 87b96221 fix readPin example for Teensys with 1 ADC module 91222e0e fix readPin example for Teensys with 1 ADC daaa3522 fix =| and &| operators, now it works c794b7fa change #defines to enum 8c9b1f0a fix typo cd74ce74 Merge remote-tracking branch 'refs/remotes/origin/external_Atomic' 05d20ccd documentation 0cb3c63d documentation 8306eed9 change initialization in ADC_Module 21405717 Don't use VREF in Teensy LC f26f94d9 simplify *Flag methods for Teensy LC db1a0aa8 update methods for Teensy LC 836d689d bitband_address returns a reference 8e3f9eeb optimize internal ADC variables 540a60f2 optimize internal ADC variables 2ad1270f revert back external atomic bug 28f018f7 make consts fd34dde3 check interrupts bit before starting measurement 46648b23 use refs to the registers and atomic methods 367e3e6e use references to registers in al methods 370b112e use atomic methods in VREF 298d6b6b Use kinets.h flags. templated the atomic methods for KINETISK. 09c9c8e1 Revert "first attempt at atomic VREF" c343a3d3 first attempt at atomic VREF af6ba4fc Merge remote-tracking branch 'refs/remotes/origin/master' into external_Atomic f5db12e6 Revert "add isOn method to VREF" 86dd5045 add isOn method to VREF a0919307 add differential and single-ended methods ed543618 update documentation aa1009e1 add isOn for VREF 9adc4a71 external file for atomic bit manipulation af285555 Merge remote-tracking branch 'refs/remotes/origin/master' into external_Atomic 4254b269 Merge remote-tracking branch 'refs/remotes/origin/VREF' 2d332c1b use external atomic functions 34c09754 Merge remote-tracking branch 'refs/remotes/origin/master' into VREF 59d8ccb1 add VREF keyword 0772c18c Merge remote-tracking branch 'refs/remotes/origin/master' into external_Atomic eec9da73 add pins A25, A26 for Teensy 3.5 2a4abefc Merge remote-tracking branch 'refs/remotes/origin/master' into external_Atomic 03a93b7f Create LICENSE.md 5844ea10 disable ADC_INTERNAL_SOURCE::VREF for Teensy LC 89f28acf works with Teensy 3.0 too b5078a96 add VREF example, add trim function 361aa310 fix readPin example 2970ce7d add getPDBFrequency method 8b22509a change VREF from a class to a namespace b73d5faf use VREF in external file 85ede054 Revert "Revert "Revert "add VREF file""" 090a7c4f Revert "Revert "add VREF file"" 4969713e Revert "add VREF file" 5000b704 add VREF file 1ab71a63 add resetError method and fix ADC speeds for T3.6 70dd0ee1 add buffer power mode to start internal reference ee10faf5 make start/stopInternalReference public 0cc2f1c2 add library.properties 712ce31e Merge pull request #22 from KurtE/T3.6-fixes f49331bd T3.5/6 - Fix IRQ_ADC1 issue c7a89dea Fix T3.5/6 channel to pix tables a3cef196 fix another error with merge 96d9a0f7 fix some force merge errors 8f511fe0 Merge remote-tracking branch 'refs/remotes/origin/dev' 36f8e48b T3.5/3.6: added pins A23, A24 a072923f fix Teensy 3.5 and 3.6 differential 89afccbb documentation link 76d433c4 ignore docs for language detection 857602cf github language detector ignore docs 9a8da989 Create master branch via GitHub df7845c0 use class enums for settings 2f7b11c8 documentation link 7d0c906f Merge pull request #16 from aforren1/rename d3ac0ff1 Replace 3.4 -> 3.5 and 3.5 -> 3.6 f66c970e Merge pull request #13 from valeros/patch-1 0feeef21 @platformio Library Registry manifest file 724f938e fix for A11 in T3.5 b0ce5f96 documentation update b5ad8aac support for Teensy 3.4/3.5 24e0743e 16 synch differential bug 807906b1 ADC_0 will be selected when in doubt git-subtree-dir: lib/ADC git-subtree-split: 6663acd226b16b61b189c5e69fba38a0dcc0a862

matrixlinks added 16 commits March 21, 2017 14:20

Add files via upload

ea9ace0

Adding autopilot functionality

Delete main.cpp

3b0ed7d

Add autopilot functionality

cef86c2

Adding autopilot functionality

bdae883

Adding autopilot functionality

3c3720a

Adding autopilot functionality

9a17bd6

Adding autopilot functionality

49afcf6

Added variable to configure KBOX orientation

e827589

Warning when IMU not configured

db87e76

Rudder sensor input on bat3

f74058c

Create test

8d32f7a

Delete test

37f5edd

Create test

84e2047

Adding PID library

3c0fb54

Delete test

4157c50

Adds autopilot functionality

a10acb1

sarfata requested changes Mar 22, 2017

View reviewed changes

matrixlinks added 11 commits March 23, 2017 18:05

Add RudderSensor task

222e808

Reformat IMU message generation

534ea02

Improve autopilot functionality

b14cb22

Suppressed debug message

32a19b0

Added RudderSensor task

4ca6c68

Format fixes and add RudderSensor message

7f13d34

Add RudderSensor task

3759b36

Add RudderSensor task

af382de

Wrong location

f13b9f4

File in wrong location

4a7b4ce

Update Navigation page

3002738

sarfata requested changes Mar 30, 2017

View reviewed changes

matrixlinks added 9 commits March 30, 2017 10:55

Removed RudderSensor task

dd59141

Removed RudderSensorTask (relocated to ADCTask)

d11ed62

Reload correct file

5fce3c3

Various minor edits

b6935c8

Move IMUSequence inside function

1558573

Add RudderSensor functionality to ADCTask

839101e

Delete RudderSensorTask.cpp

0cb29fd

Delete RudderSensorTask.h

11e8794

Use MAXRUDDERSWING to set default rudder position

d55348d

sarfata added 6 commits April 1, 2017 00:09

Make ADCTask and IMUTask use const class variables for configuration

52a7115

You can configure the orientation of KBox (IMU) and whether to use bat3 as a rudder input (adctask).

Removing Arduino-PID-Library to re-add it as a subtree

9aa8f6d

Squashed 'lib/Arduino-PID-Library/' content from commit 5adeed5

785ed14

git-subtree-dir: lib/Arduino-PID-Library git-subtree-split: 5adeed52b04d4413002d8371ec1525040a143cf3

Merge commit '785ed147204832fecd5a5405f7d2d0b4f68b796e' as 'lib/Ardui…

7dbe35a

…no-PID-Library'

Finalizing adding the Arduino PID Lib as a subtree

039985b

Move headingPID inside AutoPilotTask

aeba2ff

- do not run autopilot continuously but only every 250ms - and also fix some indentation issues

fix indentation

ce0b4b9

sarfata changed the title ~~Rigm edits~~ Add navigation page and autopilot task Apr 1, 2017

sarfata requested changes Apr 1, 2017

View reviewed changes

sarfata reviewed Apr 3, 2017

View reviewed changes

sarfata changed the base branch from master to kbox-v0 December 10, 2017 05:26


		autoPilotOffCourse = apTargetHeading - apCurrentHeading; //variable not currently used for anything

		autoPilotDeadZone = 5; //pending tuning on water


		};

		class RUDMessage: public KMessage { //RIGM added for autopilot functionality

Add navigation page and autopilot task #28

Are you sure you want to change the base?

Add navigation page and autopilot task #28

Conversation

matrixlinks commented Mar 21, 2017

sarfata left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sarfata commented Mar 23, 2017 via email

matrixlinks commented Mar 23, 2017 via email

sarfata commented Mar 30, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

matrixlinks commented Mar 30, 2017

sarfata commented Apr 1, 2017

sarfata left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sarfata Apr 1, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

matrixlinks Apr 3, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sarfata Apr 1, 2017 •

edited

Loading

matrixlinks Apr 3, 2017 •

edited

Loading