From 23329612ea0f9f6c9775e147c275610cb82031dc Mon Sep 17 00:00:00 2001
From: BrianAdams <brian@babastik.com>
Date: Thu, 19 Nov 2015 12:47:48 -0800
Subject: [PATCH 01/22] Initial port to 30.0.3

---
 OpenROV/AConfig.h           |   8 +-
 OpenROV/CThrusters_2Xv2.cpp | 414 ++++++++++++++++++++++++++++++++++++
 2 files changed, 419 insertions(+), 3 deletions(-)
 create mode 100644 OpenROV/CThrusters_2Xv2.cpp

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index 67c4ef5..781bce3 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -33,12 +33,14 @@
 #define HAS_ALT_SERVO (1)
 #define DEADMANSWITCH_ON (1)
 
-// Thrusters configurations
+// Thrusters configurations  (These appear depricated and replaced by the THRUSTER_CONFIGURATION below)
 #define THRUSTER_CONFIG_NONE (0)
-#define THRUSTER_CONFIG_2X1 (1)
+#define THRUSTER_CONFIG_2X1 (0)
+#define THRUSTER_CONFIG_2Xv2 (1)
 
 // Selected Thruster Configuration
-#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1
+//#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1
+#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2Xv2
 
 
 // ---------------------------------------------------------
diff --git a/OpenROV/CThrusters_2Xv2.cpp b/OpenROV/CThrusters_2Xv2.cpp
new file mode 100644
index 0000000..288dd7d
--- /dev/null
+++ b/OpenROV/CThrusters_2Xv2.cpp
@@ -0,0 +1,414 @@
+#include "AConfig.h"
+#if( THRUSTER_CONFIGURATION == THRUSTER_CONFIG_2Xv2 )
+
+// Includes
+#include "CThrusters.h"
+#include "NConfigManager.h"
+#include "NDataManager.h"
+#include "CMotor.h"
+#include "CTimer.h"
+#include "CPin.h"
+
+#if(HAS_STD_CAPE)
+    #include "CCape.h"
+#endif
+
+#if(HAS_OROV_CONTROLLERBOARD_25)
+    #include "CControllerBoard.h"
+#endif
+
+
+// Static variable initialization
+const int CThrusters::kMotorCount = 4;
+
+namespace
+{
+    CMotor port_motor( PORT_PIN );
+    CMotor port_vertical_motor( VERTICAL_PIN );
+    CMotor starboard_motor( STARBOARD_PIN );
+    CMotor starboard_vertical_motor( VERTICAL_PIN );
+
+    int new_p	= MOTOR_TARGET_NEUTRAL_US;
+    int new_s	= MOTOR_TARGET_NEUTRAL_US;
+    int new_vp = MOTOR_TARGET_NEUTRAL_US;
+    int new_vs = MOTOR_TARGET_NEUTRAL_US;
+    int new_st = 0; //strafe differential
+    int p = MOTOR_TARGET_NEUTRAL_US;
+    int vp = MOTOR_TARGET_NEUTRAL_US;
+    int vs = MOTOR_TARGET_NEUTRAL_US;
+    int vp2 = MOTOR_TARGET_NEUTRAL_US;
+    int vs2 = MOTOR_TARGET_NEUTRAL_US;
+    int s = MOTOR_TARGET_NEUTRAL_US;
+    int st;
+
+    float trg_throttle,trg_yaw,trg_lift,trg_strafe;
+    int trg_motor_power;
+    int maxVtransDelta;
+
+
+    CTimer controltime;
+    CTimer thrusterOutput;
+    boolean bypasssmoothing;
+
+#ifdef ESCPOWER_PIN
+    bool canPowerESCs = true;
+    CPin escpower( "escpower", ESCPOWER_PIN, CPin::kDigital, CPin::kOutput );
+#else
+    boolean canPowerESCs = false;
+#endif
+}
+
+void CThrusters::Initialize()
+{
+    port_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    port_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    port_motor.Activate();
+
+    vertical_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    vertical_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    vertical_motor.Activate();
+
+    starboard_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    starboard_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    starboard_motor.Activate();
+
+    thrusterOutput.Reset();
+    controltime.Reset();
+
+    bypasssmoothing = false;
+
+    #ifdef ESCPOWER_PIN
+    escpower.Reset();
+    escpower.Write( 1 ); //Turn on the ESCs
+    #endif
+}
+
+void CThrusters::Update( CCommand& command )
+{
+    if( command.Equals( "mtrmod1" ) )
+    {
+        port_motor.m_positiveModifier = command.m_arguments[1] / 100;
+        port_vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
+        starboard_vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
+        starboard_motor.m_positiveModifier = command.m_arguments[3] / 100;
+    }
+
+    if( command.Equals( "mtrmod2" ) )
+    {
+        port_motor.m_negativeModifier = command.m_arguments[1] / 100;
+        port_vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
+        starboard_vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
+        starboard_motor.m_negativeModifier = command.m_arguments[3] / 100;
+    }
+
+    if( command.Equals( "rmtrmod" ) )
+    {
+        Serial.print( F( "mtrmod:" ) );
+        Serial.print( port_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_vertical_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_vertical_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( port_vertical_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_vertical_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_motor.m_negativeModifier );
+        Serial.println( ";" );
+    }
+
+    if( command.Equals( "go" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[2] > 999 && command.m_arguments[3] > 999 && command.m_arguments[1] < 2001 && command.m_arguments[2] < 2001 && command.m_arguments[3] < 2001 )
+        {
+            p = command.m_arguments[1];
+            vp = command.m_arguments[2];
+            vs = vp;
+            s = command.m_arguments[3];
+
+            if( command.m_arguments[4] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    if( command.Equals( "port" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
+        {
+            p = command.m_arguments[1];
+
+            if( command.m_arguments[2] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    if( command.Equals( "vertical" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
+        {
+            vp = command.m_arguments[1];
+            vs = vp;
+
+            if( command.m_arguments[2] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    if( command.Equals( "starboard" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
+        {
+            s = command.m_arguments[1];
+
+            if( command.m_arguments[2] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    if( command.Equals( "thro" ) || command.Equals( "yaw" ) )
+    {
+        if( command.Equals( "thro" ) )
+        {
+            if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
+            {
+                trg_throttle = command.m_arguments[1] / 100.0;
+            }
+        }
+
+        if( command.Equals( "yaw" ) )
+        {
+            //ignore corrupt data
+            if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 ) //percent of max turn
+            {
+                trg_yaw = command.m_arguments[1] / 100.0;
+            }
+        }
+
+        // The code below spreads the throttle spectrum over the possible range
+        // of the motor. Not sure this belongs here or should be placed with
+        // deadzon calculation in the motor code.
+        if( trg_throttle >= 0 )
+        {
+            p = 1500 + ( 500.0 / abs( port_motor.m_positiveModifier ) ) * trg_throttle;
+            s = p;
+        }
+        else
+        {
+            p = 1500 + ( 500.0 / abs( port_motor.m_negativeModifier ) ) * trg_throttle;
+            s = p;
+        }
+
+        trg_motor_power = s;
+
+        int turn = trg_yaw * 250; //max range due to reverse range
+
+        if( trg_throttle >= 0 )
+        {
+            int offset = ( abs( turn ) + trg_motor_power ) - 2000;
+
+            if( offset < 0 )
+            {
+                offset = 0;
+            }
+
+            p = trg_motor_power + turn - offset;
+            s = trg_motor_power - turn - offset;
+        }
+        else
+        {
+            int offset = 1000 - ( trg_motor_power - abs( turn ) );
+
+            if( offset < 0 )
+            {
+                offset = 0;
+            }
+
+            p = trg_motor_power + turn + offset;
+            s = trg_motor_power - turn + offset;
+        }
+
+    }
+
+    if( command.Equals( "lift" ) )
+    {
+        if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
+        {
+            trg_lift = command.m_arguments[1] / 100.0;
+            v = 1500 + 500 * trg_lift;
+        }
+    }
+    if (command.Equals("lift") || command.Equals("strafe")  ){
+      if (command.Equals("lift")){
+        if (command.m_arguments[1]>=-100 && command.m_arguments[1]<=100) {
+          trg_lift = command.m_arguments[1]/100.0;
+
+          //the vertical component of the thrust factor is
+          //vThrust = sin(65)*thrust
+
+          //This spreads the percentage power over the fully available
+          //range of the thruster.  Since the current thrusters
+          //are less powerfull in the reverse direction and both motors
+          //should provide the same thrust, the maximum thurst is
+          //governed by the negative modifier of the motor.
+
+          //assume that port and starboard vertical thrusters
+          //have the same modifiers for the moment.
+          vp = 1500 + 500 * trg_lift;
+          vs = 1500 + 500 * trg_lift;
+          vp2 = vp;
+          vs2 = vs;
+          Serial.print("THR2XV2.vp1:");Serial.print(vp);Serial.println(";");
+          Serial.print("THR2XV2.vs1:");Serial.print(vs);Serial.println(";");
+        }
+      }
+
+      if (command.Equals("strafe")){
+        if (command.m_arguments[1]>=-100 && command.m_arguments[1]<=100) {
+          trg_strafe = command.m_arguments[1]/100.0;
+          //strafe (side motion) is limited to whatever thrust is still available
+          //from the vertical thruster range.  If vertical is full power,
+          //the strafe will be zero.
+          maxVtransDelta = abs((500.0/abs(port_vertical_motor.m_negativeModifier))*(1.0-abs(trg_lift)));
+          Serial.print("THR2XV2.mvd:");Serial.print(maxVtransDelta);Serial.println(";");
+          st = constrain( (500.0/abs(port_vertical_motor.m_negativeModifier))*trg_strafe,-maxVtransDelta,maxVtransDelta);
+          Serial.print("THR2XV2.st:");Serial.print(st);Serial.println(";");
+          //Adjust the vertrans thrusters with the ideal translate value
+          //if we go the wrong way... switch these.
+          Serial.print("THR2XV2.vp0:");Serial.print(vp);Serial.println(";");
+
+          vp2=vp+st;
+          vs2=vs-st;
+          Serial.print("THR2XV2.vp2:");Serial.print(vp2);Serial.println(";");
+          Serial.print("THR2XV2.vs2:");Serial.print(vs2);Serial.println(";");
+        }
+      }
+
+    }
+
+    #ifdef ESCPOWER_PIN
+    else if( command.Equals( "escp" ) )
+    {
+        escpower.Write( command.m_arguments[1] ); //Turn on the ESCs
+        Serial.print( F( "log:escpower=" ) );
+        Serial.print( command.m_arguments[1] );
+        Serial.println( ';' );
+    }
+
+    #endif
+    else if( command.Equals( "start" ) )
+    {
+        port_motor.Activate();
+        port_vertical_motor.Activate();
+        starboard_vertical_motor.Activate();
+        starboard_motor.Activate();
+    }
+    else if( command.Equals( "stop" ) )
+    {
+        p = MOTOR_TARGET_NEUTRAL_US;
+        vp = MOTOR_TARGET_NEUTRAL_US;
+        vs = MOTOR_TARGET_NEUTRAL_US;
+        s = MOTOR_TARGET_NEUTRAL_US;
+        // Not sure why the reset does not re-attach the servo.
+        //port_motor.stop();
+        //vertical_motor.stop();
+        //starboard_motor.stop();
+    }
+
+    #ifdef ESCPOWER_PIN
+    else if( ( command.Equals( "mcal" ) ) && ( canPowerESCs ) )
+    {
+        Serial.println( F( "log:Motor Callibration Staring;" ) );
+        //Experimental. Add calibration code here
+        Serial.println( F( "log:Motor Callibration Complete;" ) );
+    }
+
+    #endif
+
+    //to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
+    //to their new positions in increments.  The incriment should eventually be adjustable from the cockpit so that
+    //the pilot could have more aggressive response profiles for the ROV.
+    if( controltime.HasElapsed( 50 ) )
+    {
+        if (p!=new_p || vp2!=new_vp || vs2!=new_vs || s!=new_s) {
+        {
+            new_p = p;
+            new_vp = vp2;
+            new_vs = vs2;
+            new_s = s;
+
+            // Check to see if any motors are non-neutral to signal system that at least one motor is running
+            if( p != MOTOR_TARGET_NEUTRAL_US || vp != MOTOR_TARGET_NEUTRAL_US || vs != MOTOR_TARGET_NEUTRAL_US || s != MOTOR_TARGET_NEUTRAL_US )
+            {
+                NDataManager::m_thrusterData.MotorsActive = true;
+            }
+            else
+            {
+                NDataManager::m_thrusterData.MotorsActive = false;
+            }
+
+            Serial.print( F( "motors:" ) );
+            Serial.print( port_motor.SetMotorTarget( new_p ) );
+            Serial.print( ',' );
+            Serial.print( port_vertical_motor.SetMotorTarget( new_vp ) );
+            Serial.print( ',' );
+            Serial.print( starboard_vertical_motor.SetMotorTarget( new_vs ) );
+            Serial.print( ',' );
+            Serial.print( starboard_motor.SetMotorTarget( new_s ) );
+            Serial.println( ';' );
+        }
+
+    }
+
+    NDataManager::m_navData.FTHR = map( ( new_p + new_s ) / 2, 1000, 2000, -100, 100 );
+
+    //The output from the motors is unique to the thruster configuration
+    if( thrusterOutput.HasElapsed( 1000 ) )
+    {
+        Serial.print( F( "mtarg:" ) );
+        Serial.print( p );
+        Serial.print( ',' );
+        Serial.print( vp );
+        Serial.print( ',' );
+        Serial.print( vs );
+        Serial.print( ',' );
+        Serial.print( s );
+        Serial.println( ';' );
+        NDataManager::m_thrusterData.MATC = port_motor.IsActive() || port_motor.IsActive() || port_motor.IsActive();
+        Serial.print( F( "mtrmod:" ) );
+        Serial.print( port_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_vertical_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_vertical_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( port_vertical_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_vertical_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_motor.m_negativeModifier );
+        Serial.println( ";" );
+    }
+}
+
+#endif

From ad0a20088e178132c0b14cf23aa14623a1c00f51 Mon Sep 17 00:00:00 2001
From: BrianAdams <brian@babastik.com>
Date: Thu, 19 Nov 2015 13:04:43 -0800
Subject: [PATCH 02/22] Fixes to get it to compile

---
 OpenROV/CThrusters_2Xv2.cpp | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/OpenROV/CThrusters_2Xv2.cpp b/OpenROV/CThrusters_2Xv2.cpp
index 288dd7d..09bb8e2 100644
--- a/OpenROV/CThrusters_2Xv2.cpp
+++ b/OpenROV/CThrusters_2Xv2.cpp
@@ -64,9 +64,13 @@ void CThrusters::Initialize()
     port_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
     port_motor.Activate();
 
-    vertical_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
-    vertical_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
-    vertical_motor.Activate();
+    port_vertical_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    port_vertical_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    port_vertical_motor.Activate();
+
+    starboard_vertical_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    starboard_vertical_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    starboard_vertical_motor.Activate();
 
     starboard_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
     starboard_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
@@ -251,7 +255,8 @@ void CThrusters::Update( CCommand& command )
         if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
         {
             trg_lift = command.m_arguments[1] / 100.0;
-            v = 1500 + 500 * trg_lift;
+            vp = 1500 + 500 * trg_lift;
+            vs = vp;
         }
     }
     if (command.Equals("lift") || command.Equals("strafe")  ){
@@ -346,7 +351,7 @@ void CThrusters::Update( CCommand& command )
     //the pilot could have more aggressive response profiles for the ROV.
     if( controltime.HasElapsed( 50 ) )
     {
-        if (p!=new_p || vp2!=new_vp || vs2!=new_vs || s!=new_s) {
+        if (p!=new_p || vp2!=new_vp || vs2!=new_vs || s!=new_s)
         {
             new_p = p;
             new_vp = vp2;

From f597faed493e1d879d781300b3b13fe1332839ec Mon Sep 17 00:00:00 2001
From: badevguru <brian@babastik.com>
Date: Thu, 19 Nov 2015 21:29:04 -0800
Subject: [PATCH 03/22] Update SystemConstants.h

---
 OpenROV/SystemConstants.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/OpenROV/SystemConstants.h b/OpenROV/SystemConstants.h
index 28e692f..9f10858 100644
--- a/OpenROV/SystemConstants.h
+++ b/OpenROV/SystemConstants.h
@@ -35,6 +35,8 @@
 #define CALIBRATIONLASERS_PIN	45
 #define PORT_PIN				6
 #define VERTICAL_PIN			7
+#define PORT_VERTICAL_PIN 7
+#define STARBOARD_VERTICAL_PIN 9
 #define STARBOARD_PIN			8
 #define ESCPOWER_PIN			16
 #define ALTSERVO_PIN			9
@@ -54,4 +56,4 @@
 // AltServo
 #define ALTS_MIDPOINT 1500
 #define ALTS_MINPOINT 1000
-#define ALTS_MAXPOINT 2000
\ No newline at end of file
+#define ALTS_MAXPOINT 2000

From d7e6c6e1ff82cc4acfc269eb9fd8b415da10eff4 Mon Sep 17 00:00:00 2001
From: badevguru <brian@babastik.com>
Date: Thu, 19 Nov 2015 21:37:57 -0800
Subject: [PATCH 04/22] Update CThrusters_2Xv2.cpp

---
 OpenROV/CThrusters_2Xv2.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/OpenROV/CThrusters_2Xv2.cpp b/OpenROV/CThrusters_2Xv2.cpp
index 09bb8e2..b0337e4 100644
--- a/OpenROV/CThrusters_2Xv2.cpp
+++ b/OpenROV/CThrusters_2Xv2.cpp
@@ -24,9 +24,9 @@ const int CThrusters::kMotorCount = 4;
 namespace
 {
     CMotor port_motor( PORT_PIN );
-    CMotor port_vertical_motor( VERTICAL_PIN );
+    CMotor port_vertical_motor( PORT_VERTICAL_PIN );
     CMotor starboard_motor( STARBOARD_PIN );
-    CMotor starboard_vertical_motor( VERTICAL_PIN );
+    CMotor starboard_vertical_motor( STARBOARD_VERTICAL_PIN );
 
     int new_p	= MOTOR_TARGET_NEUTRAL_US;
     int new_s	= MOTOR_TARGET_NEUTRAL_US;

From 419be17ed36b65150ce9d10558a3eb217d4885b0 Mon Sep 17 00:00:00 2001
From: BrianAdams <brian@babastik.com>
Date: Wed, 6 Jan 2016 20:42:57 -0800
Subject: [PATCH 05/22] Initial commit of v2X1Cv2

---
 OpenROV/AConfig.h              |   7 +-
 OpenROV/CThrusters_v2X1Xv2.cpp | 442 +++++++++++++++++++++++++++++++++
 OpenROV/SystemConstants.h      |   4 +
 3 files changed, 450 insertions(+), 3 deletions(-)
 create mode 100644 OpenROV/CThrusters_v2X1Xv2.cpp

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index 781bce3..0770e35 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -36,12 +36,13 @@
 // Thrusters configurations  (These appear depricated and replaced by the THRUSTER_CONFIGURATION below)
 #define THRUSTER_CONFIG_NONE (0)
 #define THRUSTER_CONFIG_2X1 (0)
-#define THRUSTER_CONFIG_2Xv2 (1)
+#define THRUSTER_CONFIG_2Xv2 (0)
+#define THRUSTER_CONFIG_v2X1Xv2 (1)
 
 // Selected Thruster Configuration
 //#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1
-#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2Xv2
-
+//#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2Xv2
+#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_v2X1Xv2
 
 // ---------------------------------------------------------
 // After Market Modules
diff --git a/OpenROV/CThrusters_v2X1Xv2.cpp b/OpenROV/CThrusters_v2X1Xv2.cpp
new file mode 100644
index 0000000..2c23c13
--- /dev/null
+++ b/OpenROV/CThrusters_v2X1Xv2.cpp
@@ -0,0 +1,442 @@
+#include "AConfig.h"
+#if( THRUSTER_CONFIGURATION == THRUSTER_CONFIG_v2X1Xv2 )
+
+// Includes
+#include "CThrusters.h"
+#include "NConfigManager.h"
+#include "NDataManager.h"
+#include "CMotor.h"
+#include "CTimer.h"
+#include "CPin.h"
+
+#if(HAS_STD_CAPE)
+    #include "CCape.h"
+#endif
+
+#if(HAS_OROV_CONTROLLERBOARD_25)
+    #include "CControllerBoard.h"
+#endif
+
+
+// Static variable initialization
+const int CThrusters::kMotorCount = 5;
+
+namespace
+{
+    CMotor port_forward_motor( PORT_FORWARD_PIN );
+    CMotor port_aft_motor( PORT_AFT_PIN );
+    CMotor vertical_motor( VERTICAL_PIN );
+    CMotor starboard_forward_motor( STARBOARD_FORWARD_PIN );
+    CMotor starboard_aft_motor( STARBOARD_AFT_PIN );
+
+    int new_pf	= MOTOR_TARGET_NEUTRAL_US;
+    int new_pa	= MOTOR_TARGET_NEUTRAL_US;
+    int new_sf	= MOTOR_TARGET_NEUTRAL_US;
+    int new_sa	= MOTOR_TARGET_NEUTRAL_US;
+    int new_v = MOTOR_TARGET_NEUTRAL_US;
+    int new_st = 0; //strafe differential
+    int pf = MOTOR_TARGET_NEUTRAL_US;
+    int pa = MOTOR_TARGET_NEUTRAL_US;
+    int sf = MOTOR_TARGET_NEUTRAL_US;
+    int sa = MOTOR_TARGET_NEUTRAL_US;
+    int v = MOTOR_TARGET_NEUTRAL_US;
+    int st;
+
+    float trg_throttle,trg_yaw,trg_lift,trg_strafe;
+    int trg_motor_power;
+    int maxVtransDelta;
+
+
+    CTimer controltime;
+    CTimer thrusterOutput;
+    boolean bypasssmoothing;
+
+#ifdef ESCPOWER_PIN
+    bool canPowerESCs = true;
+    CPin escpower( "escpower", ESCPOWER_PIN, CPin::kDigital, CPin::kOutput );
+#else
+    boolean canPowerESCs = false;
+#endif
+}
+
+void CThrusters::Initialize()
+{
+    port_forward_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    port_fordard_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    port_forward_motor.Activate();
+
+    port_aft_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    port_aft_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    port_aft_motor.Activate();
+
+    vertical_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    vertical_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    vertical_motor.Activate();
+
+    starboard_forward_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    starboard_forward_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    starboard_forward_motor.Activate();
+
+    starboard_aft_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
+    starboard_aft_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    starboard_aft_motor.Activate();
+
+    thrusterOutput.Reset();
+    controltime.Reset();
+
+    bypasssmoothing = false;
+
+    #ifdef ESCPOWER_PIN
+    escpower.Reset();
+    escpower.Write( 1 ); //Turn on the ESCs
+    #endif
+}
+
+void CThrusters::Update( CCommand& command )
+{
+    if( command.Equals( "mtrmod1" ) )
+    {
+        port_forward_motor.m_positiveModifier = command.m_arguments[1] / 100;
+        port_aft_motor.m_positiveModifier = port_forward_motor.m_positiveModifier;
+        port_vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
+        starboard_forward_motor.m_positiveModifier = command.m_arguments[3] / 100;
+        starboard_aft_motor.m_positiveModifier = port_aft_motor.m_positiveModifier;
+    }
+
+    if( command.Equals( "mtrmod2" ) )
+    {
+        port_forward_motor.m_negativeModifier = command.m_arguments[1] / 100;
+        port_aft_motor.m_negativeModifier = port_forward_motor.m_negativeModifier;
+        port_vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
+        starboard_forward_motor.m_negativeModifier = command.m_arguments[3] / 100;
+        starboard_aft_motor.m_negativeModifier = port_aft_motor.m_negativeModifier;
+    }
+
+    if( command.Equals( "rmtrmod" ) )
+    {
+        Serial.print( F( "mtrmod:" ) );
+        Serial.print( port_forward_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_aft_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( vertical_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_forward_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_aft_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_forward_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( port_aft_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( vertical_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_forward_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_aft_motor.m_negativeModifier );
+        Serial.println( ";" );
+    }
+
+    //This is a legacy command that allows individual direct control of a motor. Safe to ignore.
+    if( command.Equals( "go" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[2] > 999 && command.m_arguments[3] > 999 && command.m_arguments[1] < 2001 && command.m_arguments[2] < 2001 && command.m_arguments[3] < 2001 )
+        {
+            pf = command.m_arguments[1];
+            pa = pf;
+            v = command.m_arguments[2];
+            sf = command.m_arguments[3];
+            sa = sf;
+
+            if( command.m_arguments[4] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    //Legacy Control just the port motors, only used for manual callibration
+    if( command.Equals( "port" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
+        {
+            pf = command.m_arguments[1];
+            pa = pf;
+
+            if( command.m_arguments[2] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    //Legacy Control, only used for manual callibration
+    if( command.Equals( "vertical" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
+        {
+            v = command.m_arguments[1];
+
+            if( command.m_arguments[2] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    //Legacy Control, only used for manual callibration
+    if( command.Equals( "starboard" ) )
+    {
+        //ignore corrupt data
+        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
+        {
+            sf = command.m_arguments[1];
+            sa = sf;
+
+            if( command.m_arguments[2] == 1 )
+            {
+                bypasssmoothing = true;
+            }
+        }
+    }
+
+    //Since we have shared control surfaces (same motors that are used to thrust are used to yaw) we have to blend
+    //the controls.  In this case, Throttle is the primary which means yaw is limited to whatever power still
+    //remains in the motor range after processing the thrust.  In other words, if your at full thrust you will not
+    //have any turn authority remaining.
+    if( command.Equals( "thro" ) || command.Equals( "yaw" ) || command.Equals("strafe") )
+    {
+        if( command.Equals( "thro" ) )
+        {
+            if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
+            {
+                trg_throttle = command.m_arguments[1] / 100.0;
+            }
+        }
+
+        if( command.Equals( "yaw" ) )
+        {
+            //ignore corrupt data
+            if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 ) //percent of max turn
+            {
+                trg_yaw = command.m_arguments[1] / 100.0;
+            }
+        }
+
+        if (command.Equals("strafe")){
+          if (command.m_arguments[1]>=-100 && command.m_arguments[1]<=100) {
+            trg_strafe = command.m_arguments[1]/100.0;
+          }
+        }
+
+        // The code below spreads the throttle spectrum over the possible range
+        // of the motor. Not sure this belongs here or should be placed with
+        // deadzon calculation in the motor code.
+        if( trg_throttle >= 0 )
+        {
+            pf = 1500 + ( 500.0 / abs( port_motor.m_positiveModifier ) ) * trg_throttle;
+            sf = pf;
+            pa = pf;
+            sa = pf;
+        }
+        else
+        {
+            pf = 1500 + ( 500.0 / abs( port_motor.m_negativeModifier ) ) * trg_throttle;
+            sf = pf;
+            pa = pf;
+            sa = pf;
+        }
+
+        trg_motor_power = sf;
+
+        int turn = trg_yaw * 250; //max range due to reverse range
+
+        if( trg_throttle >= 0 )
+        {
+            int offset = ( abs( turn ) + trg_motor_power ) - 2000;
+
+            if( offset < 0 )
+            {
+                offset = 0;
+            }
+
+            //This is the resulting of the blend of controls. In this thruster
+            //configuration, the port thrusters push together to induce a yaw.
+            pf = trg_motor_power + turn - offset;
+            pa = pf;
+            sf = trg_motor_power - turn - offset;
+            sa = sf;
+        }
+        else
+        {
+            int offset = 1000 - ( trg_motor_power - abs( turn ) );
+
+            if( offset < 0 )
+            {
+                offset = 0;
+            }
+
+            pf = trg_motor_power + turn + offset;
+            pa = pf;
+            sf = trg_motor_power - turn + offset;
+            sa = sf;
+        }
+
+        //So in this configuration, the pf=sa and the sf=pa to work tother to strafe.  The pf will be opposit of sf.
+        //strafe (side motion) is limited to whatever thrust is still available
+        //from the thust and yaw.
+        //maxHtransDelta = max range - remaining authority from throttle and yaw
+        maxHtransDelta = abs((500.0/abs(port_forward_motor.m_negativeModifier))*(1.0-abs(trg_throttle)-abs(trg_yaw)));
+        Serial.print("THRV2X1XV2.mhd:");Serial.print(maxHtransDelta);Serial.println(";");
+        st = constrain( (500.0/abs(port_forward_motor.m_negativeModifier))*trg_strafe,-maxHtransDelta,maxHtransDelta);
+        //st is the actual strafe translation we will employ
+        Serial.print("THRV2X1XV2.st:");Serial.print(st);Serial.println(";");
+        //Adjust the vertrans thrusters with the ideal translate value
+        //if we go the wrong way... switch these.
+        Serial.print("THRV2X1XV2.pf0:");Serial.print(pf);Serial.println(";");
+        Serial.print("THRV2X1XV2.sf0:");Serial.print(sf);Serial.println(";");
+
+        pf=pf+st;
+        sa=sa+st;
+
+        sf=sf-st;
+        sa=sa-st;
+
+        Serial.print("THRV2X1XV2.pf2:");Serial.print(pf);Serial.println(";");
+        Serial.print("THRV2X1XV2.sf2:");Serial.print(sf);Serial.println(";");
+
+    }
+
+    if( command.Equals( "lift" ) )
+    {
+        if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
+        {
+            trg_lift = command.m_arguments[1] / 100.0;
+            v = 1500 + 500 * trg_lift;
+        }
+    }
+
+    #ifdef ESCPOWER_PIN
+    else if( command.Equals( "escp" ) )
+    {
+        escpower.Write( command.m_arguments[1] ); //Turn on the ESCs
+        Serial.print( F( "log:escpower=" ) );
+        Serial.print( command.m_arguments[1] );
+        Serial.println( ';' );
+    }
+
+    #endif
+    else if( command.Equals( "start" ) )
+    {
+        port_forward_motor.Activate();
+        post_aft_motor.Activate();
+        vertical_motor.Activate();
+        starboard_forward_motor.Activate();
+        starboard_aft_motor.Activate();
+    }
+    else if( command.Equals( "stop" ) )
+    {
+        pf = MOTOR_TARGET_NEUTRAL_US;
+        pa = MOTOR_TARGET_NEUTRAL_US;
+        v = MOTOR_TARGET_NEUTRAL_US;
+        sf = MOTOR_TARGET_NEUTRAL_US;
+        sa = MOTOR_TARGET_NEUTRAL_US;
+        // Not sure why the reset does not re-attach the servo.
+        //port_motor.stop();
+        //vertical_motor.stop();
+        //starboard_motor.stop();
+    }
+
+    #ifdef ESCPOWER_PIN
+    else if( ( command.Equals( "mcal" ) ) && ( canPowerESCs ) )
+    {
+        Serial.println( F( "log:Motor Callibration Staring;" ) );
+        //Experimental. Add calibration code here
+        Serial.println( F( "log:Motor Callibration Complete;" ) );
+    }
+
+    #endif
+
+    //to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
+    //to their new positions in increments.  The incriment should eventually be adjustable from the cockpit so that
+    //the pilot could have more aggressive response profiles for the ROV.
+    if( controltime.HasElapsed( 50 ) )
+    {
+        if (pf!=new_pf || pa=new_pa || v!=new_v || sf!=new_sf || sa!=new_sa)
+        {
+            new_pf = pf;
+            new_pa = pa;
+            new_v = v;
+            new_sf = sf;
+            new_sa = sa;
+
+            // Check to see if any motors are non-neutral to signal system that at least one motor is running
+            if( pf != MOTOR_TARGET_NEUTRAL_US || pa != MOTOR_TARGET_NEUTRAL_US ||v != MOTOR_TARGET_NEUTRAL_US || sf != MOTOR_TARGET_NEUTRAL_US || sa != MOTOR_TARGET_NEUTRAL_US )
+            {
+                NDataManager::m_thrusterData.MotorsActive = true;
+            }
+            else
+            {
+                NDataManager::m_thrusterData.MotorsActive = false;
+            }
+
+            Serial.print( F( "motors:" ) );
+            Serial.print( port_forward_motor.SetMotorTarget( new_pf ) );
+            Serial.print( ',' );
+            Serial.print( port_aft_motor.SetMotorTarget( new_pa ) );
+            Serial.print( ',' );
+            Serial.print( vertical_motor.SetMotorTarget( new_v ) );
+            Serial.print( ',' );
+            Serial.print( starboard_forward_motor.SetMotorTarget( new_sf ) );
+            Serial.print( ',' );
+            Serial.print( starboard_aft_motor.SetMotorTarget( new_sa ) );
+            Serial.println( ';' );
+        }
+
+    }
+
+    NDataManager::m_navData.FTHR = map( ( new_pf + new_pa + new_sf + new_sa) / 4, 1000, 2000, -100, 100 );
+
+    //The output from the motors is unique to the thruster configuration
+    if( thrusterOutput.HasElapsed( 1000 ) )
+    {
+        Serial.print( F( "mtarg:" ) );
+        Serial.print( pf );
+        Serial.print( ',' );
+        Serial.print( pa );
+        Serial.print( ',' );
+        Serial.print( v );
+        Serial.print( ',' );
+        Serial.print( sf );
+        Serial.print( ',' );
+        Serial.print( sa );
+        Serial.println( ';' );
+        NDataManager::m_thrusterData.MATC = port_forward_motor.IsActive() || port_aft_motor.IsActive() || vertical_motor.IsActive() || starboard_forward_motor.IsActive() || starboard_aft_motor.IsActive();
+        Serial.print( F( "mtrmod:" ) );
+        Serial.print( port_forward_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_aft_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( vertical_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_forward_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( starboard_aft_motor.m_positiveModifier );
+        Serial.print( "," );
+        Serial.print( port_forward_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( port_aft_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( vertical_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_forward_motor.m_negativeModifier );
+        Serial.print( "," );
+        Serial.print( starboard_aft_motor.m_negativeModifier );
+        Serial.println( ";" );
+    }
+}
+
+#endif
diff --git a/OpenROV/SystemConstants.h b/OpenROV/SystemConstants.h
index 9f10858..ea5fe53 100644
--- a/OpenROV/SystemConstants.h
+++ b/OpenROV/SystemConstants.h
@@ -42,6 +42,10 @@
 #define ALTSERVO_PIN			9
 #define ELIGHTS_PIN				46
 #define I2CPOWER_PIN			48
+#define PORT_FORWARD_PIN 9
+#define PORT_AFT_PIN 6
+#define STARBOARD_FORWARD_PIN 10
+#define STARBOARD_AFT_PIN 8
 
 // Commands
 #define COMMAND_MAX_ARGUMENTS			10

From 029faa7d588bd352018f4ed79b1feebdd0ed23a8 Mon Sep 17 00:00:00 2001
From: BrianAdams <brian@babastik.com>
Date: Thu, 7 Jan 2016 12:17:13 -0800
Subject: [PATCH 06/22] Now compiles

---
 OpenROV/CThrusters_v2X1Xv2.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/OpenROV/CThrusters_v2X1Xv2.cpp b/OpenROV/CThrusters_v2X1Xv2.cpp
index 2c23c13..e7b68fd 100644
--- a/OpenROV/CThrusters_v2X1Xv2.cpp
+++ b/OpenROV/CThrusters_v2X1Xv2.cpp
@@ -44,7 +44,7 @@ namespace
 
     float trg_throttle,trg_yaw,trg_lift,trg_strafe;
     int trg_motor_power;
-    int maxVtransDelta;
+    int maxHtransDelta;
 
 
     CTimer controltime;
@@ -62,7 +62,7 @@ namespace
 void CThrusters::Initialize()
 {
     port_forward_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
-    port_fordard_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
+    port_forward_motor.m_positiveDeadzoneBuffer = NConfigManager::m_deadZoneMax;
     port_forward_motor.Activate();
 
     port_aft_motor.m_negativeDeadzoneBuffer = NConfigManager::m_deadZoneMin;
@@ -98,7 +98,7 @@ void CThrusters::Update( CCommand& command )
     {
         port_forward_motor.m_positiveModifier = command.m_arguments[1] / 100;
         port_aft_motor.m_positiveModifier = port_forward_motor.m_positiveModifier;
-        port_vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
+        vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
         starboard_forward_motor.m_positiveModifier = command.m_arguments[3] / 100;
         starboard_aft_motor.m_positiveModifier = port_aft_motor.m_positiveModifier;
     }
@@ -107,7 +107,7 @@ void CThrusters::Update( CCommand& command )
     {
         port_forward_motor.m_negativeModifier = command.m_arguments[1] / 100;
         port_aft_motor.m_negativeModifier = port_forward_motor.m_negativeModifier;
-        port_vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
+        vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
         starboard_forward_motor.m_negativeModifier = command.m_arguments[3] / 100;
         starboard_aft_motor.m_negativeModifier = port_aft_motor.m_negativeModifier;
     }
@@ -237,14 +237,14 @@ void CThrusters::Update( CCommand& command )
         // deadzon calculation in the motor code.
         if( trg_throttle >= 0 )
         {
-            pf = 1500 + ( 500.0 / abs( port_motor.m_positiveModifier ) ) * trg_throttle;
+            pf = 1500 + ( 500.0 / abs( port_forward_motor.m_positiveModifier ) ) * trg_throttle;
             sf = pf;
             pa = pf;
             sa = pf;
         }
         else
         {
-            pf = 1500 + ( 500.0 / abs( port_motor.m_negativeModifier ) ) * trg_throttle;
+            pf = 1500 + ( 500.0 / abs( port_forward_motor.m_negativeModifier ) ) * trg_throttle;
             sf = pf;
             pa = pf;
             sa = pf;
@@ -332,7 +332,7 @@ void CThrusters::Update( CCommand& command )
     else if( command.Equals( "start" ) )
     {
         port_forward_motor.Activate();
-        post_aft_motor.Activate();
+        port_aft_motor.Activate();
         vertical_motor.Activate();
         starboard_forward_motor.Activate();
         starboard_aft_motor.Activate();
@@ -365,7 +365,7 @@ void CThrusters::Update( CCommand& command )
     //the pilot could have more aggressive response profiles for the ROV.
     if( controltime.HasElapsed( 50 ) )
     {
-        if (pf!=new_pf || pa=new_pa || v!=new_v || sf!=new_sf || sa!=new_sa)
+        if (pf!=new_pf || pa!=new_pa || v!=new_v || sf!=new_sf || sa!=new_sa)
         {
             new_pf = pf;
             new_pa = pa;

From 277d1d7ca14f3ebe8c0b8a17079c12dee99fb78a Mon Sep 17 00:00:00 2001
From: root <root@openrov.localdomain>
Date: Tue, 12 Apr 2016 18:25:17 +0000
Subject: [PATCH 07/22] Removed IMU mode switching

---
 OpenROV/CBNO055.cpp | 82 ++-------------------------------------------
 1 file changed, 2 insertions(+), 80 deletions(-)

diff --git a/OpenROV/CBNO055.cpp b/OpenROV/CBNO055.cpp
index 75465b1..4e2530e 100644
--- a/OpenROV/CBNO055.cpp
+++ b/OpenROV/CBNO055.cpp
@@ -11,15 +11,11 @@ namespace
 	CTimer bno055_sample_timer;
 	CTimer report_timer;
 	CTimer imuTimer;
-	CTimer fusionTimer;
 
 	bool initalized				= false;
 	bool browserPingReceived	= false;
 
 	bool inFusionMode			= false;
-	bool waitingToSwitch		= false;
-
-	bool inOverride				= false;
 
 	CAdaBNO055 bno;
 
@@ -54,14 +50,12 @@ namespace
 	}
 }
 
-
 void CBNO055::Initialize()
 {
 	// Reset timers
 	bno055_sample_timer.Reset();
 	report_timer.Reset();
 	imuTimer.Reset();
-	fusionTimer.Reset();
 }
 
 void CBNO055::Update( CCommand& commandIn )
@@ -79,7 +73,6 @@ void CBNO055::Update( CCommand& commandIn )
 			if( commandIn.m_arguments[ 1 ] == 0 )
 			{
 				// Turn off override
-				inOverride		= false;
 				inFusionMode	= true;
 				bno.EnterNDOFMode();
 
@@ -88,14 +81,14 @@ void CBNO055::Update( CCommand& commandIn )
 			if( commandIn.m_arguments[ 1 ] == 12 )
 			{
 				// Override to NDOF
-				inOverride = true;
+				inFusionMode	= true;
 				bno.EnterNDOFMode();
 			}
 
 			if( commandIn.m_arguments[ 1 ] == 8 )
 			{
 				// Override to IMU mode
-				inOverride = true;
+				inFusionMode	= false;
 				bno.EnterIMUMode();
 			}
 		}
@@ -222,77 +215,6 @@ void CBNO055::Update( CCommand& commandIn )
 			NDataManager::m_navData.PITC	= euler.z();
 			NDataManager::m_navData.ROLL	= -euler.y();
         }
-
-		if( inOverride )
-		{
-			// Temp - Do nothing!
-		}
-		else
-		{
-			// If we're in fusion mode, check to see if we have a good mag and system calibration
-			if( inFusionMode )
-			{
-				// If motors ever come on during calibration, drop to IMU mode
-				if( NDataManager::m_thrusterData.MotorsActive )
-				{
-					// Switch to gyro mode
-					bno.EnterIMUMode();
-					inFusionMode = false;
-
-					imuTimer.Reset();
-				}
-
-				// Try to stay in fusion mode until some kind of calibration is achieved, and then for at least three seconds to get a better calibration
-				//if( bno.m_magCal != 0 && bno.m_systemCal != 0 )
-				//{
-				//	if( fusionTimer.HasElapsed( 3000 ) )
-				//	{
-				//		// Switch to gyro mode
-				//		bno.EnterIMUMode();
-				//		inFusionMode = false;
-
-				//		// Reset the timer
-				//		imuTimer.Reset();
-				//	}
-				//}
-			}
-			else
-			{
-				if( waitingToSwitch )
-				{
-					// Make sure motors aren't active
-					if( NDataManager::m_thrusterData.MotorsActive == false )
-					{
-						// Switch modes
-						bno.EnterNDOFMode();
-						fusionTimer.Reset();
-						inFusionMode	= true;
-						waitingToSwitch = false;
-					}
-				}
-				else
-				{
-					// Check to see if proper amount of time has elapsed before switching back to fusion mode
-					if( imuTimer.HasElapsed( 5000 ) )
-					{
-						// Make sure motors aren't active
-						if( NDataManager::m_thrusterData.MotorsActive == false )
-						{
-							// Switch modes
-							bno.EnterNDOFMode();
-							fusionTimer.Reset();
-							inFusionMode	= true;
-							waitingToSwitch = false;
-						}
-						else
-						{
-							// Not ready to switch because motors are on
-							waitingToSwitch = true;
-						}
-					}
-				}
-			}
-		}
 	}
 }
 #endif

From 6093c8f686320401afad9ccd445ebbe7196a7249 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Tue, 12 Apr 2016 18:32:25 +0000
Subject: [PATCH 08/22] Turned off deadman switch by default

---
 OpenROV/AConfig.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index 0770e35..1d005e2 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -31,7 +31,7 @@
 #define HAS_STD_AUTOPILOT (1)
 #define HAS_EXP_AUTOPILOT (0)
 #define HAS_ALT_SERVO (1)
-#define DEADMANSWITCH_ON (1)
+#define DEADMANSWITCH_ON (0)
 
 // Thrusters configurations  (These appear depricated and replaced by the THRUSTER_CONFIGURATION below)
 #define THRUSTER_CONFIG_NONE (0)

From 1ed26691afdd5c63459515fe45d1595453fbff74 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Tue, 12 Apr 2016 18:32:49 +0000
Subject: [PATCH 09/22] Changed thruster configuration back to 2X1

---
 OpenROV/AConfig.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index 1d005e2..9af5f4d 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -40,9 +40,9 @@
 #define THRUSTER_CONFIG_v2X1Xv2 (1)
 
 // Selected Thruster Configuration
-//#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1
+#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1
 //#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2Xv2
-#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_v2X1Xv2
+//#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_v2X1Xv2
 
 // ---------------------------------------------------------
 // After Market Modules

From 852c52c957797e21d2fe546a2268849d791062a6 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Tue, 12 Apr 2016 18:33:27 +0000
Subject: [PATCH 10/22] Fixed thruster config numbering

---
 OpenROV/AConfig.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index 9af5f4d..b66455f 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -35,9 +35,9 @@
 
 // Thrusters configurations  (These appear depricated and replaced by the THRUSTER_CONFIGURATION below)
 #define THRUSTER_CONFIG_NONE (0)
-#define THRUSTER_CONFIG_2X1 (0)
-#define THRUSTER_CONFIG_2Xv2 (0)
-#define THRUSTER_CONFIG_v2X1Xv2 (1)
+#define THRUSTER_CONFIG_2X1 (1)
+#define THRUSTER_CONFIG_2Xv2 (2)
+#define THRUSTER_CONFIG_v2X1Xv2 (3)
 
 // Selected Thruster Configuration
 #define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1

From 7bf8f348337e0c79551ada3c91584ef1094da611 Mon Sep 17 00:00:00 2001
From: root <root@openrov.localdomain>
Date: Sat, 26 Mar 2016 05:14:04 +0000
Subject: [PATCH 11/22] Changed timer5's divisor to make PWM frequency 3921Hz
 on PWM1,2 and 3

---
 OpenROV/OpenROV.ino | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/OpenROV/OpenROV.ino b/OpenROV/OpenROV.ino
index 99deabc..707449a 100644
--- a/OpenROV/OpenROV.ino
+++ b/OpenROV/OpenROV.ino
@@ -18,6 +18,9 @@ void setup()
 	NModuleManager::Initialize();
 	NDataManager::Initialize();
 
+	// Set timer 5 divisor to 8 for PWM frequency of 3921.16Hz (D44, D45, D46)
+	TCCR5B = TCCR5B & B11111000 | B00000010;
+
 	// Boot complete
 	Serial.println( F( "boot:1;" ) );
 }

From 35cee7b4ad133e81dce0a81bbfebaeb3b08257c5 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Sat, 26 Mar 2016 06:06:43 +0000
Subject: [PATCH 12/22] Updated value calculation for lights due to changes in
 cockpit, and removed external lights code, since it is in a plugin

---
 OpenROV/CLights.cpp | 19 +++++--------------
 1 file changed, 5 insertions(+), 14 deletions(-)

diff --git a/OpenROV/CLights.cpp b/OpenROV/CLights.cpp
index 4912842..d6a9e76 100644
--- a/OpenROV/CLights.cpp
+++ b/OpenROV/CLights.cpp
@@ -47,7 +47,9 @@ void CLights::Update( CCommand& commandIn )
 	if( commandIn.Equals( "ligt" ) )
 	{
 		// 0 - 255
-		int value = commandIn.m_arguments[1];
+		float percentValue = ( float )commandIn.m_arguments[1] / 100.0f; //0 - 255
+		int value = (int)( 255.0f * percentValue );
+		
 		light.Write( value );
 
 		// LIGT - Light toggle
@@ -57,22 +59,11 @@ void CLights::Update( CCommand& commandIn )
 
 		// LIGP - Light percentage
 		Serial.print( F( "LIGP:" ) );
-		Serial.print( commandIn.m_arguments[1] / 255.0f );
+		Serial.print( percentValue );
 		Serial.println( ';' );
 	}
 
-	if( commandIn.Equals( "eligt" ) )
-	{
-		float percentvalue = ( float )commandIn.m_arguments[1] / 100.0f; //0 - 255
-		int value = 255 * percentvalue;
-		elight.Write( value );
-		Serial.print( F( "LIGTE:" ) );
-		Serial.print( value );
-		Serial.print( ';' );
-		Serial.print( F( "LIGPE:" ) );
-		Serial.print( percentvalue );
-		Serial.println( ';' );
-	}
+	
 }
 
 #endif

From aee39fab7142c93fef965a963c99970cacb7407d Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Thu, 14 Apr 2016 04:20:17 +0000
Subject: [PATCH 13/22] Working on MS5837 depth sensor library. Not ready yet

---
 OpenROV/AConfig.h          |   6 +-
 OpenROV/CMS5803_XX.cpp     |   3 +
 OpenROV/CMS5837.cpp        | 139 +++++++++++++++++++++
 OpenROV/CMS5837.h          |  11 ++
 OpenROV/LibMS5837_30BA.cpp | 241 +++++++++++++++++++++++++++++++++++++
 OpenROV/LibMS5837_30BA.h   |  87 +++++++++++++
 6 files changed, 486 insertions(+), 1 deletion(-)
 create mode 100644 OpenROV/CMS5837.cpp
 create mode 100644 OpenROV/CMS5837.h
 create mode 100644 OpenROV/LibMS5837_30BA.cpp
 create mode 100644 OpenROV/LibMS5837_30BA.h

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index b66455f..0ce7aeb 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -52,9 +52,13 @@
 #define HAS_POLOLU_MINIMUV (0)
 
 // MS5803_XXBA Depth Sensor
-#define HAS_MS5803_XXBA (1)
+#define HAS_MS5803_XXBA (0)
 #define MS5803_XXBA_I2C_ADDRESS 0x76
 
+// MS5837_30BA Depth Sensor
+#define HAS_MS5837_30BA (1)
+#define MS5837_30BA_I2C_ADDRESS 0x76
+
 // MPU9150 IMU
 #define HAS_MPU9150 (1)
 #define HAS_BNO055 (1)
diff --git a/OpenROV/CMS5803_XX.cpp b/OpenROV/CMS5803_XX.cpp
index dd896d5..ec83bde 100644
--- a/OpenROV/CMS5803_XX.cpp
+++ b/OpenROV/CMS5803_XX.cpp
@@ -36,9 +36,12 @@
 
 #define CMD_RESET		0x1E	// ADC reset command
 #define CMD_ADC_READ	0x00	// ADC read command
+
 #define CMD_ADC_CONV	0x40	// ADC conversion command
+
 #define CMD_ADC_D1		0x00	// ADC D1 conversion
 #define CMD_ADC_D2		0x10	// ADC D2 conversion
+
 #define CMD_ADC_256		0x00	// ADC resolution=256
 #define CMD_ADC_512		0x02	// ADC resolution=512
 #define CMD_ADC_1024	0x04	// ADC resolution=1024
diff --git a/OpenROV/CMS5837.cpp b/OpenROV/CMS5837.cpp
new file mode 100644
index 0000000..5c62a60
--- /dev/null
+++ b/OpenROV/CMS5837.cpp
@@ -0,0 +1,139 @@
+#include "AConfig.h"
+#if(HAS_MS5837_30BA)
+
+#include "CMS5837_30BA.h"
+#include "LibMS5837_30BA.h"
+#include "CTimer.h"
+#include "CI2C.h"
+
+#include "NCommManager.h"
+#include "NConfigManager.h"
+#include "NDataManager.h"
+
+namespace
+{
+    enum class ESensorState
+    {
+        UNINITIALIZED,
+        IDLE,
+        CONVERTING_PRESSURE,
+        CONVERTING_TEMPERATURE
+    };
+    
+    MS5837_30BA m_sensor( MS5837_RES_4096 );
+
+	CTimer m_initTimer;         // 10s timer for initialization attempts
+	CTimer m_readTimer;         // 50ms timer for generating data points
+	CTimer m_conversionTimer;   // 20ms timer for allowing the sensor to perform conversion before attempting to read the value
+
+	float m_depth_m         = 0;
+	float m_depthOffset_m   = 0;
+	
+	const int k_maxRetries  = 3;
+	int m_retries           = 0;
+
+	ESensorState m_state    = ESensorState::UNINITIALIZED;
+}
+
+void CMS5837_30BA::Initialize()
+{
+    // Announce depth capability
+	NConfigManager::m_capabilityBitmask |= ( 1 << DEPTH_CAPABLE );
+	
+	// Attempt to initialize the sensor
+	if( m_sensor.Initialize() )
+	{
+	    // Set the state to IDLE, ready to start fetching data
+	    m_state == ESensorState::IDLE;
+	}
+	
+	m_initTimer.Reset();
+}
+
+
+void CMS5837_30BA::Update( CCommand& commandIn )
+{
+    // Handle commands
+    
+    if( m_state == ESensorState::UNINITIALIZED )
+    {
+        if( m_retries < k_maxRetries )
+        {
+            // Make an attempt to initialize the sensor every 10 seconds
+            if( m_initTimer.HasElapsed( 10000 ) )
+    		{
+            	if( m_sensor.Initialize() )
+            	{
+            	    // Set the state to IDLE, ready to start fetching data
+            	    m_state == ESensorState::IDLE;
+            	}
+            	else
+            	{
+            	    m_initTimer.Reset();
+            	    m_retries++;
+            	}
+    		}
+        }
+    }
+    else
+    {
+        // Handle commands
+        if( NCommManager::m_isCommandAvailable )
+        {
+            // Zero the depth value
+            if( commandIn.Equals( "dzer" ) )
+            {
+            	dev.m_depthOffset = dev.m_depth;
+            }
+            else if( commandIn.Equals( "dtwa" ) )
+            {
+            	// Print water type value
+            	if( NConfigManager::m_waterType == WATERTYPE_FRESH )
+            	{
+            		NConfigManager::m_waterType = WATERTYPE_SALT;
+            		Serial.println( F( "dtwa:1;" ) );
+            	}
+            	else
+            	{
+            		NConfigManager::m_waterType = WATERTYPE_FRESH;
+            		Serial.println( F( "dtwa:0;" ) );
+            	}
+            }
+        }
+        
+        // TODO: Handle error checking for sensor actions, since they can fail due to I2C problems
+        // Handle sensor update process
+        if( m_state == ESensorState::IDLE )
+        {
+            // Kick off a conversion every 50ms
+            if( m_readTimer.HasElapsed( 50 ) )
+    		{
+                m_sensor.StartConversion();
+                
+                // Reset the read timer
+                m_readTimer.Reset();
+                
+                // Start the conversion timer
+                m_conversionTimer.Reset();
+                
+                m_state = ESensorState::CONVERTING;
+    		}
+        }
+        else if( m_state == ESensorState::CONVERTING )
+        {
+            // Wait 20ms for a conversion to complete
+            if( m_conversionTimer.HasElapsed( 20 ) )
+    		{
+                m_sensor.Read();
+                
+                // Perform calculations
+                
+                // Report results
+                
+                m_state = ESensorState::IDLE;
+    		}
+        }
+    }
+}
+
+#endif
\ No newline at end of file
diff --git a/OpenROV/CMS5837.h b/OpenROV/CMS5837.h
new file mode 100644
index 0000000..b9bfed4
--- /dev/null
+++ b/OpenROV/CMS5837.h
@@ -0,0 +1,11 @@
+#pragma once
+
+// Includes
+#include "CModule.h"
+
+class CMS5837_30BA : public CModule
+{
+public:
+	void Initialize();
+	void Update( CCommand& commandIn );
+};
diff --git a/OpenROV/LibMS5837_30BA.cpp b/OpenROV/LibMS5837_30BA.cpp
new file mode 100644
index 0000000..a38b2bb
--- /dev/null
+++ b/OpenROV/LibMS5837_30BA.cpp
@@ -0,0 +1,241 @@
+// Includes
+#include "LibMS5837_30BA.h"
+#include "CI2C.h"
+
+// For I2C, set the CSB Pin (pin 3) high for address 0x76, and pull low for address 0x77.
+#define I2C_ADDRESS             0x76    // or 0x77
+
+#define CMD_RESET               0x1E
+#define CMD_ADC_READ            0x00
+#define CMD_ADC_CONV_BASE	    0x40	// ADC conversion base command. Modify based on D1/D2 and resolution
+
+#define CMD_ADC_D1              0x00
+#define CMD_ADC_D2              0x10
+
+#define CMD_ADC_256             0x00	// ADC resolution = 256
+#define CMD_ADC_512             0x02	// ADC resolution = 512
+#define CMD_ADC_1024	        0x04	// ADC resolution = 1024
+#define CMD_ADC_2048        	0x06	// ADC resolution = 2048
+#define CMD_ADC_4096	        0x08	// ADC resolution = 4096
+#define CMD_ADC_8192	        0x0A	// ADC resolution = 8192
+
+// TODO: Add more error handling
+
+int MS5837_30BA::Initialize()
+{
+    // First Reset
+    Reset();
+    
+    
+}
+
+int MS5837_30BA::Reset()
+{
+    if( WriteRegisterByte( CMD_RESET ) )
+    {
+        return 1;
+    }
+
+    // Specified by data sheet
+	delay( 10 );
+	
+	return 0;
+}
+
+int MS5837_30BA::GetCalibrationCoefficients()
+{
+    bool ret = true;
+	bool gotCRC = true;
+
+	// Read sensor coefficients
+	for( int i = 0; i < 8; i++ )
+	{
+		// The PROM starts at address 0xA0
+		if( i != 7 )
+		{
+			ret &= WriteRegisterByte( 0xA0 + ( i * 2 ) );
+		}
+		else
+		{
+			gotCRC &= WriteRegisterByte( 0xA0 + ( i * 2 ) );
+		}
+
+		if( I2c.read( MS5803_I2C_ADDRESS, 2 ) )
+		{
+			// Don't treat the crc read as an overall failure (yet)
+			if( i != 7 )
+			{
+				ret &= false;
+			}
+			else
+			{
+				gotCRC &= false;
+			}
+
+			Serial.print( "Depth.CoeffFailure:C" );
+			Serial.print( i );
+			Serial.println( ";" );
+
+			// Print out coefficients
+			Serial.print( "Depth.C" );
+			Serial.print( i );
+			Serial.println( ":INVALID;" );
+			delay( 10 );
+		}
+		else
+		{
+			// Get coefficient bytes
+			while( I2c.available() )
+			{
+				HighByte = I2c.receive();
+				LowByte = I2c.receive();
+			}
+
+			// Combine bytes
+			sensorCoeffs[i] = ( ( ( unsigned int )HighByte << 8 ) + LowByte );
+
+			// Print out coefficient
+			Serial.print( "Depth.C" );
+			Serial.print( i );
+			Serial.print( ":" );
+			Serial.print( sensorCoeffs[i] );
+			Serial.println( ";" );
+			delay( 10 );
+		}
+	}
+
+	if( gotCRC == true )
+	{
+		// The last 4 bits of the 7th coefficient form a CRC error checking code.
+		unsigned char p_crc = sensorCoeffs[7];
+		// Use a function to calculate the CRC value
+		unsigned char n_crc = MS_5803_CRC( sensorCoeffs );
+
+		// If the CRC value doesn't match the sensor's CRC value, then the
+		// connection can't be trusted. Check your wiring.
+		if( p_crc != n_crc )
+		{
+			Serial.println( "Depth.crcCheck:Failed;" );
+			//ret &= false;
+		}
+		else
+		{
+			Serial.println( "Depth.crcCheck:Succeeded;" );
+		}
+	}
+	else
+	{
+		Serial.println( "Depth.crcCheck:CantPerform;" );
+	}
+
+	if( ret )
+	{
+		// Correct between 14mbar/30mbar sensor type
+		if( sensorCoeffs[0] != 0 )
+		{
+			m_is30bar = true;
+			m_divisor = 8192; // (2^13)
+		}
+		else
+		{
+			m_is30bar = false;
+			m_divisor = 32768; // (2^15)
+		}
+	}
+
+	// Otherwise, return true when everything checks out OK.
+	return ret;
+}
+
+int MS5837_30BA::StartConversion()
+{
+    
+}
+
+int MS5837_30BA::Read()
+{
+    
+}
+
+void MS5837_30BA::SetWaterType( int waterTypeIn )
+{
+    
+}
+
+uint8_t MS5837_30BA::GetCRC( uint32_t n_prom[] )
+{
+    int cnt; // simple counter
+    uint32_t n_rem=0; // crc remainder
+    uint8_t n_bit;
+    
+    n_prom[0]=((n_prom[0]) & 0x0FFF); // CRC byte is replaced by 0
+    n_prom[7]=0; // Subsidiary value, set to 0
+    
+    for (cnt =0; cnt < 16; cnt++) // operation is performed on bytes
+    { 
+        // choose LSB or MSB
+        if (cnt%2==1) 
+            n_rem ^= (uint16_t) ((n_prom[cnt>>1]) &0x00FF);
+        else 
+            n_rem ^= (uint16_t) (n_prom[cnt>>1]>>8);
+            
+        for (n_bit = 8; n_bit > 0; n_bit--)
+        {
+            if (n_rem & (0x8000)) 
+                n_rem = (n_rem << 1) ^ 0x3000;
+            else 
+                n_rem = (n_rem << 1);
+        }
+    }
+    
+    n_rem= ((n_rem >> 12) & 0x000F); // final 4-bit remainder is CRC code
+    
+    return (n_rem ^ 0x00);
+}
+
+uint32_t MS5837_30BA::CommandADC( uint8_t commandIn )
+{
+    // Send the read command
+	WriteRegisterByte( CMD_ADC_READ );
+
+	// Then request the results. This should be a 24-bit result (3 bytes)
+	I2c.read( I2C_ADDRESS, 3 );
+
+	while( I2c.available() )
+	{
+		HighByte = I2c.receive();
+		MidByte = I2c.receive();
+		LowByte = I2c.receive();
+	}
+
+	// Combine the bytes into one integer
+	result = ( ( uint32_t )HighByte << 16 ) + ( ( uint32_t )MidByte << 8 ) + ( uint32_t )LowByte;
+	return result;
+}
+
+int MS5837_30BA::WriteRegisterByte( uint8_t addressIn )
+{
+    return I2c.write( ( uint8_t )I2C_ADDRESS, addressIn );
+}
+
+int MS5837_30BA::WriteDataByte( uint8_t addressIn, uint8_t dataIn )
+{
+    return I2c.write( ( uint8_t )I2C_ADDRESS, addressIn, dataIn );
+}
+
+int MS5837_30BA::ReadByte( uint8_t addressIn, uint8_t& dataOut )
+{
+    // Set address to request from
+	uint8_t ret = I2c.read( ( uint8_t )MS5803_I2C_ADDRESS, ( uint8_t )addressIn, ( uint8_t )1 );
+
+	// Non-zero failure
+	if( ret )
+	{
+		return ret;
+	}
+
+	// Request single byte from slave
+	dataOut = I2c.receive();
+
+	return ret;
+}
\ No newline at end of file
diff --git a/OpenROV/LibMS5837_30BA.h b/OpenROV/LibMS5837_30BA.h
new file mode 100644
index 0000000..15d5950
--- /dev/null
+++ b/OpenROV/LibMS5837_30BA.h
@@ -0,0 +1,87 @@
+#pragma once
+
+// Includes
+#include <Arduino.h>
+
+// Defines
+#define MS5837_RES_256          256u
+#define MS5837_RES_512          512u
+#define MS5837_RES_1024         1024u
+#define MS5837_RES_2048         2048u
+#define MS5837_RES_4096         4096u
+#define MS5837_RES_8192         8192u
+
+#define MS5837_WATERTYPE_FRESH  0
+#define MS5837_WATERTYPE_SALT   1
+
+class MS5837_30BA
+{
+public:
+   
+    // Attributes
+    float       m_depth;
+	float       m_depthOffset;
+
+	bool        m_hasValidCoefficients;
+
+	 // Methods
+	MS5837_30BA( uint16_t resolutionIn = MS5837_RES_4096 );
+	
+	int Initialize();
+	int GetCalibrationCoefficients();
+	int Reset();
+	int StartPressureConversion();
+	int StartTemperatureConversion();
+	int Read();
+	
+	void SetWaterType( int waterTypeIn );
+
+private:
+
+    // Attributes
+	float m_pressure_mbar;
+	float m_temperature_c;
+	
+	// Create array to hold the 8 sensor calibration coefficients
+    uint16_t m_sensorCoeffs[8]; // unsigned 16-bit integer (0-65535)
+    
+    uint32_t D1 = 0;        // Digital pressure value
+    uint32_t D2 = 0;        // Digital temperature value
+    
+    int32_t dT = 0;         // Difference between actual and reference temperature
+    int32_t TEMP = 0;       // Actual temperature
+    
+    int64_t OFF = 0;        // Offset at actual temperature
+    int64_t SENS = 0;       // Sensitivity at actual temperature
+    
+    int64_t Ti = 0;         // Second order temperature component       
+    int64_t OFFi = 0;       // Second order offset component            
+    int64_t SENSi = 0;      // Second order sens component             
+    
+    int64_t OFF2 = 0;       // Second order final off 
+    int64_t SENS2 = 0;      // Second order final sens
+    int64_t TEMP2 = 0;      // Second order final temp
+    
+    int32_t P = 0;          // Temperature compensated pressure
+    
+    // Bytes to hold the results from I2C communications with the sensor
+    uint8_t HighByte;
+    uint8_t MidByte;
+    uint8_t LowByte;
+    
+	uint16_t m_oversampleResolution;
+    
+    // Some constants used in calculations below
+    const uint32_t POW_2_13 = 8192u;            // 2^13
+    const uint32_t POW_2_21 = 2097152u;         // 2^21
+    const uint64_t POW_2_33 = 8589934592ULL;    // 2^33
+    const uint64_t POW_2_37 = 137438953472ULL;  // 2^37
+	
+	// Methods
+	uint8_t GetCRC( uint32_t n_prom[] );
+	uint32_t CommandADC( uint8_t commandIn );
+	
+	int WriteRegisterByte( uint8_t addressIn );
+	int WriteDataByte( uint8_t addressIn, uint8_t dataIn );
+	int ReadByte( uint8_t addressIn, uint8_t& dataOut );
+};
\ No newline at end of file

From 352cd3184e89497602e3837691b51c7b4592a697 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Thu, 14 Apr 2016 07:34:39 +0000
Subject: [PATCH 14/22] Removed old LibMS5803 files, no longer used. Finished
 fleshing out functionality for the MS5837. Not tested or compiled yet

---
 OpenROV/CMS5803_XXBALib.cpp | 214 -------------------------------
 OpenROV/CMS5837.cpp         | 109 +++++++++++++---
 OpenROV/LibMS5803_XXBA.h    |  15 ---
 OpenROV/LibMS5837_30BA.cpp  | 243 ++++++++++++++++++------------------
 OpenROV/LibMS5837_30BA.h    |  47 ++++---
 5 files changed, 243 insertions(+), 385 deletions(-)
 delete mode 100644 OpenROV/CMS5803_XXBALib.cpp
 delete mode 100644 OpenROV/LibMS5803_XXBA.h

diff --git a/OpenROV/CMS5803_XXBALib.cpp b/OpenROV/CMS5803_XXBALib.cpp
deleted file mode 100644
index a88222b..0000000
--- a/OpenROV/CMS5803_XXBALib.cpp
+++ /dev/null
@@ -1,214 +0,0 @@
-#include "LibMS5803_XXBA.h"
-#include <Arduino.h>
-
-/* It appears that the gen 2 of the MS5803 series is embedding
-   the pow(2, X) used in the final calculation that is unique
-   to the sensor in the vendor reserved space.  8192 & CalConstant[0]
-   seems to mostly = 8192.  Not reliable enough to pull yet.  But
-   all of the new sensors do have values in the CalConstant[0] whereas
-   the 14bar sensor we were using does not, so I will use that to auto
-   detect which sensor is used at the moment
-*/
-static unsigned int MS5803_Model_Varient( unsigned int CalConstant[8] )
-{
-	if( CalConstant[0] != 0 )
-	{
-		return 8192;    // (2^13)
-	}
-
-	return 32768;// (2^15)
-}
-
-float CorrectedTemperature( long AdcTemperature, unsigned int CalConstant[8] )
-{
-	float T2, Off2, Sens2, TempDifference, Temperature;
-	// Calculate the Temperature (first-order computation)
-
-	TempDifference = ( float )( AdcTemperature - ( ( long )CalConstant[5] << 8 ) );
-	Temperature = ( TempDifference * ( float )CalConstant[6] ) / pow( 2, 23 );
-	Temperature = Temperature + 2000;  // This is the temperature in hundredths of a degree C
-
-	// Calculate the second-order offsets
-
-	if( Temperature < 2000.0 ) // Is temperature below or above 20.00 deg C ?
-	{
-		T2 = 3 * pow( TempDifference, 2 ) / pow( 2, 33 );
-		Off2 = 1.5 * pow( ( Temperature - 2000.0 ), 2 );
-		Sens2 = 0.625 * pow( ( Temperature - 2000.0 ), 2 );
-	}
-	else
-	{
-		T2 = ( TempDifference * TempDifference ) * 7 / pow( 2, 37 );
-		Off2 = 0.0625 * pow( ( Temperature - 2000.0 ), 2 );
-		Sens2 = 0.0;
-	}
-
-	// Print the temperature results
-
-	Temperature = Temperature / 100;  // Convert to degrees C
-	// Calculate the pressure parameters
-	return Temperature - ( T2 / 100 );
-}
-
-float TemperatureCorrectedPressure( long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] )
-{
-	return TemperatureCorrectedPressure_2( AdcPressure, AdcTemperature, CalConstant );
-}
-
-
-float TemperatureCorrectedPressure_1( long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] )
-{
-	float T2, Off2, Sens2, TempDifference, Temperature;
-	// Calculate the Temperature (first-order computation)
-
-	TempDifference = ( float )( AdcTemperature - ( ( long )CalConstant[5] << 8 ) );
-	Temperature = ( TempDifference * ( float )CalConstant[6] ) / pow( 2, 23 );
-	Temperature = Temperature + 2000;  // This is the temperature in hundredths of a degree C
-
-	// Calculate the second-order offsets
-
-	if( Temperature < 2000.0 ) // Is temperature below or above 20.00 deg C ?
-	{
-		T2 = 3 * pow( TempDifference, 2 ) / pow( 2, 33 );
-		Off2 = 1.5 * pow( ( Temperature - 2000.0 ), 2 );
-		Sens2 = 0.625 * pow( ( Temperature - 2000.0 ), 2 );
-	}
-	else
-	{
-		T2 = ( TempDifference * TempDifference ) * 7 / pow( 2, 37 );
-		Off2 = 0.0625 * pow( ( Temperature - 2000.0 ), 2 );
-		Sens2 = 0.0;
-	}
-
-	float Offset, Sensitivity, Pressure;
-
-	TempDifference = ( float )( AdcTemperature - ( ( long )CalConstant[5] << 8 ) );
-
-	Offset = ( float )CalConstant[2] * pow( 2, 16 );
-	Offset = Offset + ( ( float )CalConstant[4] * TempDifference / pow( 2, 7 ) );
-
-	Sensitivity = ( float )CalConstant[1] * pow( 2, 15 );
-	Sensitivity = Sensitivity + ( ( float )CalConstant[3] * TempDifference / pow( 2, 8 ) );
-
-	// Add second-order corrections
-
-	Offset = Offset - Off2;
-	Sensitivity = Sensitivity - Sens2;
-
-	// Calculate absolute pressure in bars
-
-	Pressure = ( float )AdcPressure * Sensitivity / pow( 2, 21 );
-	Pressure = Pressure - Offset;
-	Pressure = Pressure / pow( 2, 15 );
-	Pressure = Pressure / 10;  // Set output to mbars = hectopascal;
-	return Pressure;
-}
-
-float TemperatureCorrectedPressure_2( long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] )
-{
-	uint32_t     D1 = AdcPressure;    // Store uncompensated pressure value
-	uint32_t     D2 = AdcTemperature;    // Store uncompensated temperature value
-	// These three variables are used for the conversion steps
-	// They should be signed 32-bit integer initially
-	// i.e. signed long from -2147483648 to 2147483647
-	int32_t	dT = 0;
-	int32_t 	TEMP = 0;
-	// These values need to be signed 64 bit integers
-	// (long long = int64_t)
-	int64_t	Offset = 0;
-	int64_t	Sensitivity  = 0;
-	int64_t	T2 = 0;
-	int64_t	OFF2 = 0;
-	int64_t	Sens2 = 0;
-	// Some constants used in calculations below
-	const uint64_t POW_2_33 = 8589934592ULL; // 2^33 = 8589934592
-	const uint64_t POW_2_37 = 137438953472ULL; // 2^37 = 137438953472
-	float mbar; // Store pressure in mbar.
-	float tempC; // Store temperature in degrees Celsius
-	int32_t mbarInt; // pressure in mbar, initially as a signed long integer
-
-	dT = ( int32_t )D2 - ( ( int32_t )CalConstant[5] * 256 );
-	// Use integer division to calculate TEMP. It is necessary to cast
-	// one of the operands as a signed 64-bit integer (int64_t) so there's no
-	// rollover issues in the numerator.
-	TEMP = 2000 + ( ( int64_t )dT * CalConstant[6] ) / 8388608LL;
-	// Recast TEMP as a signed 32-bit integer
-	TEMP = ( int32_t )TEMP;
-
-
-	// All operations from here down are done as integer math until we make
-	// the final calculation of pressure in mbar.
-
-
-	// Do 2nd order temperature compensation (see pg 9 of MS5803 data sheet)
-	// I have tried to insert the fixed values wherever possible
-	// (i.e. 2^31 is hard coded as 2147483648).
-	if( TEMP < 2000 )
-	{
-		// For 30 bar model
-		T2 = 3 * ( ( int64_t )dT * dT ) / POW_2_33 ;
-		T2 = ( int32_t )T2; // recast as signed 32bit integer
-		OFF2 = 3 * ( ( TEMP - 2000 ) * ( TEMP - 2000 ) ) / 2 ;
-		Sens2 = 5 * ( ( TEMP - 2000 ) * ( TEMP - 2000 ) ) / 8 ;
-	}
-	else     // if TEMP is > 2000 (20.0C)
-	{
-		// For 30 bar model
-		T2 = 7 * ( ( int64_t )dT * dT ) / POW_2_37;
-		T2 = ( int32_t )T2; // recast as signed 32bit integer
-		OFF2 = 1 * ( ( TEMP - 2000 ) * ( TEMP - 2000 ) ) / 16;
-		Sens2 = 0;
-	}
-
-	// Additional compensation for very low temperatures (< -15C)
-	if( TEMP < -1500 )
-	{
-		// For 30 bar model
-		OFF2 = OFF2 + 7 * ( ( TEMP + 1500 ) * ( TEMP + 1500 ) );
-		Sens2 = Sens2 + 4 * ( ( TEMP + 1500 ) * ( TEMP + 1500 ) );
-	}
-
-	// Calculate initial Offset and Sensitivity
-	// Notice lots of casts to int64_t to ensure that the
-	// multiplication operations don't overflow the original 16 bit and 32 bit
-	// integers
-	// For 30 bar sensor
-	Offset = ( int64_t )CalConstant[2] * 65536 + ( CalConstant[4] * ( int64_t )dT ) / 128;
-	Sensitivity = ( int64_t )CalConstant[1] * 32768 + ( CalConstant[3] * ( int64_t )dT ) / 256;
-
-	// Adjust TEMP, Offset, Sensitivity values based on the 2nd order
-	// temperature correction above.
-	TEMP = TEMP - T2; // both should be int32_t
-	Offset = Offset - OFF2; // both should be int64_t
-	Sensitivity = Sensitivity - Sens2; // both should be int64_t
-
-	// Final compensated pressure calculation. We first calculate the pressure
-	// as a signed 32-bit integer (mbarInt), then convert that value to a
-	// float (mbar).
-	// For 30 bar sensor
-	mbarInt = ( ( D1 * Sensitivity ) / 2097152 - Offset ) / MS5803_Model_Varient( CalConstant );
-
-	//  Serial.print("D1:");Serial.println(D1);
-	//  Serial.print("D2:");Serial.println(D2);
-	//  Serial.print("Sensitivity:");Serial.println(Sensitivity);
-	//  Serial.print("Offset:");Serial.println(Offset);
-	mbar = ( float )mbarInt / 10;
-
-	// Calculate the human-readable temperature in Celsius
-	tempC  = ( float )TEMP / 100;
-
-
-	//    // Start other temperature conversions by converting mbar to psi absolute
-	//    psiAbs = mbar * 0.0145038;
-	//    // Convert psi absolute to inches of mercury
-	//    inHgPress = psiAbs * 2.03625;
-	//    // Convert psi absolute to gauge pressure
-	//    psiGauge = psiAbs - 14.7;
-	//    // Convert mbar to mm Hg
-	//    mmHgPress = mbar * 0.7500617;
-	//    // Convert temperature to Fahrenheit
-	//    tempF = (tempC * 1.8) + 32;
-	return mbar;
-
-}
-
diff --git a/OpenROV/CMS5837.cpp b/OpenROV/CMS5837.cpp
index 5c62a60..cc18b03 100644
--- a/OpenROV/CMS5837.cpp
+++ b/OpenROV/CMS5837.cpp
@@ -20,7 +20,7 @@ namespace
         CONVERTING_TEMPERATURE
     };
     
-    MS5837_30BA m_sensor( MS5837_RES_4096 );
+    MS5837_30BA m_sensor( MS5837_ADC_4096 );
 
 	CTimer m_initTimer;         // 10s timer for initialization attempts
 	CTimer m_readTimer;         // 50ms timer for generating data points
@@ -28,49 +28,83 @@ namespace
 
 	float m_depth_m         = 0;
 	float m_depthOffset_m   = 0;
+	float m_waterTemp_c     = 0;
+	float m_pressure_mbar   = 0;
 	
-	const int k_maxRetries  = 3;
-	int m_retries           = 0;
+	const int k_maxAttempts = 5;
+	int m_initAttempts      = 0;
 
 	ESensorState m_state    = ESensorState::UNINITIALIZED;
+	
+	void PrintCoefficients()
+	{
+	    for( int i = 0; i < 8; ++i )
+	    {
+        	// Print out coefficients
+    		Serial.print( "MS5837.C" );
+    		Serial.print( i );
+    		Serial.print( ":" );
+    		Serial.print( m_sensorCoeffs[i] );
+    		Serial.println( ";" );
+	    }
+	    
+	    if( m_sensor.m_crcCheckSuccessful )
+	    {
+	        Serial.println( "MS5837.crcCheck:Success;" );
+	    }
+	    else
+	    {
+	        Serial.println( "MS5837.crcCheck:Fail;" );
+	    }
+	}
 }
 
 void CMS5837_30BA::Initialize()
 {
-    // Announce depth capability
-	NConfigManager::m_capabilityBitmask |= ( 1 << DEPTH_CAPABLE );
-	
 	// Attempt to initialize the sensor
-	if( m_sensor.Initialize() )
+	if( m_sensor.Initialize() != 0 )
 	{
+	    // Announce depth capability
+	    NConfigManager::m_capabilityBitmask |= ( 1 << DEPTH_CAPABLE );
+	    
+	    // Print calibration coefficients and CRC check result
+	    PrintCoefficients();
+	    
 	    // Set the state to IDLE, ready to start fetching data
 	    m_state == ESensorState::IDLE;
 	}
 	
 	m_initTimer.Reset();
+	
 }
 
 
 void CMS5837_30BA::Update( CCommand& commandIn )
 {
-    // Handle commands
-    
     if( m_state == ESensorState::UNINITIALIZED )
     {
-        if( m_retries < k_maxRetries )
+        if( m_initAttempts < k_maxAttempts )
         {
             // Make an attempt to initialize the sensor every 10 seconds
             if( m_initTimer.HasElapsed( 10000 ) )
     		{
             	if( m_sensor.Initialize() )
             	{
+            	    m_initAttempts = 0;
+            	    
+            	    // Announce depth capability
+            	    NConfigManager::m_capabilityBitmask |= ( 1 << DEPTH_CAPABLE );
+            	    
+            	    // Print calibration coefficients and CRC check result
+            	    PrintCoefficients();
+            	    
             	    // Set the state to IDLE, ready to start fetching data
             	    m_state == ESensorState::IDLE;
             	}
             	else
             	{
             	    m_initTimer.Reset();
-            	    m_retries++;
+            	    m_initAttempts++;
             	}
     		}
         }
@@ -83,10 +117,12 @@ void CMS5837_30BA::Update( CCommand& commandIn )
             // Zero the depth value
             if( commandIn.Equals( "dzer" ) )
             {
-            	dev.m_depthOffset = dev.m_depth;
+            	m_depthOffset = m_depth_m;
             }
             else if( commandIn.Equals( "dtwa" ) )
             {
+                // TODO: Make this a command to change water type properly
+                
             	// Print water type value
             	if( NConfigManager::m_waterType == WATERTYPE_FRESH )
             	{
@@ -108,7 +144,7 @@ void CMS5837_30BA::Update( CCommand& commandIn )
             // Kick off a conversion every 50ms
             if( m_readTimer.HasElapsed( 50 ) )
     		{
-                m_sensor.StartConversion();
+                m_sensor.StartConversion( MS5837_MEAS_PRESSURE );
                 
                 // Reset the read timer
                 m_readTimer.Reset();
@@ -116,19 +152,56 @@ void CMS5837_30BA::Update( CCommand& commandIn )
                 // Start the conversion timer
                 m_conversionTimer.Reset();
                 
-                m_state = ESensorState::CONVERTING;
+                m_state = ESensorState::CONVERTING_PRESSURE;
     		}
         }
-        else if( m_state == ESensorState::CONVERTING )
+        else if( m_state == ESensorState::CONVERTING_PRESSURE )
         {
             // Wait 20ms for a conversion to complete
-            if( m_conversionTimer.HasElapsed( 20 ) )
+            if( m_conversionTimer.HasElapsed( 10 ) )
     		{
-                m_sensor.Read();
+    		    // Read the pressure value
+                m_sensor.Read( MS5837_MEAS_PRESSURE );
+                
+                // Kick off the temperature conversion
+                m_sensor.StartConversion( MS5837_MEAS_TEMPERATURE );
+                
+                m_conversionTimer.Reset();
+                
+                m_state = ESensorState::CONVERTING_TEMPERATURE;
+    		}
+        }
+        else if( m_state == ESensorState::CONVERTING_TEMPERATURE )
+        {
+            // Wait 20ms for a conversion to complete
+            if( m_conversionTimer.HasElapsed( 10 ) )
+    		{
+    		    // Read the temperature value
+                m_sensor.Read( MS5837_MEAS_TEMPERATURE );
                 
                 // Perform calculations
+                m_sensor.CalculateOutputs();
+                
+                // Get and report results
+                m_depth_m       = m_sensor.m_depth_m - m_depthOffset_m;
+                m_waterTemp_c   = m_sensor.m_temperature_c;
+                m_pressure_mbar = m_sensor.m_pressure_mbar;
                 
-                // Report results
+                Serial.print( "MS5837.depth_m:" );
+    		    Serial.print( m_depth_m );
+    		    Serial.println( ";" );
+    		    
+    		    Serial.print( "MS5837.waterTemp_c:" );
+    		    Serial.print( m_waterTemp_c );
+    		    Serial.println( ";" );
+    		    
+    		    Serial.print( "MS5837.pressure_mbar:" );
+    		    Serial.print( m_pressure_mbar );
+    		    Serial.println( ";" );
+    		    
+    		    NDataManager::m_environmentData.TEMP = m_waterTemp_c;
+    		    NDataManager::m_environmentData.PRES = m_pressure_mbar;
+    		    NDataManager::m_environmentData.DEEP = m_depth_m;
                 
                 m_state = ESensorState::IDLE;
     		}
diff --git a/OpenROV/LibMS5803_XXBA.h b/OpenROV/LibMS5803_XXBA.h
deleted file mode 100644
index d8887df..0000000
--- a/OpenROV/LibMS5803_XXBA.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef __MS5803_14BALIB_H_
-#define __MS5803_14BALIB_H_
-
-
-float CorrectedTemperature(long AdcTemperature, unsigned int CalConstant[8]);
-
-float TemperatureCorrectedPressure(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] );
-
-
-float TemperatureCorrectedPressure_1(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] );
-
-float TemperatureCorrectedPressure_2(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] );
-
-#endif
-
diff --git a/OpenROV/LibMS5837_30BA.cpp b/OpenROV/LibMS5837_30BA.cpp
index a38b2bb..5833650 100644
--- a/OpenROV/LibMS5837_30BA.cpp
+++ b/OpenROV/LibMS5837_30BA.cpp
@@ -6,27 +6,29 @@
 #define I2C_ADDRESS             0x76    // or 0x77
 
 #define CMD_RESET               0x1E
+#define CMD_PROM_READ_BASE      0xA0    // 112 bytes of factory calibration and vendor data
 #define CMD_ADC_READ            0x00
 #define CMD_ADC_CONV_BASE	    0x40	// ADC conversion base command. Modify based on D1/D2 and resolution
 
 #define CMD_ADC_D1              0x00
 #define CMD_ADC_D2              0x10
 
-#define CMD_ADC_256             0x00	// ADC resolution = 256
-#define CMD_ADC_512             0x02	// ADC resolution = 512
-#define CMD_ADC_1024	        0x04	// ADC resolution = 1024
-#define CMD_ADC_2048        	0x06	// ADC resolution = 2048
-#define CMD_ADC_4096	        0x08	// ADC resolution = 4096
-#define CMD_ADC_8192	        0x0A	// ADC resolution = 8192
-
 // TODO: Add more error handling
+// TODO: The casting done in a lot of these calculations is terribly inefficient. Probably better to make more variables default to larger sizes.
+
+MS5837_30BA::MS5837_30BA( uint8_t resolutionIn = MS5837_ADC_4096 )
+{
+    m_oversampleResolution = resolutionIn;
+}
 
 int MS5837_30BA::Initialize()
 {
-    // First Reset
-    Reset();
+    int ret = Reset();
     
+    // TODO: This should be the real test, but we don't know if it will work yet
+    GetCalibrationCoefficients();
     
+    return ret;
 }
 
 int MS5837_30BA::Reset()
@@ -44,125 +46,96 @@ int MS5837_30BA::Reset()
 
 int MS5837_30BA::GetCalibrationCoefficients()
 {
-    bool ret = true;
-	bool gotCRC = true;
-
 	// Read sensor coefficients
-	for( int i = 0; i < 8; i++ )
+	for( int i = 0; i < 8; ++i )
 	{
-		// The PROM starts at address 0xA0
-		if( i != 7 )
+	    // Request data at each PROM register
+	    WriteRegisterByte( 0xA0 + ( i * 2 ) );
+	    
+	    if( I2c.read( I2C_ADDRESS, 2 ) )
 		{
-			ret &= WriteRegisterByte( 0xA0 + ( i * 2 ) );
-		}
-		else
-		{
-			gotCRC &= WriteRegisterByte( 0xA0 + ( i * 2 ) );
-		}
-
-		if( I2c.read( MS5803_I2C_ADDRESS, 2 ) )
-		{
-			// Don't treat the crc read as an overall failure (yet)
-			if( i != 7 )
-			{
-				ret &= false;
-			}
-			else
-			{
-				gotCRC &= false;
-			}
-
-			Serial.print( "Depth.CoeffFailure:C" );
-			Serial.print( i );
-			Serial.println( ";" );
-
-			// Print out coefficients
-			Serial.print( "Depth.C" );
-			Serial.print( i );
-			Serial.println( ":INVALID;" );
-			delay( 10 );
-		}
-		else
-		{
-			// Get coefficient bytes
-			while( I2c.available() )
-			{
-				HighByte = I2c.receive();
-				LowByte = I2c.receive();
-			}
-
-			// Combine bytes
-			sensorCoeffs[i] = ( ( ( unsigned int )HighByte << 8 ) + LowByte );
-
-			// Print out coefficient
-			Serial.print( "Depth.C" );
-			Serial.print( i );
-			Serial.print( ":" );
-			Serial.print( sensorCoeffs[i] );
-			Serial.println( ";" );
-			delay( 10 );
+		    HighByte    = I2c.receive();
+			LowByte     = I2c.receive();
+			
+			// Combine high and low bytes
+			m_sensorCoeffs[i] = ( ( ( uint16_t )HighByte << 8 ) | LowByte );
 		}
+		
+	
 	}
 
-	if( gotCRC == true )
-	{
-		// The last 4 bits of the 7th coefficient form a CRC error checking code.
-		unsigned char p_crc = sensorCoeffs[7];
-		// Use a function to calculate the CRC value
-		unsigned char n_crc = MS_5803_CRC( sensorCoeffs );
-
-		// If the CRC value doesn't match the sensor's CRC value, then the
-		// connection can't be trusted. Check your wiring.
-		if( p_crc != n_crc )
-		{
-			Serial.println( "Depth.crcCheck:Failed;" );
-			//ret &= false;
-		}
-		else
-		{
-			Serial.println( "Depth.crcCheck:Succeeded;" );
-		}
+    // Get the CRC value, which resides in the most significant four bits of C0
+    uint8_t crcRead = m_sensorCoeffs[ 0 ] >> 12;
+    
+    // Calculate the CRC value of the coefficients to make sure they are correct
+    uint8_t crcCalc = CalculateCRC4( m_sensorCoeffs );
+
+    if( crcRead == crcCalc )
+    {
+		
+		m_crcCheckSuccessful = true;
+		return 0;
 	}
 	else
 	{
-		Serial.println( "Depth.crcCheck:CantPerform;" );
+		Serial.println( "MS5837.crcCheck:Fail;" );
+		m_crcCheckSuccessful = false;
+		return 1;
 	}
-
-	if( ret )
-	{
-		// Correct between 14mbar/30mbar sensor type
-		if( sensorCoeffs[0] != 0 )
-		{
-			m_is30bar = true;
-			m_divisor = 8192; // (2^13)
-		}
-		else
-		{
-			m_is30bar = false;
-			m_divisor = 32768; // (2^15)
-		}
-	}
-
-	// Otherwise, return true when everything checks out OK.
-	return ret;
 }
 
-int MS5837_30BA::StartConversion()
+int MS5837_30BA::StartConversion( int measurementTypeIn )
 {
+    // Send the command to do the ADC conversion on the chip, address dependent on measurement type and sampling resolution
+    if( measurementTypeIn == MS5837_MEAS_PRESSURE )
+    {
+	    return WriteRegisterByte( CMD_ADC_CONV + m_oversampleResolution + CMD_ADC_D1 );
+    }
+    else if( MS5837_MEAS_TEMPERATURE )
+    {
+	    return WriteRegisterByte( CMD_ADC_CONV + m_oversampleResolution + CMD_ADC_D2 );
+    }
     
+    return -1;
 }
 
-int MS5837_30BA::Read()
+int MS5837_30BA::Read( int measurementTypeIn )
 {
+    // Send the read command
+	WriteRegisterByte( CMD_ADC_READ );
+
+	// Then request the results. This should be a 24-bit result (3 bytes)
+	I2c.read( I2C_ADDRESS, 3 );
+
+	while( I2c.available() )
+	{
+		HighByte = I2c.receive();
+		MidByte = I2c.receive();
+		LowByte = I2c.receive();
+	}
+
+	// Combine the bytes into one integer
+	result = ( ( uint32_t )HighByte << 16 ) + ( ( uint32_t )MidByte << 8 ) + ( uint32_t )LowByte;
+	
+	// Set the appropriate variable
+	if( measurementTypeIn == MS5837_MEAS_PRESSURE )
+    {
+        D1 = result;
+    }
+    else if( measurementTypeIn == MS5837_MEAS_TEMPERATURE )
+    {
+        D2 = result;
+    }
     
+    return 0;
 }
 
 void MS5837_30BA::SetWaterType( int waterTypeIn )
 {
-    
+    m_waterType = waterTypeIn;
 }
 
-uint8_t MS5837_30BA::GetCRC( uint32_t n_prom[] )
+uint8_t MS5837_30BA::CalculateCRC4( uint16_t n_prom[] )
 {
     int cnt; // simple counter
     uint32_t n_rem=0; // crc remainder
@@ -193,24 +166,56 @@ uint8_t MS5837_30BA::GetCRC( uint32_t n_prom[] )
     return (n_rem ^ 0x00);
 }
 
-uint32_t MS5837_30BA::CommandADC( uint8_t commandIn )
+void MS5837_30BA::CalculateOutputs()
 {
-    // Send the read command
-	WriteRegisterByte( CMD_ADC_READ );
-
-	// Then request the results. This should be a 24-bit result (3 bytes)
-	I2c.read( I2C_ADDRESS, 3 );
-
-	while( I2c.available() )
+    // Calculate base terms
+	dT      = (int32_t)D2 - ( (int32_t)m_sensorCoeffs[5] * POW_2_8 );
+	TEMP    = 2000 + ( ( (int64_t)dT * (int32_t)m_sensorCoeffs[6] ) / POW_2_23 );
+	
+	OFF     = ( (int64_t)m_sensorCoeffs[2] * POW_2_16 ) + ( ( (int64_t)m_sensorCoeffs[4] * dT ) / POW_2_7 );
+	SENS    = ( (int64_t)m_sensorCoeffs[1] * POW_2_15 ) + ( ( (int64_t)m_sensorCoeffs[3] * dT ) / POW_2_8 );
+	
+	// Calculate intermediate values depending on temperature
+	if( TEMP < 2000 )
 	{
-		HighByte = I2c.receive();
-		MidByte = I2c.receive();
-		LowByte = I2c.receive();
+	    // Temps < 20C
+		Ti      = 3 * ( ( int64_t )dT * dT ) / POW_2_33;
+		OFFi    = 3 * ( ( TEMP - 2000 ) * ( TEMP - 2000 ) ) / 2 ;
+		SENSi   = 5 * ( ( TEMP - 2000 ) * ( TEMP - 2000 ) ) / 8 ;
+		
+		// Additional compensation for very low temperatures (< -15C)
+    	if( TEMP < -1500 )
+    	{
+    		// For 14 bar model
+    		OFFi    = OFFi + 7 * ( ( TEMP + 1500 ) * ( TEMP + 1500 ) );
+    		SENSi   = SENSi + 4 * ( ( TEMP + 1500 ) * ( TEMP + 1500 ) );
+    	}
+	}
+	else
+	{
+	    Ti      = 2 * ( ( int64_t )dT * dT ) / POW_2_37;
+		OFFi    = 1 * ( ( TEMP - 2000 ) * ( TEMP - 2000 ) ) / 16;
+		SENSi   = 0;
+	}
+	
+	OFF2    = OFF - OFFi;
+	SENS2   = SENS - SENSi;
+	
+	TEMP2 = (TEMP - Ti);
+	P = ( ( ( ( (int64_t)D1 * SENS2 ) / POW_2_21 ) - OFF2 ) / POW_2_13 );
+	
+	m_temperature_c = (float)TEMP2 / 100.0f;
+	m_pressure_mbar = (float)P / 10.0f;
+	
+	// Calculate depth based on water type
+	if( m_waterType == MS5837_WATERTYPE_FRESH )
+	{
+	    m_depth_m = ( m_pressure_mbar - 1013.25f ) * 1.019716f / 100.0f;
+	}
+	else
+	{
+	    m_depth_m = ( mbar - 1013.25f ) * 0.9945f / 100.0f;
 	}
-
-	// Combine the bytes into one integer
-	result = ( ( uint32_t )HighByte << 16 ) + ( ( uint32_t )MidByte << 8 ) + ( uint32_t )LowByte;
-	return result;
 }
 
 int MS5837_30BA::WriteRegisterByte( uint8_t addressIn )
diff --git a/OpenROV/LibMS5837_30BA.h b/OpenROV/LibMS5837_30BA.h
index 15d5950..94e7001 100644
--- a/OpenROV/LibMS5837_30BA.h
+++ b/OpenROV/LibMS5837_30BA.h
@@ -4,16 +4,19 @@
 #include <Arduino.h>
 
 // Defines
-#define MS5837_RES_256          256u
-#define MS5837_RES_512          512u
-#define MS5837_RES_1024         1024u
-#define MS5837_RES_2048         2048u
-#define MS5837_RES_4096         4096u
-#define MS5837_RES_8192         8192u
-
 #define MS5837_WATERTYPE_FRESH  0
 #define MS5837_WATERTYPE_SALT   1
 
+#define MS5837_MEAS_PRESSURE    0
+#define MS5837_MEAS_TEMPERATURE 1
+
+#define MS5837_ADC_256          0x00	// ADC resolution = 256
+#define MS5837_ADC_512          0x02	// ADC resolution = 512
+#define MS5837_ADC_1024	        0x04	// ADC resolution = 1024
+#define MS5837_ADC_2048        	0x06	// ADC resolution = 2048
+#define MS5837_ADC_4096	        0x08	// ADC resolution = 4096
+#define MS5837_ADC_8192	        0x0A	// ADC resolution = 8192
+
 class MS5837_30BA
 {
 public:
@@ -22,25 +25,28 @@ class MS5837_30BA
     float       m_depth;
 	float       m_depthOffset;
 
-	bool        m_hasValidCoefficients;
+	bool        m_crcCheckSuccessful;
 
 	 // Methods
-	MS5837_30BA( uint16_t resolutionIn = MS5837_RES_4096 );
+	MS5837_30BA( uint8_t resolutionIn = MS5837_ADC_4096 );
 	
 	int Initialize();
 	int GetCalibrationCoefficients();
 	int Reset();
-	int StartPressureConversion();
-	int StartTemperatureConversion();
-	int Read();
+	int StartConversion( int measurementTypeIn );
+	int Read( int measurementType );
+	void CalculateOutputs();
 	
 	void SetWaterType( int waterTypeIn );
 
 private:
 
     // Attributes
+    uint8_t m_oversampleResolution;
+    
 	float m_pressure_mbar;
 	float m_temperature_c;
+	flaot m_depth_m;
 	
 	// Create array to hold the 8 sensor calibration coefficients
     uint16_t m_sensorCoeffs[8]; // unsigned 16-bit integer (0-65535)
@@ -69,16 +75,19 @@ class MS5837_30BA
     uint8_t MidByte;
     uint8_t LowByte;
     
-	uint16_t m_oversampleResolution;
-    
     // Some constants used in calculations below
-    const uint32_t POW_2_13 = 8192u;            // 2^13
-    const uint32_t POW_2_21 = 2097152u;         // 2^21
-    const uint64_t POW_2_33 = 8589934592ULL;    // 2^33
-    const uint64_t POW_2_37 = 137438953472ULL;  // 2^37
+    const int32_t POW_2_7  = 128u;             // 2^7
+    const int32_t POW_2_7  = 256u;             // 2^8
+    const int32_t POW_2_13 = 8192u;            // 2^13
+    const int32_t POW_2_15 = 32768u;           // 2^15
+    const int32_t POW_2_16 = 65536u;           // 2^16
+    const int32_t POW_2_21 = 2097152u;         // 2^21
+    const int32_t POW_2_23 = 8388608u;         // 2^23
+    const int64_t POW_2_33 = 8589934592ULL;    // 2^33
+    const int64_t POW_2_37 = 137438953472ULL;  // 2^37
 	
 	// Methods
-	uint8_t GetCRC( uint32_t n_prom[] );
+	uint8_t CalculateCRC4( uint16_t n_prom[] );
 	uint32_t CommandADC( uint8_t commandIn );
 	
 	int WriteRegisterByte( uint8_t addressIn );

From ffaa57111dab156811d4b2e2e37c2d541e50510e Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Thu, 14 Apr 2016 07:40:14 +0000
Subject: [PATCH 15/22] Fixed some typos and bugs

---
 OpenROV/CMS5837.cpp        |  2 +-
 OpenROV/LibMS5837_30BA.cpp |  4 +---
 OpenROV/LibMS5837_30BA.h   | 17 +++++++----------
 3 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/OpenROV/CMS5837.cpp b/OpenROV/CMS5837.cpp
index cc18b03..6453a37 100644
--- a/OpenROV/CMS5837.cpp
+++ b/OpenROV/CMS5837.cpp
@@ -88,7 +88,7 @@ void CMS5837_30BA::Update( CCommand& commandIn )
             // Make an attempt to initialize the sensor every 10 seconds
             if( m_initTimer.HasElapsed( 10000 ) )
     		{
-            	if( m_sensor.Initialize() )
+            	if( m_sensor.Initialize() != 0 )
             	{
             	    m_initAttempts = 0;
             	    
diff --git a/OpenROV/LibMS5837_30BA.cpp b/OpenROV/LibMS5837_30BA.cpp
index 5833650..9f74a24 100644
--- a/OpenROV/LibMS5837_30BA.cpp
+++ b/OpenROV/LibMS5837_30BA.cpp
@@ -52,7 +52,7 @@ int MS5837_30BA::GetCalibrationCoefficients()
 	    // Request data at each PROM register
 	    WriteRegisterByte( 0xA0 + ( i * 2 ) );
 	    
-	    if( I2c.read( I2C_ADDRESS, 2 ) )
+	    if( I2c.read( I2C_ADDRESS, 2 ) != 0 )
 		{
 		    HighByte    = I2c.receive();
 			LowByte     = I2c.receive();
@@ -60,8 +60,6 @@ int MS5837_30BA::GetCalibrationCoefficients()
 			// Combine high and low bytes
 			m_sensorCoeffs[i] = ( ( ( uint16_t )HighByte << 8 ) | LowByte );
 		}
-		
-	
 	}
 
     // Get the CRC value, which resides in the most significant four bits of C0
diff --git a/OpenROV/LibMS5837_30BA.h b/OpenROV/LibMS5837_30BA.h
index 94e7001..cdc1adf 100644
--- a/OpenROV/LibMS5837_30BA.h
+++ b/OpenROV/LibMS5837_30BA.h
@@ -22,9 +22,10 @@ class MS5837_30BA
 public:
    
     // Attributes
-    float       m_depth;
-	float       m_depthOffset;
-
+    float m_pressure_mbar = 0;
+	float m_temperature_c = 0;
+	float m_depth_m = 0;
+	
 	bool        m_crcCheckSuccessful;
 
 	 // Methods
@@ -43,10 +44,6 @@ class MS5837_30BA
 
     // Attributes
     uint8_t m_oversampleResolution;
-    
-	float m_pressure_mbar;
-	float m_temperature_c;
-	flaot m_depth_m;
 	
 	// Create array to hold the 8 sensor calibration coefficients
     uint16_t m_sensorCoeffs[8]; // unsigned 16-bit integer (0-65535)
@@ -71,9 +68,9 @@ class MS5837_30BA
     int32_t P = 0;          // Temperature compensated pressure
     
     // Bytes to hold the results from I2C communications with the sensor
-    uint8_t HighByte;
-    uint8_t MidByte;
-    uint8_t LowByte;
+    uint8_t HighByte = 0;
+    uint8_t MidByte = 0;
+    uint8_t LowByte = 0;
     
     // Some constants used in calculations below
     const int32_t POW_2_7  = 128u;             // 2^7

From b9527cf96e65cb8f63f5a2608044bf8184771313 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Thu, 14 Apr 2016 08:32:44 +0000
Subject: [PATCH 16/22] Fixed more typos and minor bugs, commented out 50hz
 print stream in MS5837. Tested at the most basic level and it seems to work
 correctly.

---
 OpenROV/{CMS5837.cpp => CMS5837_30BA.cpp} | 78 +++++++++++++----------
 OpenROV/{CMS5837.h => CMS5837_30BA.h}     |  0
 OpenROV/LibMS5837_30BA.cpp                | 16 ++---
 OpenROV/LibMS5837_30BA.h                  | 11 ++--
 OpenROV/ModuleDefinitions.h               |  6 ++
 5 files changed, 63 insertions(+), 48 deletions(-)
 rename OpenROV/{CMS5837.cpp => CMS5837_30BA.cpp} (74%)
 rename OpenROV/{CMS5837.h => CMS5837_30BA.h} (100%)

diff --git a/OpenROV/CMS5837.cpp b/OpenROV/CMS5837_30BA.cpp
similarity index 74%
rename from OpenROV/CMS5837.cpp
rename to OpenROV/CMS5837_30BA.cpp
index 6453a37..f8b3cb1 100644
--- a/OpenROV/CMS5837.cpp
+++ b/OpenROV/CMS5837_30BA.cpp
@@ -12,12 +12,12 @@
 
 namespace
 {
-    enum class ESensorState
+    enum ESensorState
     {
-        UNINITIALIZED,
-        IDLE,
-        CONVERTING_PRESSURE,
-        CONVERTING_TEMPERATURE
+        MS5837_UNINITIALIZED,
+        MS5837_IDLE,
+        MS5837_CONVERTING_PRESSURE,
+        MS5837_CONVERTING_TEMPERATURE
     };
     
     MS5837_30BA m_sensor( MS5837_ADC_4096 );
@@ -34,7 +34,7 @@ namespace
 	const int k_maxAttempts = 5;
 	int m_initAttempts      = 0;
 
-	ESensorState m_state    = ESensorState::UNINITIALIZED;
+	ESensorState m_state    = MS5837_UNINITIALIZED;
 	
 	void PrintCoefficients()
 	{
@@ -44,7 +44,7 @@ namespace
     		Serial.print( "MS5837.C" );
     		Serial.print( i );
     		Serial.print( ":" );
-    		Serial.print( m_sensorCoeffs[i] );
+    		Serial.print( m_sensor.m_sensorCoeffs[i] );
     		Serial.println( ";" );
 	    }
 	    
@@ -62,8 +62,10 @@ namespace
 void CMS5837_30BA::Initialize()
 {
 	// Attempt to initialize the sensor
-	if( m_sensor.Initialize() != 0 )
+	if( m_sensor.Initialize() == 0 )
 	{
+	    Serial.println( "MS5837.init:Success;" );
+	    
 	    // Announce depth capability
 	    NConfigManager::m_capabilityBitmask |= ( 1 << DEPTH_CAPABLE );
 	    
@@ -71,27 +73,31 @@ void CMS5837_30BA::Initialize()
 	    PrintCoefficients();
 	    
 	    // Set the state to IDLE, ready to start fetching data
-	    m_state == ESensorState::IDLE;
+	    m_state = MS5837_IDLE;
+	}
+	else
+	{
+    	Serial.println( "MS5837.init:Fail;" );
+    	m_initTimer.Reset();
 	}
-	
-	m_initTimer.Reset();
-	
 }
 
 
 void CMS5837_30BA::Update( CCommand& commandIn )
 {
-    if( m_state == ESensorState::UNINITIALIZED )
+    if( m_state == MS5837_UNINITIALIZED )
     {
         if( m_initAttempts < k_maxAttempts )
         {
             // Make an attempt to initialize the sensor every 10 seconds
-            if( m_initTimer.HasElapsed( 10000 ) )
+            if( m_initTimer.HasElapsed( 5000 ) )
     		{
-            	if( m_sensor.Initialize() != 0 )
+            	if( m_sensor.Initialize() == 0 )
             	{
             	    m_initAttempts = 0;
             	    
+            	    Serial.println( "MS5837.init:Success;" );
+            	    
             	    // Announce depth capability
             	    NConfigManager::m_capabilityBitmask |= ( 1 << DEPTH_CAPABLE );
             	    
@@ -99,10 +105,12 @@ void CMS5837_30BA::Update( CCommand& commandIn )
             	    PrintCoefficients();
             	    
             	    // Set the state to IDLE, ready to start fetching data
-            	    m_state == ESensorState::IDLE;
+            	    m_state = MS5837_IDLE;
             	}
             	else
             	{
+            	    Serial.println( "MS5837.init:Fail;" );
+            	    
             	    m_initTimer.Reset();
             	    m_initAttempts++;
             	}
@@ -117,7 +125,7 @@ void CMS5837_30BA::Update( CCommand& commandIn )
             // Zero the depth value
             if( commandIn.Equals( "dzer" ) )
             {
-            	m_depthOffset = m_depth_m;
+            	m_depthOffset_m = m_depth_m;
             }
             else if( commandIn.Equals( "dtwa" ) )
             {
@@ -139,7 +147,7 @@ void CMS5837_30BA::Update( CCommand& commandIn )
         
         // TODO: Handle error checking for sensor actions, since they can fail due to I2C problems
         // Handle sensor update process
-        if( m_state == ESensorState::IDLE )
+        if( m_state == MS5837_IDLE )
         {
             // Kick off a conversion every 50ms
             if( m_readTimer.HasElapsed( 50 ) )
@@ -152,10 +160,10 @@ void CMS5837_30BA::Update( CCommand& commandIn )
                 // Start the conversion timer
                 m_conversionTimer.Reset();
                 
-                m_state = ESensorState::CONVERTING_PRESSURE;
+                m_state = MS5837_CONVERTING_PRESSURE;
     		}
         }
-        else if( m_state == ESensorState::CONVERTING_PRESSURE )
+        else if( m_state == MS5837_CONVERTING_PRESSURE )
         {
             // Wait 20ms for a conversion to complete
             if( m_conversionTimer.HasElapsed( 10 ) )
@@ -168,10 +176,10 @@ void CMS5837_30BA::Update( CCommand& commandIn )
                 
                 m_conversionTimer.Reset();
                 
-                m_state = ESensorState::CONVERTING_TEMPERATURE;
+                m_state = MS5837_CONVERTING_TEMPERATURE;
     		}
         }
-        else if( m_state == ESensorState::CONVERTING_TEMPERATURE )
+        else if( m_state == MS5837_CONVERTING_TEMPERATURE )
         {
             // Wait 20ms for a conversion to complete
             if( m_conversionTimer.HasElapsed( 10 ) )
@@ -187,23 +195,23 @@ void CMS5837_30BA::Update( CCommand& commandIn )
                 m_waterTemp_c   = m_sensor.m_temperature_c;
                 m_pressure_mbar = m_sensor.m_pressure_mbar;
                 
-                Serial.print( "MS5837.depth_m:" );
-    		    Serial.print( m_depth_m );
-    		    Serial.println( ";" );
-    		    
-    		    Serial.print( "MS5837.waterTemp_c:" );
-    		    Serial.print( m_waterTemp_c );
-    		    Serial.println( ";" );
-    		    
-    		    Serial.print( "MS5837.pressure_mbar:" );
-    		    Serial.print( m_pressure_mbar );
-    		    Serial.println( ";" );
+                //Serial.print( "MS5837.depth_m:" );
+                //Serial.print( m_depth_m );
+                //Serial.println( ";" );
+                
+                //Serial.print( "MS5837.waterTemp_c:" );
+                //Serial.print( m_waterTemp_c );
+                //Serial.println( ";" );
+                
+                //Serial.print( "MS5837.pressure_mbar:" );
+                //Serial.print( m_pressure_mbar );
+                //Serial.println( ";" );
     		    
     		    NDataManager::m_environmentData.TEMP = m_waterTemp_c;
     		    NDataManager::m_environmentData.PRES = m_pressure_mbar;
-    		    NDataManager::m_environmentData.DEEP = m_depth_m;
+    		    NDataManager::m_navData.DEEP = m_depth_m;
                 
-                m_state = ESensorState::IDLE;
+                m_state = MS5837_IDLE;
     		}
         }
     }
diff --git a/OpenROV/CMS5837.h b/OpenROV/CMS5837_30BA.h
similarity index 100%
rename from OpenROV/CMS5837.h
rename to OpenROV/CMS5837_30BA.h
diff --git a/OpenROV/LibMS5837_30BA.cpp b/OpenROV/LibMS5837_30BA.cpp
index 9f74a24..8139a3e 100644
--- a/OpenROV/LibMS5837_30BA.cpp
+++ b/OpenROV/LibMS5837_30BA.cpp
@@ -16,7 +16,7 @@
 // TODO: Add more error handling
 // TODO: The casting done in a lot of these calculations is terribly inefficient. Probably better to make more variables default to larger sizes.
 
-MS5837_30BA::MS5837_30BA( uint8_t resolutionIn = MS5837_ADC_4096 )
+MS5837_30BA::MS5837_30BA( uint8_t resolutionIn )
 {
     m_oversampleResolution = resolutionIn;
 }
@@ -50,9 +50,9 @@ int MS5837_30BA::GetCalibrationCoefficients()
 	for( int i = 0; i < 8; ++i )
 	{
 	    // Request data at each PROM register
-	    WriteRegisterByte( 0xA0 + ( i * 2 ) );
+	    WriteRegisterByte( CMD_PROM_READ_BASE + ( i * 2 ) );
 	    
-	    if( I2c.read( I2C_ADDRESS, 2 ) != 0 )
+	    if( I2c.read( I2C_ADDRESS, 2 ) == 0 )
 		{
 		    HighByte    = I2c.receive();
 			LowByte     = I2c.receive();
@@ -87,11 +87,11 @@ int MS5837_30BA::StartConversion( int measurementTypeIn )
     // Send the command to do the ADC conversion on the chip, address dependent on measurement type and sampling resolution
     if( measurementTypeIn == MS5837_MEAS_PRESSURE )
     {
-	    return WriteRegisterByte( CMD_ADC_CONV + m_oversampleResolution + CMD_ADC_D1 );
+	    return WriteRegisterByte( CMD_ADC_CONV_BASE + m_oversampleResolution + CMD_ADC_D1 );
     }
     else if( MS5837_MEAS_TEMPERATURE )
     {
-	    return WriteRegisterByte( CMD_ADC_CONV + m_oversampleResolution + CMD_ADC_D2 );
+	    return WriteRegisterByte( CMD_ADC_CONV_BASE + m_oversampleResolution + CMD_ADC_D2 );
     }
     
     return -1;
@@ -113,7 +113,7 @@ int MS5837_30BA::Read( int measurementTypeIn )
 	}
 
 	// Combine the bytes into one integer
-	result = ( ( uint32_t )HighByte << 16 ) + ( ( uint32_t )MidByte << 8 ) + ( uint32_t )LowByte;
+	uint32_t result = ( ( uint32_t )HighByte << 16 ) + ( ( uint32_t )MidByte << 8 ) + ( uint32_t )LowByte;
 	
 	// Set the appropriate variable
 	if( measurementTypeIn == MS5837_MEAS_PRESSURE )
@@ -212,7 +212,7 @@ void MS5837_30BA::CalculateOutputs()
 	}
 	else
 	{
-	    m_depth_m = ( mbar - 1013.25f ) * 0.9945f / 100.0f;
+	    m_depth_m = ( m_pressure_mbar - 1013.25f ) * 0.9945f / 100.0f;
 	}
 }
 
@@ -229,7 +229,7 @@ int MS5837_30BA::WriteDataByte( uint8_t addressIn, uint8_t dataIn )
 int MS5837_30BA::ReadByte( uint8_t addressIn, uint8_t& dataOut )
 {
     // Set address to request from
-	uint8_t ret = I2c.read( ( uint8_t )MS5803_I2C_ADDRESS, ( uint8_t )addressIn, ( uint8_t )1 );
+	uint8_t ret = I2c.read( ( uint8_t )I2C_ADDRESS, ( uint8_t )addressIn, ( uint8_t )1 );
 
 	// Non-zero failure
 	if( ret )
diff --git a/OpenROV/LibMS5837_30BA.h b/OpenROV/LibMS5837_30BA.h
index cdc1adf..f969354 100644
--- a/OpenROV/LibMS5837_30BA.h
+++ b/OpenROV/LibMS5837_30BA.h
@@ -26,7 +26,11 @@ class MS5837_30BA
 	float m_temperature_c = 0;
 	float m_depth_m = 0;
 	
-	bool        m_crcCheckSuccessful;
+	bool m_crcCheckSuccessful = false;
+	
+	int m_waterType;
+	
+	uint16_t m_sensorCoeffs[8]; // unsigned 16-bit integer (0-65535)
 
 	 // Methods
 	MS5837_30BA( uint8_t resolutionIn = MS5837_ADC_4096 );
@@ -44,9 +48,6 @@ class MS5837_30BA
 
     // Attributes
     uint8_t m_oversampleResolution;
-	
-	// Create array to hold the 8 sensor calibration coefficients
-    uint16_t m_sensorCoeffs[8]; // unsigned 16-bit integer (0-65535)
     
     uint32_t D1 = 0;        // Digital pressure value
     uint32_t D2 = 0;        // Digital temperature value
@@ -74,7 +75,7 @@ class MS5837_30BA
     
     // Some constants used in calculations below
     const int32_t POW_2_7  = 128u;             // 2^7
-    const int32_t POW_2_7  = 256u;             // 2^8
+    const int32_t POW_2_8  = 256u;             // 2^8
     const int32_t POW_2_13 = 8192u;            // 2^13
     const int32_t POW_2_15 = 32768u;           // 2^15
     const int32_t POW_2_16 = 65536u;           // 2^16
diff --git a/OpenROV/ModuleDefinitions.h b/OpenROV/ModuleDefinitions.h
index f17fe67..842e494 100644
--- a/OpenROV/ModuleDefinitions.h
+++ b/OpenROV/ModuleDefinitions.h
@@ -70,6 +70,12 @@ CAltServo altservo1;
 CMS5803_14BA m_depthSensor;
 #endif
 
+#if(HAS_MS5837_30BA)
+#define DEPTH_ENABLED 1
+#include "CMS5837_30BA.h"
+CMS5837_30BA m_depthSensor;
+#endif
+
 #if(DEADMANSWITCH_ON)
 #include "CDeadManSwitch.h"
 CDeadManSwitch m_deadManSwitch;

From 53ded472a492b229a6880629238d800afb1fc68b Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Thu, 14 Apr 2016 08:41:02 +0000
Subject: [PATCH 17/22] Reduced initial startup delay to 1s from 10s

---
 OpenROV/NArduinoManager.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/OpenROV/NArduinoManager.cpp b/OpenROV/NArduinoManager.cpp
index af6a118..9d6df74 100644
--- a/OpenROV/NArduinoManager.cpp
+++ b/OpenROV/NArduinoManager.cpp
@@ -11,7 +11,7 @@ namespace NArduinoManager
 
 	void Initialize()
 	{
-		delay( 10000 );
+		delay( 1000 );
 
 		// Hardware intialization
 		DisableWatchdogTimer();

From e205571c81e774e232eebd2f2525518b4f28efe7 Mon Sep 17 00:00:00 2001
From: root <root@openrov.localdomain>
Date: Thu, 14 Apr 2016 18:18:36 +0000
Subject: [PATCH 18/22] Removed browser ping requirement from BNO055
 initialization, added watertype set in MS5837, added deep print to data
 manager to support cockpit 30.1.x+

---
 OpenROV/CBNO055.cpp      | 17 ++++-------------
 OpenROV/CMS5837_30BA.cpp |  2 ++
 OpenROV/NDataManager.cpp |  3 +++
 3 files changed, 9 insertions(+), 13 deletions(-)

diff --git a/OpenROV/CBNO055.cpp b/OpenROV/CBNO055.cpp
index 4e2530e..930bd33 100644
--- a/OpenROV/CBNO055.cpp
+++ b/OpenROV/CBNO055.cpp
@@ -13,8 +13,6 @@ namespace
 	CTimer imuTimer;
 
 	bool initalized				= false;
-	bool browserPingReceived	= false;
-
 	bool inFusionMode			= false;
 
 	CAdaBNO055 bno;
@@ -56,16 +54,12 @@ void CBNO055::Initialize()
 	bno055_sample_timer.Reset();
 	report_timer.Reset();
 	imuTimer.Reset();
+	
+	InitializeSensor();
 }
 
 void CBNO055::Update( CCommand& commandIn )
 {
-
-	if( commandIn.Equals( "ping" ) )
-	{
-		browserPingReceived = true;
-	}
-
 	if( commandIn.Equals( "imumode" ) )
 	{
 		if( initalized && commandIn.m_arguments[ 0 ] != 0 )
@@ -106,11 +100,8 @@ void CBNO055::Update( CCommand& commandIn )
 			// Attempt every 10 secs
 			if( report_timer.HasElapsed( 10000 ) )
 			{
-				if( browserPingReceived )
-				{
-					// Attempt to initialize the chip again
-					InitializeSensor();
-				}
+				// Attempt to initialize the chip again
+				InitializeSensor();
 			}
 
 			return;
diff --git a/OpenROV/CMS5837_30BA.cpp b/OpenROV/CMS5837_30BA.cpp
index f8b3cb1..2deafc5 100644
--- a/OpenROV/CMS5837_30BA.cpp
+++ b/OpenROV/CMS5837_30BA.cpp
@@ -135,11 +135,13 @@ void CMS5837_30BA::Update( CCommand& commandIn )
             	if( NConfigManager::m_waterType == WATERTYPE_FRESH )
             	{
             		NConfigManager::m_waterType = WATERTYPE_SALT;
+            		m_sensor.SetWaterType( MS5837_WATERTYPE_SALT );
             		Serial.println( F( "dtwa:1;" ) );
             	}
             	else
             	{
             		NConfigManager::m_waterType = WATERTYPE_FRESH;
+            		m_sensor.SetWaterType( MS5837_WATERTYPE_FRESH );
             		Serial.println( F( "dtwa:0;" ) );
             	}
             }
diff --git a/OpenROV/NDataManager.cpp b/OpenROV/NDataManager.cpp
index b79e730..441a847 100644
--- a/OpenROV/NDataManager.cpp
+++ b/OpenROV/NDataManager.cpp
@@ -36,6 +36,9 @@ namespace NDataManager
 		Serial.print( F( "deap:" ) );
 		Serial.print( m_navData.DEEP );
 		Serial.print( ';' );
+		Serial.print( F( "deep:" ) );		// Compatibility for 30.1.x cockpit updates
+		Serial.print( m_navData.DEEP );
+		Serial.print( ';' );
 		Serial.print( F( "pitc:" ) );
 		Serial.print( m_navData.PITC );
 		Serial.print( ';' );

From 01d74036a597190458ce2f9a8ae33070a86ca918 Mon Sep 17 00:00:00 2001
From: root <root@openrov.localdomain>
Date: Sun, 17 Apr 2016 06:45:57 +0000
Subject: [PATCH 19/22] Added compatibility for heading lock and depth lock
 commands from 30.1.x cockpit

---
 OpenROV/CAutopilot_STD.cpp | 198 +++++++++++++++++++------------------
 1 file changed, 100 insertions(+), 98 deletions(-)

diff --git a/OpenROV/CAutopilot_STD.cpp b/OpenROV/CAutopilot_STD.cpp
index 2c67058..e79a7ec 100644
--- a/OpenROV/CAutopilot_STD.cpp
+++ b/OpenROV/CAutopilot_STD.cpp
@@ -48,127 +48,129 @@ void CAutopilot::Update( CCommand& command )
 {
 	//intended to respond to fly by wire commands: MaintainHeading(); TurnTo(compassheading); DiveTo(depth);
 
-
-
-	if( command.Equals( "headloff" ) )
-	{
-		_headingHoldEnabled = false;
-		raw_Left = 0;
-		raw_Right = 0;
-		hdg_Error_Integral = 0;  // Reset error integrator
-		tgt_Hdg = -500;  // -500 = system not in hdg hold
-
-		int m_argumentsToSend[] = {1, 00}; //include number of parms as fist parm
-		command.PushCommand( "yaw", m_argumentsToSend );
-		Serial.println( F( "log:heading_hold_disabled;" ) );
-		Serial.print( F( "targetHeading:" ) );
-		Serial.print( DISABLED );
-		Serial.println( ';' );
-
-	}
-
-	if( command.Equals( "headlon" ) )
-	{
-		_headingHoldEnabled = true;
-
-		if( command.m_arguments[0] == 0 )
-		{
-			_headingHoldTarget = NDataManager::m_navData.HDGD;
-		}
-		else
-		{
-			_headingHoldTarget = command.m_arguments[1];
-		}
-
-		tgt_Hdg = _headingHoldTarget;
-		Serial.print( F( "log:heading_hold_enabled on=" ) );
-		Serial.print( tgt_Hdg );
-		Serial.println( ';' );
-		Serial.print( F( "targetHeading:" ) );
-		Serial.print( tgt_Hdg );
-		Serial.println( ';' );
-	}
-
-	//Backwards compatibility for a release or two (2.5.1 release)
-	if( command.Equals( "holdHeading_toggle" ) )
+	if( NCommManager::m_isCommandAvailable )
 	{
-		if( _headingHoldEnabled )
+		
+		if( command.Equals( "headloff" ) || command.Equals( "holdHeading_off" )  )
 		{
-			int m_argumentsToSend[] = {0}; //include number of parms as fist parm
-			command.PushCommand( "headloff", m_argumentsToSend );
+			_headingHoldEnabled = false;
+			raw_Left = 0;
+			raw_Right = 0;
+			hdg_Error_Integral = 0;  // Reset error integrator
+			tgt_Hdg = -500;  // -500 = system not in hdg hold
+	
+			int m_argumentsToSend[] = {1, 00}; //include number of parms as fist parm
+			command.PushCommand( "yaw", m_argumentsToSend );
+			Serial.println( F( "log:heading_hold_disabled;" ) );
+			Serial.print( F( "targetHeading:" ) );
+			Serial.print( DISABLED );
+			Serial.println( ';' );
+	
 		}
-		else
+	
+		if( command.Equals( "headlon" ) || command.Equals( "holdHeading_off" ) )
 		{
+			_headingHoldEnabled = true;
+	
 			if( command.m_arguments[0] == 0 )
 			{
-				int m_argumentsToSend[] = {0}; //include number of parms as fist parm
-				command.PushCommand( "headlon", m_argumentsToSend );
+				_headingHoldTarget = NDataManager::m_navData.HDGD;
 			}
 			else
 			{
-				int m_argumentsToSend[] = {1, command.m_arguments[1]}; //include number of parms as fist parm
-				command.PushCommand( "headlon", m_argumentsToSend );
+				_headingHoldTarget = command.m_arguments[1];
 			}
-
+	
+			tgt_Hdg = _headingHoldTarget;
+			Serial.print( F( "log:heading_hold_enabled on=" ) );
+			Serial.print( tgt_Hdg );
+			Serial.println( ';' );
+			Serial.print( F( "targetHeading:" ) );
+			Serial.print( tgt_Hdg );
+			Serial.println( ';' );
 		}
-	}
-
-	if( command.Equals( "deptloff" ) )
-	{
-		_depthHoldEnabled = false;
-		raw_lift = 0;
-		target_depth = 0;
-
-		int m_argumentsToSend[] = {1, 0}; //include number of parms as fist parm
-		command.PushCommand( "lift", m_argumentsToSend );
-		Serial.println( F( "log:depth_hold_disabled;" ) );
-		Serial.print( F( "targetDepth:" ) );
-		Serial.print( DISABLED );
-		Serial.println( ';' );
-
-	}
-
-	if( command.Equals( "deptlon" ) )
-	{
-		_depthHoldEnabled = true;
-
-		if( command.m_arguments[0] == 0 )
+	
+		//Backwards compatibility for a release or two (2.5.1 release)
+		if( command.Equals( "holdHeading_toggle" ) )
 		{
-			_depthHoldTarget = NDataManager::m_navData.DEEP * 100; //casting to cm
+			if( _headingHoldEnabled )
+			{
+				int m_argumentsToSend[] = {0}; //include number of parms as fist parm
+				command.PushCommand( "headloff", m_argumentsToSend );
+			}
+			else
+			{
+				if( command.m_arguments[0] == 0 )
+				{
+					int m_argumentsToSend[] = {0}; //include number of parms as fist parm
+					command.PushCommand( "headlon", m_argumentsToSend );
+				}
+				else
+				{
+					int m_argumentsToSend[] = {1, command.m_arguments[1]}; //include number of parms as fist parm
+					command.PushCommand( "headlon", m_argumentsToSend );
+				}
+	
+			}
 		}
-		else
+	
+		if( command.Equals( "deptloff" ) || command.Equals( "holdDepth_off" ) )
 		{
-			_depthHoldTarget = command.m_arguments[1];
+			_depthHoldEnabled = false;
+			raw_lift = 0;
+			target_depth = 0;
+	
+			int m_argumentsToSend[] = {1, 0}; //include number of parms as fist parm
+			command.PushCommand( "lift", m_argumentsToSend );
+			Serial.println( F( "log:depth_hold_disabled;" ) );
+			Serial.print( F( "targetDepth:" ) );
+			Serial.print( DISABLED );
+			Serial.println( ';' );
+	
 		}
-
-		target_depth = _depthHoldTarget;
-		Serial.print( F( "log:depth_hold_enabled on=" ) );
-		Serial.print( target_depth );
-		Serial.println( ';' );
-		Serial.print( F( "targetDepth:" ) );
-		Serial.print( target_depth );
-		Serial.println( ';' );
-	}
-
-
-	if( command.Equals( "holdDepth_toggle" ) )
-	{
-		if( _depthHoldEnabled )
+	
+		if( command.Equals( "deptlon" ) || command.Equals( "holdDepth_on" ) )
 		{
-			int m_argumentsToSend[] = {0}; //include number of parms as fist parm
-			command.PushCommand( "deptloff", m_argumentsToSend );
+			_depthHoldEnabled = true;
+	
+			if( command.m_arguments[0] == 0 )
+			{
+				_depthHoldTarget = NDataManager::m_navData.DEEP * 100; //casting to cm
+			}
+			else
+			{
+				_depthHoldTarget = command.m_arguments[1];
+			}
+	
+			target_depth = _depthHoldTarget;
+			Serial.print( F( "log:depth_hold_enabled on=" ) );
+			Serial.print( target_depth );
+			Serial.println( ';' );
+			Serial.print( F( "targetDepth:" ) );
+			Serial.print( target_depth );
+			Serial.println( ';' );
 		}
-		else
+	
+	
+		if( command.Equals( "holdDepth_toggle" ) )
 		{
-			if( command.m_arguments[0] == 0 )
+			if( _depthHoldEnabled )
 			{
 				int m_argumentsToSend[] = {0}; //include number of parms as fist parm
-				command.PushCommand( "deptlon", m_argumentsToSend );
+				command.PushCommand( "deptloff", m_argumentsToSend );
 			}
 			else
 			{
-				int m_argumentsToSend[] = {1, command.m_arguments[1]}; //include number of parms as fist parm
-				command.PushCommand( "deptlon", m_argumentsToSend );
+				if( command.m_arguments[0] == 0 )
+				{
+					int m_argumentsToSend[] = {0}; //include number of parms as fist parm
+					command.PushCommand( "deptlon", m_argumentsToSend );
+				}
+				else
+				{
+					int m_argumentsToSend[] = {1, command.m_arguments[1]}; //include number of parms as fist parm
+					command.PushCommand( "deptlon", m_argumentsToSend );
+				}
 			}
 		}
 	}

From 5f573e432f0b3e0b8b629eae63773e9e70b58518 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Sun, 17 Apr 2016 01:30:47 +0000
Subject: [PATCH 20/22] Fixed typo with heading hold

---
 OpenROV/CAutopilot_STD.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/OpenROV/CAutopilot_STD.cpp b/OpenROV/CAutopilot_STD.cpp
index e79a7ec..29b0572 100644
--- a/OpenROV/CAutopilot_STD.cpp
+++ b/OpenROV/CAutopilot_STD.cpp
@@ -68,7 +68,7 @@ void CAutopilot::Update( CCommand& command )
 	
 		}
 	
-		if( command.Equals( "headlon" ) || command.Equals( "holdHeading_off" ) )
+		if( command.Equals( "headlon" ) || command.Equals( "holdHeading_on" ) )
 		{
 			_headingHoldEnabled = true;
 	

From 05e7ca7f585a24f78db8eac19121b9930ef331a9 Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Thu, 26 May 2016 17:45:11 -0700
Subject: [PATCH 21/22] Removed external lights from base firmware. Moved to
 plugins

---
 OpenROV/CLights.cpp       | 8 +-------
 OpenROV/SystemConstants.h | 1 -
 2 files changed, 1 insertion(+), 8 deletions(-)

diff --git a/OpenROV/CLights.cpp b/OpenROV/CLights.cpp
index d6a9e76..29bf66f 100644
--- a/OpenROV/CLights.cpp
+++ b/OpenROV/CLights.cpp
@@ -21,7 +21,6 @@
 namespace
 {
 	CPin light( "light", LIGHTS_PIN, CPin::kAnalog, CPin::kOutput );
-	CPin elight( "elight", ELIGHTS_PIN, CPin::kAnalog, CPin::kOutput );
 }
 
 void CLights::Initialize()
@@ -30,9 +29,6 @@ void CLights::Initialize()
 
 	light.Reset();
 	light.Write( 0 );
-
-	elight.Reset();
-	elight.Write( 0 );
 }
 
 void CLights::Update( CCommand& commandIn )
@@ -61,9 +57,7 @@ void CLights::Update( CCommand& commandIn )
 		Serial.print( F( "LIGP:" ) );
 		Serial.print( percentValue );
 		Serial.println( ';' );
-	}
-
-	
+	}   
 }
 
 #endif
diff --git a/OpenROV/SystemConstants.h b/OpenROV/SystemConstants.h
index ea5fe53..dd35269 100644
--- a/OpenROV/SystemConstants.h
+++ b/OpenROV/SystemConstants.h
@@ -40,7 +40,6 @@
 #define STARBOARD_PIN			8
 #define ESCPOWER_PIN			16
 #define ALTSERVO_PIN			9
-#define ELIGHTS_PIN				46
 #define I2CPOWER_PIN			48
 #define PORT_FORWARD_PIN 9
 #define PORT_AFT_PIN 6

From ee2ed27610e168ba0e41b37a3bd6874d4411f12a Mon Sep 17 00:00:00 2001
From: Charles Cross <charles@openrov.com>
Date: Tue, 31 May 2016 13:58:33 -0700
Subject: [PATCH 22/22] Fix for automatic plugin inclusion from cockpit

---
 OpenROV/AConfig.h           | 1 +
 OpenROV/ModuleDefinitions.h | 2 ++
 OpenROV/PluginConfig.h      | 1 +
 3 files changed, 4 insertions(+)
 create mode 100644 OpenROV/PluginConfig.h

diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h
index 0ce7aeb..7adfb1f 100644
--- a/OpenROV/AConfig.h
+++ b/OpenROV/AConfig.h
@@ -26,6 +26,7 @@
 #endif
 
 #define HAS_STD_LIGHTS (1)
+#define HAS_EXT_LIGHTS (1)
 #define HAS_STD_CALIBRATIONLASERS (1)
 #define HAS_STD_CAMERAMOUNT (1)
 #define HAS_STD_AUTOPILOT (1)
diff --git a/OpenROV/ModuleDefinitions.h b/OpenROV/ModuleDefinitions.h
index 842e494..0f97c4b 100644
--- a/OpenROV/ModuleDefinitions.h
+++ b/OpenROV/ModuleDefinitions.h
@@ -1,6 +1,8 @@
 #pragma once
 #include "AConfig.h"
 
+#include "PluginConfig.h"
+
 // ---------------------------------------
 // Conditional module definitions
 // -------------------------------
diff --git a/OpenROV/PluginConfig.h b/OpenROV/PluginConfig.h
new file mode 100644
index 0000000..35d79f2
--- /dev/null
+++ b/OpenROV/PluginConfig.h
@@ -0,0 +1 @@
+// Do not delete! This gets automatically populated at compile time with additional plugins.
\ No newline at end of file