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/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 <br3ttb@gmail.com> brettbeauregard.com
+ *
+ * This Library is licensed under a GPLv3 License
+ **********************************************************************************************/
+
+#if ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#include <PID_v1.h>
+
+/*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..77b3e4b5
--- /dev/null
+++ b/lib/Arduino-PID-Library/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/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 <br3ttb@gmail.com> 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/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/lib/Arduino-PID-Library/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino
new file mode 100644
index 00000000..94f3faf5
--- /dev/null
+++ b/lib/Arduino-PID-Library/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 <PID_v1.h>
+
+#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/lib/Arduino-PID-Library/examples/PID_Basic/PID_Basic.ino b/lib/Arduino-PID-Library/examples/PID_Basic/PID_Basic.ino
new file mode 100644
index 00000000..8028b588
--- /dev/null
+++ b/lib/Arduino-PID-Library/examples/PID_Basic/PID_Basic.ino
@@ -0,0 +1,35 @@
+/********************************************************
+ * PID Basic Example
+ * Reading analog input 0 to control analog PWM output 3
+ ********************************************************/
+
+#include <PID_v1.h>
+
+#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/lib/Arduino-PID-Library/examples/PID_RelayOutput/PID_RelayOutput.ino b/lib/Arduino-PID-Library/examples/PID_RelayOutput/PID_RelayOutput.ino
new file mode 100644
index 00000000..17fbe1a3
--- /dev/null
+++ b/lib/Arduino-PID-Library/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 <PID_v1.h>
+
+#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/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=* 
diff --git a/lib/KBox/src/KBox.h b/lib/KBox/src/KBox.h
index 956b1505..35219c81 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"
 
 /* Converters */
 #include "converters/VoltageN2kConverter.h"
@@ -51,6 +52,7 @@
 /* Pages */
 #include "pages/BatteryMonitorPage.h"
 #include "pages/StatsPage.h"
+#include "pages/NavigationPage.h"
 
 /* Other dependencies */
 #include <Adafruit_NeoPixel.h>
diff --git a/lib/KBox/src/KMessage.h b/lib/KBox/src/KMessage.h
index 59651b85..1686033e 100644
--- a/lib/KBox/src/KMessage.h
+++ b/lib/KBox/src/KMessage.h
@@ -1,3 +1,4 @@
+
 /*
   The MIT License
 
@@ -34,6 +35,9 @@ class BarometerMeasurement;
 class VoltageMeasurement;
 class NMEA2000Message;
 class IMUMessage;
+class NAVMessage;
+class APMessage;
+class RUDMessage;
 
 class KMessage {
   private:
@@ -43,7 +47,6 @@ class KMessage {
     virtual void accept(KVisitor& v) const {};
 };
 
-
 class KVisitor {
   public:
     virtual void visit(const NMEASentence &) {};
@@ -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 &) {};
+    virtual void visit(const RUDMessage &) {};
 };
 
 class NMEASentence : public KMessage {
@@ -151,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);
@@ -163,14 +168,14 @@ class IMUMessage: public KMessage {
     };
 
     /*
-     * Heading in Radians
+     * Heading in Radians: course value are compass (magnetic) values not adjusted for variation
      */
     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
      */
     double getYaw() const {
       return yaw;
@@ -191,6 +196,94 @@ class IMUMessage: public KMessage {
     };
 };
 
+class NAVMessage: public KMessage {
+  private:
+    bool apMode, apHeadingMode, apWaypointMode, apDodgeMode;
+    double currentHeading, targetHeading, courseToWaypoint;
+
+  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);
+    };
+
+    bool getApMode() const {
+      return apMode;
+    };
+
+    bool getApHeadingMode() const {
+      return apHeadingMode;
+    };
+
+    bool getApWaypointMode() const {
+      return apWaypointMode;
+    };
+
+    bool getApDodgeMode() const {
+      return apDodgeMode;
+    };
+
+    /*
+     * Headings in Degrees for messages passed back and forth between the Autopilot task and the Nav page
+     */
+    double getCurrentHeading() const {
+      return currentHeading;
+    };
+
+    double getTargetHeading() const {
+      return targetHeading;
+    };
+
+    double getCourseToWaypoint() const {
+      return courseToWaypoint;
+    };
+};
+
+class APMessage: public KMessage {
+  private:
+    double targetRudderPosition, rudderCommandSent;
+
+  public:
+    APMessage(double targetRudderPosition, double rudderCommandSent) : targetRudderPosition(targetRudderPosition),rudderCommandSent(rudderCommandSent){};
+
+    void accept(KVisitor &v) const {
+      v.visit(*this);
+    };
+
+    double getTargetRudderPosition() const {
+      return targetRudderPosition;
+    };
+
+    double getRudderCommandSent() const {
+      return rudderCommandSent;
+    };
+
+};
+
+class RUDMessage: public KMessage {
+  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:
     virtual void processMessage(const KMessage&) = 0;
diff --git a/lib/KBox/src/KMessageNMEAVisitor.cpp b/lib/KBox/src/KMessageNMEAVisitor.cpp
index ae154b14..afaca3b9 100644
--- a/lib/KBox/src/KMessageNMEAVisitor.cpp
+++ b/lib/KBox/src/KMessageNMEAVisitor.cpp
@@ -25,6 +25,11 @@
 #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
 
 void KMessageNMEAVisitor::visit(const NMEASentence& s) {
   nmeaContent += s.getSentence() + "\r\n";
@@ -64,6 +69,35 @@ void KMessageNMEAVisitor::visit(const NMEA2000Message &n2km) {
 }
 
 void KMessageNMEAVisitor::visit(const IMUMessage &imu) {
+    unsigned int _imuSequence = 1;
+    
+#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 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 output viewed on other devices.  Needs further exploration to find the cause
+    } 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);
+    
+#else
+    
   NMEASentenceBuilder sb("II", "XDR", 16);
   sb.setField(1, "A");
   sb.setField(2, imu.getYaw(), 1);
@@ -92,5 +126,7 @@ void KMessageNMEAVisitor::visit(const IMUMessage &imu) {
   sb2.setField(2, "M");
 
   nmeaContent += sb2.toNMEA() + "\r\n";
+#endif
+
 }
 
diff --git a/lib/KBox/src/pages/NavigationPage.cpp b/lib/KBox/src/pages/NavigationPage.cpp
new file mode 100644
index 00000000..d4bfab6a
--- /dev/null
+++ b/lib/KBox/src/pages/NavigationPage.cpp
@@ -0,0 +1,280 @@
+/*
+  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 <stdio.h>
+#include "KBoxDebug.h"
+
+
+NavigationPage::NavigationPage() {
+  static const int col1 = 5;
+  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 (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;
+        }
+    }
+    //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;
+    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), "%.0f %s  ", measure, unit); //do not display decimals for headings
+  return String(s);
+}
+
+
+void NavigationPage::processMessage(const KMessage &message) {
+  message.accept(*this);
+}
+
+void NavigationPage::visit(const APMessage &ap) {
+    navTargetRudderPosition = ap.getTargetRudderPosition();
+    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){
+        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));
+        
+        //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 command - 32 as negative number
+            rudderCommandForDisplay = (navTargetRudderPosition - 32) * -1 ; //rounding anomaly to avoid displaying -0
+        }
+        rudderCommandDisplay->setText(formatMeasurement(rudderCommandForDisplay, ""));
+        rudderCommandDisplay->setColor(colorForRudder(rudderCommandForDisplay));
+        
+        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, "|"));
+            }
+        } 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; //reset variables
+       navEncoderClicks = 0;
+        
+       targetHeadingDisplay->setText("----  ");
+       targetHeadingDisplay->setColor(ColorWhite);
+        
+        rudderCommandDisplay->setText("----  ");
+        rudderCommandDisplay->setColor(ColorWhite);
+        
+        rudderPositionDisplay->setText(formatMeasurement(navRudderSensorPosition, ""));
+        rudderPositionDisplay->setColor(colorForRudder(navRudderSensorPosition));
+    }
+    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
new file mode 100644
index 00000000..8d37be57
--- /dev/null
+++ b/lib/KBox/src/pages/NavigationPage.h
@@ -0,0 +1,77 @@
+/*
+  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"
+
+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;
+    
+    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 RUDMessage&);
+    bool processEvent(const ButtonEvent &be);
+    bool processEvent(const EncoderEvent &ee);
+};
diff --git a/lib/KBox/src/tasks/ADCTask.cpp b/lib/KBox/src/tasks/ADCTask.cpp
index 137d43ca..fbc10491 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"
@@ -32,23 +33,50 @@ 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);
 
   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();
-
-  //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) {
+    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);
+  }
 }
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/AutoPilotTask.cpp b/lib/KBox/src/tasks/AutoPilotTask.cpp
new file mode 100644
index 00000000..1eee3092
--- /dev/null
+++ b/lib/KBox/src/tasks/AutoPilotTask.cpp
@@ -0,0 +1,132 @@
+/*
+  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 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 excellent FreeboardPLC_v1_2 code. RIGM
+ */
+
+#include "AutoPilotTask.h"
+#include "../pages/NavigationPage.h"
+#include "KBoxDebug.h"
+#include "../drivers/board.h"
+#include "KMessage.h"
+#include <NMEA2000.h>
+#include <N2kMessages.h>
+
+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)
+{
+
+}
+
+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();
+}
+
+void AutoPilotTask::visit(const RUDMessage &rm) {
+  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() {
+  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) {
+        //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");
+    }
+  }
+}
+
+
+
+
+
+
+
+
diff --git a/lib/KBox/src/tasks/AutoPilotTask.h b/lib/KBox/src/tasks/AutoPilotTask.h
new file mode 100644
index 00000000..a457618a
--- /dev/null
+++ b/lib/KBox/src/tasks/AutoPilotTask.h
@@ -0,0 +1,61 @@
+/*
+  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 "TaskManager.h"
+#include "KMessage.h"
+#include <PID_v1.h>
+
+//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
+#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:
+    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();
+    void processMessage(const KMessage& message);
+    void visit(const NAVMessage&);
+    void visit(const RUDMessage&);
+    void setup();
+    void loop();
+};
diff --git a/lib/KBox/src/tasks/IMUTask.cpp b/lib/KBox/src/tasks/IMUTask.cpp
index 28008e3b..0bea95f6 100644
--- a/lib/KBox/src/tasks/IMUTask.cpp
+++ b/lib/KBox/src/tasks/IMUTask.cpp
@@ -42,31 +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());
-    sendMessage(m);
+  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;
   }
-  
-  //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());
-  //}
+  IMUMessage m(sysCalib, DegToRad(eheading), /* yaw?*/ 0, DegToRad(epitch), DegToRad(eroll));
+  sendMessage(m);
 }
diff --git a/lib/KBox/src/tasks/IMUTask.h b/lib/KBox/src/tasks/IMUTask.h
index 0483e367..fc76a8e3 100644
--- a/lib/KBox/src/tasks/IMUTask.h
+++ b/lib/KBox/src/tasks/IMUTask.h
@@ -24,6 +24,7 @@
 #include <Adafruit_BNO055.h>
 #include "TaskManager.h"
 #include "KMessage.h"
+#include "NMEA2000Task.h" //RIGM added
 
 class IMUTask : public Task, public KGenerator {
   private:
@@ -31,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();
diff --git a/lib/KBox/src/tasks/NMEA2000Task.cpp b/lib/KBox/src/tasks/NMEA2000Task.cpp
index bad00439..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) {
@@ -113,6 +114,8 @@ void NMEA2000Task::visit(const IMUMessage &m) {
     sendN2kMessage(n2kmessage);
 
     _imuSequence++;
+  } else {
+      // DEBUG("IMU is not calibrated: move the unit around to calibrate it");
   }
 }
 
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:
 
diff --git a/src/host/main.cpp b/src/host/main.cpp
index 268d0378..b8dbb30b 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);
   imuTask->connectTo(*n2kTask);
 
   BarometerTask *baroTask = new BarometerTask();
@@ -87,6 +88,11 @@ void setup() {
   baroTask->connectTo(*sdcardTask);
   imuTask->connectTo(*sdcardTask);
 
+  AutoPilotTask *autoPilotTask = new AutoPilotTask();
+  autoPilotTask->connectTo(*wifi);
+  autoPilotTask->connectTo(*n2kTask);
+  adcTask->connectTo(*autoPilotTask);
+
   // Add all the tasks
   kbox.addTask(new IntervalTask(new RunningLightTask(), 250));
   kbox.addTask(new IntervalTask(adcTask, 1000));
@@ -97,6 +103,14 @@ void setup() {
   kbox.addTask(reader2);
   kbox.addTask(wifi);
   kbox.addTask(sdcardTask);
+  kbox.addTask(new IntervalTask(autoPilotTask, AUTOPILOT_SAMPLE_TIME));
+
+  NavigationPage *navPage = new NavigationPage();
+  imuTask->connectTo(*navPage);
+  adcTask->connectTo(*navPage);
+  autoPilotTask->connectTo(*navPage);
+  kbox.addPage(navPage);
+  navPage->connectTo(*autoPilotTask);
 
   BatteryMonitorPage *batPage = new BatteryMonitorPage();
   adcTask->connectTo(*batPage);
@@ -113,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();