Monday, December 21, 2015

How to Calibrate Water Speed for Vessel Heel/Sensor Angle (Hull Speed Through Water, NMEA 0183 VHW)

After a few rounds of ops testing my multiplexer, I've discovered that my readings are routinely off for the current. To test this, I sailed to known (predicted) current points and compared it to the calculated current from the King Tide Sailing (KTS) system. Here are two such sample points (070@2.40 means 70 degrees true at 2.40 knots):

Predicted Current: 070@2.40 knots. Calculated Current: 070.53@4.65 knots. 

Predicted Current: 060@2.28 knots. Calculated: 073.84@2.77 knots. Also I am .6 feet above the water?
The direction is pretty close (the second measurement is only 10 degrees off), but the first velocity measurement is off by a factor of almost 2. That's unacceptable, and needs to be corrected to satisfy my desire for good instruments.

Paddle Wheel Water/Hull Speed Sensor Calibration

My paddlewheel is part of a triducer that measures depth, temp, and speed (as well as a log, but that doesn't seem to work). However, because I screwed up and didn't fully investigate where to install my new sensor, I could only install it at a 23 degree angle from the vertical underneath a settee. That's really unfortunate, because the depth reading is actually saying the water is deeper than it really is due to the slant distance for the beam, but I actually correct for this offset using the KTS system (keep in mind this is only correct if the water floor is flat). But this also means my paddle wheel is off its desired axis by 23 degrees. Perhaps that means there needs to be another correction for offset angle to derive the actual speed. Which is true, because as the vessel speeds up, the boundary layer against the hull (where the sensor is) actually flows faster and faster than the actual speed of the vessel through the water. Vessel roll/heel has ta similar effect.

I found this handy website that does just that for their own sensor, with the table reproduced below.

Heelº Boat Speed (Knots)
5 10 15 20 25 30
0.0 -2.0 -3.9 -6.0 -7.8 -9.3
10º -0.2 -2.3 -4.0 -6.5 -9.6 -11.0
20º -0.4 -3.9 -6.1 -8.5 -11.5 -13.3

But how do you incorporate this table into the Arduino? It's more than just a linear relationship; at 0 degrees of heel (roll), the sensor error doesn't change at a constant rate. As with anything involving fluid dynamics, most relationships are either trigonometric (cosines and sines) or exponential. Fortunately, after dusting off a few college textbooks, I figured out how to derive a function for use in the Arduino code to correct for boat speed and roll.

Equation to Correct Water Speed for Vessel Speed & Heel/Roll

The key for interpolating between those points in the data table above lies in a Lagrangian polynomial. For those who didn't take any advanced mathematics classes, a Lagrangian polynomial is an equation that accurately describes a curve based on a few given data points. For us, those data points are given in the table (at 10 degrees of heel and at 15 knots, the correction is -4.0 knots). So we use the Lagrangian polynomial to calculate the correction for anywhere other than those data points (say, for 18.56 degrees of roll and 6.23 knots). Deriving the polynomial is left as an exercise to the reader, if you so choose to burn a weekend trying to figure it out. Or you could just keep reading.

For my vessel, I chose to only go up to 10 knots. Why? The theoretical hull speed of my 35 foot sailboat is 6.86 knots (~25' waterline). When would I ever be going faster than 10 knots?

However, the mathematically educated reader would point out that if you came up with a single equation to describe the table, you would actually get a positive correction between 0 and 5 knots, instead of a negative correction. This has to do with the polynomials, and blah blah blah, long story short is that you only use the Lagrangian equation to derive the corrections between 5 and 10 knots. For anything less than 5, it's actually a linear relationship. I know that's in direct contradiction with what I wrote earlier, but looking at the table, the corrections at 5 knots go from 0 to -0.2 to -0.4, and that's it--that's -0.2 knots for every 10 degrees of roll at 5 or less knots. For speeds under 5 knots, it really is a linear relationship.

So, on to the actual equations.

The first step is to figure out the corrections per degree of roll. This is done by doing a quick polynomial for the 5 and 10 knots columns (remember, I'm only going up to 10 knots because I'll never go faster than that through the water). Basically, here's the table I'm trying to fill in:

Obviously, at 0 knots, the correction is 0. But the first thing I need to do is fill in the 10 knot column and the 5 knot column. Then I can fill in horizontally from that. The 5 knot column is easy: it's a linear relationship, so that's solved by the simple equation "y = -.02*roll" and that's it. For the 10 knot column, we actually use a linear relationship between 0 degrees roll and 10 degrees roll because if we used the polynomial, the correction factor goes in the wrong direction. So that linear relationship is "y = -.03*roll - 2" all the way down to 10 degrees roll. So to figure out the polynomial between 10 and 20 degrees roll (and, in fact, beyond that), we come up with the following:

where x is degrees of roll. This produces the following graph:

where you can see that for a little bit, as the x-axis increases (roll), the correction factor goes the wrong way. As roll increases, the correction should always decrease, but it doesn't until about ~3 degrees or so. That's why I use a linear relationship until 10 degrees of roll, and then I pick up that equation, which, thanks to Wolfram Alpha, can be reduced down to this:


where x is roll in degrees. So now, given all this information, we can continue to fill in our table.


Now we need to fill in the horizontal rows, or the actual correction based on vessel speed. We use a similar method as before (linear for 5 and below knots (y=fiveknotcorrection*waterspeed/5), polynomial for more than 5 knots). Using the same equation format, we get something that looks like this (see the EDIT at the bottom for a link to the Wolfram Alpha post):


where x is the speed in knots, a is the 0 knot correction, b is the 5 knot correction, and c is the 10 knot correction. Since the 0 knot correction is always 0, we can eliminate that first term (multiplying anything by zero equals zero). This gives us a simplified equation as follows:


Using our new equation, we can fill in the rest of the rows based on our 5 and 10 knot corrections already calculated. This gives us the following table:


Of course, now that we have actual equations, we can calculate the correction values for up to 90 degrees of roll (which, at 10 knots, gives you a correction factor of -51.50 knots). So now how do we incorporate this into the Arduino code?

The Arduino Code for Vessel Water/Hull Speed Calibration/Correction

The first step is to figure out the 5 and 10 knot correction values for the vessel's roll. Remember, we use a linear equation for 5 knots and 10 knots (below 10 degrees roll), and then a polynomial for 10 knots (above 10 degrees roll). This is a function of roll, mind you. Once we know what our 5 knot and 10 knot corrections are for whatever roll/heel our vessel is presently at, we can plug those into the second formula, which is a function of speed. Remember, this is a linear equation up to 5 knots, and then the polynomial for anything above 5 knots.

 // VHW Vessel Water Speed Sentence //  
    if (strcmp(title,"VWVHW") == 0) {    
     wspworking = 0; // resets the "waterspeed is bad" counter back to 0 since it's good  
     float wsp0 = nmeaDecoder.term_decimal(5);  
     float wsp1 = wsp0;  
     if (wsp0 > 10){ // just in case the speed is above 10, we reset it back to 10  
      wsp1 = 10;     // so as not to get any weird correction values  
     }  
     float pwa = abs(tda - rll); // corrects for the triducer angle offset    
     float fkc = -.02*pwa;       // 5-knot correction for vessel roll  
     float tkc = -.03*pwa-2;     // 10-knot correction for vessel roll  
     if (pwa > 10){  
      tkc = pwa*(.035-.0065*pwa)-2;  
     }  
     float wspcor = fkc*wsp1/5;  
     if (wsp1>5){  
      wspcor = (wsp1*(tkc*(wsp1-5)-2*fkc*(wsp1-10)))/50;  
     }  
     wsp = wsp0+wspcor;  
     if (outputvhw == true) {    
      char vhwSentence [49];    
      byte csw;    
      PString strw(vhwSentence, sizeof(vhwSentence));    
      strw.print("$VWVHW,");    
      strw.print(hdt);    
      strw.print(",T,");    
      strw.print(hdm);    
      strw.print(",M,");    
      strw.print(wsp);    
      strw.print(",N,");    
      strw.print(wsp*1.852);    
      strw.print(",K*");    
      csw = checksum(vhwSentence);    
      if (csw < 0x10) strw.print('0');    
      strw.print(csw, HEX);    
      Serial.println(vhwSentence);    
      }    
    }  

where wsp0 is the raw waterspeed data, tda is triducer angle offset from the verticle (mine is slanted 23 degrees to starboard--this is necessary because at 23 degrees roll to starboard, the actual roll experience on the paddle wheel is 0), rll is vessel roll, and wspcor is the correction factor (always negative). If, for some reason, my boat is surfing along at faster than 10 knots, I included a quick little code (for wsp1) to only derive corrections as if the speed were only 10 knots. This is because we didn't include any data points from

Now all that's left is to test it out and see how it works. Hopefully, the new code will also include a 10 hz refresh rate for headings/attitude information, but I have to wait until the New Year to try it out. Expect a full write up and a new KTS Source Code then.

EDIT: I was curious to see the differences if I did the polynomial out to 15 knots instead of ending at 10. Here's the new equation for step one (calculating the 15 knot corrections where x is the roll):


which further simplifies to:



Now that we know what our 0, 5, 10, and 15 knot corrections are based on our roll, we can input that info to the following polynomial, which is a function based on speed (here is the Wolfram Alpha link if you want to play around with it):


which further simplifies to:


where b is the 5 knot correction, c is the 10 knot, and d is the 15 knot correction. As you can see, this can get messy really quick. When I plugged it into my table, the results got... weird, to say the least. I checked a few points, and basically sought to minimize the jump from 5 to 6 knots, since that's where it goes from linear to polynomial. If we include the 15 knot corrections, the jump increases by quite a bit more than just the 5/10 knot corrections--I don't like this, so I'm going to keep the method described originally (without a 15 knot correction factor).

Thursday, October 8, 2015

A Wireless Arduino NMEA-0183 Multiplexer


Let's put all this together. Using the previous posts with the "multiplexer" label, it's quite easy to set up an Arduino that combines multiple instruments, filters the numbers and calculates new data, and outputs a single stream of NMEA-0183 sentences usable by any chart plotter. What you see in the above picture is quite useful: You have an Arduino that computes the sentences, which then pipes that stream over to the Raspberry Pi. When power is applied, the Raspberry Pi automatically starts a Wi-Fi network with my boat's name and runs a program called kplex, which is basically a simple multiplexer program for the Raspberry Pi that can take inputs and output over Wi-Fi. This little box (which is a $7 jewelry box from Target) sits inside my nav desk, flush with two of the sides so that it points straight forward. Then, up above, I open my laptop, start OpenCPN, connect to the Wi-Fi, and bam! It starts receiving the NMEA sentences through kplex.

Recommended Gear

You'll have to follow my other guide for connecting NMEA-0183 devices to the Arduino.

Arduino Code for an NMEA-0183 Multiplexer

The code is pasted below, but I know how these things can be. Something I'm using doesn't quite match up with what you're finding. Libraries are especially susceptible to these problems. So while the code is pasted below, I'm also including the libraries and sketches in a zip file for you to download, which is identical to the files I'm using (that work). Keep in mind, though, that these libraries will probably go out of date, and may not always work if the Arduino IDE gets updated. Use at your own risk. Remember, you must calibrate the MPU-9150 for this to work (the sketch is included in this zip); follow my write up on it to get started.
 ////////////////////////////////////////////////////////////////////////////   
  //   
  // Welcome to the King Tide Sailing Multiplexer!   
  //   
  // Visit kingtidesailing.blogspot.com for more information   
  //   
  // This file is part of RTIMULib-Arduino   
  //   
  // Copyright (c) 2014-2015, richards-tech   
  //   
  // Permission is hereby granted, free of charge, to any person obtaining a copy of    
  // this software and associated documentation files (the "Software"), to deal in    
  // the Software without restriction, including without limitation the rights to use,    
  // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the    
  // Software, and to permit persons to whom the Software is furnished to do so,    
  // subject to the following conditions:   
  //   
  // The above copyright notice and this permission notice shall be included in all    
  // copies or substantial portions of the Software.   
  //   
  // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,    
  // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A    
  // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    
  // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION    
  // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE    
  // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.   
  //   
  ////////////////////////////////////////////////////////////////////////////   
     
  #include <Wire.h>   
  #include "I2Cdev.h"   
  #include "RTIMUSettings.h"   
  #include "RTIMU.h"   
  #include "RTFusionRTQF.h"    
  #include "CalLib.h"   
  #include <EEPROM.h>   
  #include <PString.h>   
  #include <nmea.h>   
     
  RTIMU *imu;       // the IMU object   
  RTFusionRTQF fusion;   // the fusion object   
  RTIMUSettings settings; // the settings object   
  NMEA gps(GPRMC);     // GPS data connection to decode RMC sentence types (otherwise the serial output becomes too buggy if you decode all GPS sentences)   
  NMEA nmeaDecoder(ALL);  // NMEA data decoder for all sentence types   
     
  // if you don't want to include a sentence, then just make the appropriate variable false    
  // it is recommended that you print only the minimum sentences desired to keep the Serial Port from getting too busy    
  boolean outputgps = true; // gps   
  boolean outputhsp = true; // waterspeed (hereafter referred to as hullspeed)   
  boolean outputdpt = true; // depth   
  boolean outputtmp = true; // water temp   
  boolean outputvlw = true; // distance through water   
  boolean outputhdm = false; // magnetic heading    
  boolean outputhdt = true; // true heading    
  boolean outputrot = false; // rate of turn    
  boolean outputshr = false; // pitch and roll   
  boolean outputvdr = true; // set and drift   
  boolean outputawv = false; // apparent wind velocity   
  boolean outputtwv = false; // true wind velocity   
     
  // User defined variables (for your own boat and geographic location):   
  float magd = -13.73; // magnetic deviation; if it's East, it's negative (13.73E in San Francisco 2015)   
  float tdo = -1.17; // depth sounder offset from waterline (positive) or from bottom of keel (negative) (in meters)   
  float tda = 23.0;  // depth sounder angle from straight down (in degrees)   
  float anvo = 0;   // vertical offset of anemometer from center of buoyancy   
  float anlo = 0;   // longitudinal offset of anemometer from center of buoyancy   
     
  // Variables preset with recommended values (but can be changed):   
  #define DISPLAY_INTERVAL 500 // rate (in milliseconds) in which MPU9150 results are displayed   
  int gpsworking = 20; // this ticks up everytime the MPU runs, which is defined in DISPLAY INTERVAL and is used as a    
  int hsworking = 20;  // counter so that if no GPS or Waterspeed data is received for 10 seconds, then VDR won't calculate  
  int imuworking = 5; // counter so that if the imu isn't working, then no wind corrections will be applied   
    
  // Global variables:   
  unsigned long lastDisplay;   
  unsigned long lastRate;   
  int sampleCount;   
  float r = M_PI/180.0f;   
  float d = 180.0f/M_PI;   
  boolean vlwfirst = true;   
  float tcd0;   
     
  // Variables exchanged between instruments:   
  float ts;   // timestamp of GPS signal in UTC   
  float sog;  // speed over ground   
  float cmg;  // course made good   
  float hs;   // water speed, or hull speed through the water   
  float hdm;  // magnetic heading   
  float hdt;  // true heading   
  float roll;  // vessel roll   
  float pitch; // vessel pitch   
  float yaw;  // vessel yaw (270 = 0, negative is left)   
  float ror;  // rate of roll (negative is left)   
  float rop;  // rate of pitch (negative is nose down)   
  float rot;  // rate of turn (negative is left)   
     
  void setup()   
  {   
   int errcode;   
     
   Serial.begin(4800);  // output port to computer   
   Serial1.begin(4800); // from GPS   
   Serial2.begin(4800); // from DST-800 transducer   
   Serial3.begin(4800); // from anemometer   
   Wire.begin();     // from MPU-9150   
   imu = RTIMU::createIMU(&settings);   
     
   // Checking the IMU, if it fails, simply restarting the connection seems to work sometimes   
   if ((errcode = imu->IMUInit()) < 0) {   
    Serial.print("Failed to init IMU: "); Serial.println(errcode);   
   }   
   if (imu->getCalibrationValid())   
    Serial.println("Using compass calibration");   
   else   
    Serial.println("No valid compass calibration data");   
     
   lastDisplay = lastRate = millis();   
   sampleCount = 0;   
     
   // Slerp power controls the fusion and can be between 0 and 1   
   // 0 means that only gyros are used, 1 means that only accels/compass are used   
   // In-between gives the fusion mix. 0.02 recommended.   
      
   fusion.setSlerpPower(0.02);   
      
   // use of sensors in the fusion algorithm can be controlled here   
   // change any of these to false to disable that sensor   
      
   fusion.setGyroEnable(true);   
   fusion.setAccelEnable(true);   
   fusion.setCompassEnable(true);   
  }   
     
  // calculate checksum function (thanks to https://mechinations.wordpress.com)   
  byte checksum(char* str)    
  {   
   byte cs = 0;   
   for (unsigned int n = 1; n < strlen(str) - 1; n++)    
   {   
    cs ^= str[n];   
   }   
   return cs;   
  }   
     
  void loop()   
  {    
     
  // ***** BU-353 GPS ***** //   
   if (Serial1.available()) {   
   if (gps.decode(Serial1.read())) {   
    if (outputgps == true) {   
     if (gps.term_decimal(7) == 0.0) { // this removes artifacts in cmg when sog = 0   
     char rmcSentence [71];    
     byte csg;    
     PString strg(rmcSentence, sizeof(rmcSentence));    
     strg.print("$GPRMC,");   
     strg.print(gps.term_decimal(1));   
     strg.print(",");   
     strg.print(gps.term(2));   
     strg.print(",");   
     strg.print(gps.term_decimal(3));   
     strg.print(",");   
     strg.print(gps.term(4));   
     strg.print(",");   
     strg.print(gps.term_decimal(5));   
     strg.print(",");   
     strg.print(gps.term(6));   
     strg.print(",");   
     strg.print(gps.term_decimal(7));   
     strg.print(",");   
     strg.print(hdt);   
     strg.print(",");   
     strg.print(gps.term_decimal(9));   
     strg.print(",,,");   
     strg.print(gps.term(12));   
     strg.print("*");   
     csg = checksum(rmcSentence);    
     if (csg < 0x10) strg.print('0');    
     strg.print(csg, HEX);    
     Serial.println(rmcSentence);   
     }   
     else {   
     Serial.println(gps.sentence());   
     }   
    }   
    sog = gps.term_decimal(7);   
    cmg = gps.term_decimal(8);   
    ts = gps.term_decimal(1);   
    gpsworking = 0; // resets the "gps is bad" counter back to 0 since it's good   
    }   
   }   
  // ***** End BU-353 GPS ***** //   
     
  // ***** DST-800 Transducer ***** //   
   if (Serial2.available() > 0 ) {   
   if (nmeaDecoder.decode(Serial2.read())) {   
    char* title = nmeaDecoder.term(0);   
    if (strcmp(title,"VWVHW") == 0) {   
    hs = nmeaDecoder.term_decimal(5);   
    hsworking = 0; // resets the "hullspeed is bad" counter back to 0 since it's good   
    if (outputhsp == true) {   
     char vhwSentence [49];    
     byte csw;    
     PString strw(vhwSentence, sizeof(vhwSentence));    
     strw.print("$VWVHW,");    
     strw.print(hdt);   
     strw.print(",T,");   
     strw.print(hdm);   
     strw.print(",M,");   
     strw.print(hs);   
     strw.print(",N,");   
     strw.print(hs*1.852);   
     strw.print(",K*");    
     csw = checksum(vhwSentence);    
     if (csw < 0x10) strw.print('0');    
     strw.print(csw, HEX);    
     Serial.println(vhwSentence);   
    }   
    }   
    if (outputdpt == true) {   
    if (strcmp(title,"SDDPT") == 0) {   
     char dptSentence [22];    
     byte csd;    
     PString strd(dptSentence, sizeof(dptSentence));    
     strd.print("$SDDPT,");    
     strd.print(nmeaDecoder.term_decimal(1)*cos(tda*r));   
     strd.print(",");   
     strd.print(tdo);   
     strd.print("*");    
     csd = checksum(dptSentence);    
     if (csd < 0x10) strd.print('0');    
     strd.print(csd, HEX);    
     Serial.println(dptSentence);   
    }   
    }   
    if (outputtmp == true) {   
    if (strcmp(title,"YXMTW") == 0) {   
     Serial.println(nmeaDecoder.sentence());   
    }   
    }   
    if (outputvlw == true) {   
    if (strcmp(title,"VWVLW") == 0) {   
     float tcd = nmeaDecoder.term_decimal(1);   
     if (vlwfirst == true) {   
     float tcd0 = tcd;   
     vlwfirst = false;   
     }   
     float dsr = tcd-tcd0;   
     char vlwSentence [23];    
     byte csl;    
     PString strl(vlwSentence, sizeof(vlwSentence));    
     strl.print("$VWVLW,");    
     strl.print(tcd);   
     strl.print(",N,");   
     strl.print(dsr);   
     strl.print(",N*");    
     csl = checksum(vlwSentence);    
     if (csl < 0x10) strl.print('0');    
     strl.print(csl, HEX);    
     Serial.println(vlwSentence);   
    }   
    }   
    }   
  }   
  // ***** End DST-800 Transducer ***** //   
     
  // ***** Integrated Navigation (MPU-9150 and derivatives) ***** //   
   unsigned long now = millis();   
   unsigned long delta;   
   int loopCount = 1;   
      
   while (imu->IMURead()) {   
    // this flushes remaining data in case we are falling behind   
    if (++loopCount >= 10)   
     continue;   
    fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());   
    sampleCount++;   
    if ((delta = now - lastRate) >= 1000) {     
     sampleCount = 0;   
     lastRate = now;   
    }   
    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {   
     imuworking = 0; // since the imu is working, this resets the counter to 0  
     lastDisplay = now;    
     RTVector3 pose = fusion.getFusionPose();    
     RTVector3 gyro = imu->getGyro();    
     roll = pose.y()*-1*d;    
     pitch = pose.x()*d;    
     yaw = pose.z()*d;    
     ror = gyro.y()*d;   
     rop = gyro.x()*d;   
     rot = gyro.z()*d;    
     hdm = yaw-90;  // converts yaw to magnetic heading   
     if (yaw < 90 && yaw >= -179.99) {    
     hdm = yaw+270;    
     }    
     hdt = hdm-magd; // calculate true heading    
     if (hdt > 360) {    
     hdt = hdt-360;    
     }    
     if (hdt < 0.0) {    
     hdt = hdt+360;    
     }   
     
     // HDM Magnetic Heading //   
     if (outputhdm == true){    
     char hdmSentence [26];    
     byte csm;    
     PString strm(hdmSentence, sizeof(hdmSentence));    
     strm.print("$HCHDM,");    
     strm.print(hdm);    
     strm.print(",M*");    
     csm = checksum(hdmSentence);    
     if (csm < 0x10) strm.print('0');    
     strm.print(csm, HEX);    
     Serial.println(hdmSentence);    
     }   
        
     // HDT True Heading //   
     if (outputhdt == true){    
     char hdtSentence [26];    
     byte cst;    
     PString strt(hdtSentence, sizeof(hdtSentence));    
     strt.print("$HCHDT,");    
     strt.print(hdt);    
     strt.print(",T*");    
     cst = checksum(hdtSentence);    
     if (cst < 0x10) strt.print('0');    
     strt.print(cst, HEX);    
     Serial.println(hdtSentence);    
     }    
      
     // ROT Rate of Turn //   
     if (outputrot == true){    
     char rotSentence [21];    
     byte csr;    
     PString strr(rotSentence, sizeof(rotSentence));    
     strr.print("$TIROT,");    
     strr.print(rot);    
     strr.print(",A*");    
     csr = checksum(rotSentence);    
     if (csr < 0x10) strr.print('0');    
     strr.print(csr, HEX);    
     Serial.println(rotSentence);    
     }    
        
     // SHR Pitch and Roll (no heave... yet...)//   
     if (outputshr == true){    
     char shrSentence [50];    
     byte csp;    
     PString strp(shrSentence, sizeof(shrSentence));    
     strp.print("$INSHR,,");    
     strp.print(hdt);    
     strp.print(",T,");    
     strp.print(roll);    
     strp.print(",");    
     strp.print(pitch);    
     strp.print(",,,,,1,0*");    
     csp = checksum(shrSentence);    
     if (csp < 0x10) strp.print('0');    
     strp.print(csp, HEX);    
     Serial.println(shrSentence);    
     }   
        
     // VDR Set & Drift //   
     if (outputvdr == true) {   
     if (gpsworking < 20 && hsworking < 20) {   
      float set0 = (cmg-hdt)*r; // gets the difference between hdt and cmg in radians   
      if (set0 < 0) {   
      set0 = set0+(2*M_PI);   
      }   
      float drift = sqrt(pow(hs,2)+pow(sog,2)-2*hs*sog*(cos(set0)));   
      float set1 = (M_PI-atan2((sog*sin(set0)),(hs-sog*cos(set0))))*d;   
      if (hs == 0 && sog == 0) { // if you're not moving, then set = 0   
      set1 = set0;   
      }   
      float set = hdt+set1;   // converting to true instead of relative degrees   
      if (set > 360) {   
      set = set-360;   
      }   
      float setmag = set+magd;  // converting to magnetic instead of true   
      if (setmag > 360) {    
      setmag = setmag-360;    
      }    
      if (setmag < 0.0) {    
      setmag = setmag+360;    
      }   
     
      char vdrSentence [49];    
      byte csv;   
      PString strv(vdrSentence, sizeof(vdrSentence));   
      strv.print("$INVDR,");   
      strv.print(set);   
      strv.print(",T,");   
      strv.print(setmag);   
      strv.print(",M,");   
      strv.print(drift);   
      strv.print(",N*");   
      csv = checksum(vdrSentence);   
      if (csv < 0x10) strv.print('0');   
      strv.print(csv, HEX);   
      Serial.println(vdrSentence);  
      gpsworking =+ 1;     // this ticks up the timer to make sure we only calculate  
      hsworking =+ 1;      // VDR if we get good gps and dst-800 data   
     }   
     }   
    }   
   }   
   // ***** End Integrated Navigation ***** //   
     
   // ***** Anemometer ***** //   
   if (outputawv == true || outputtwv == true) {   
    if (Serial3.available() > 0 ) {    
    if (nmeaDecoder.decode(Serial3.read())) {  
    if (imuworking >= 5) {            // if there's no imu data, then just  
       Serial.println(nmeaDecoder.sentence()); // print the raw sentence with no corrections  
       imuworking = 5;  
    }  
    else {  
    imuworking =+ 1;  
    float ftk = .592484;            // feet per second to knots    
    float rwa0 = nmeaDecoder.term_decimal(1); // the raw relative wind angle    
    float ws0 = nmeaDecoder.term_decimal(3);  // the raw uncorrected wind speed    
    int quad = floor(rwa0/90);         // this gets which quadrant the wind is coming from (0,1,2,3)    
       
    // ITERATION ONE of corrections, for rotational velocity of pitch/roll/yaw    
    float sor = ror*r*anvo*ftk; // this assumes your anemometer has little or no lateral offset    
    float sop = rop*r*sqrt(pow(anvo,2)+pow(anlo,2))*ftk;    
    float sot = rot*r*anlo*ftk;    
    float wx0 = ws0*sin(rwa0*r);    
    float wy0 = ws0*cos(rwa0*r);    
    float wx1 = wx0-sor-sot;  // assume starboard tack or...    
    if (rwa0 > 180.0) {     // port    
     wx1 = wx0+sor+sot;    
    }    
    float wy1 = wy0+sop;        // assume beating upwind...    
    if (rwa0 < 270.0 && rwa0 > 90.0) { // or downwind    
     wy1 = wy0-sop;    
    }    
    // Now we have wx1 and wy1, the x and y wind components corrected for the movement of the mast    
      
    // ITERATION TWO of corrections, for absolute roll and pitch of the boat (yaw has no effect)    
    float wx2 = wx1/cos(roll*r);    
    float wy2 = wy1/cos(pitch*r);    
    float aws = sqrt(pow(wx2,2)+pow(wy2,2));    
    float awa = (atan2(wx2,wy2)*d)+90*quad;    
    if (wy2 == 0.0) { // this is for when there is no wind, and you can't divide by zero    
     awa = rwa0;    
    }    
    if (quad == 1 || quad == 3) { // we have to flip the atan function if the wind is coming from quadrant 1 or 3    
     awa = (atan2(wy2,wx2)*d)+90*quad;    
     if (wx2 == 0.0) {    
     awa = rwa0;    
     }    
    }    
    if (awa < 0) {    
     awa = awa+360;    
    }    
    if (awa > 360) {    
     awa = awa-360;    
    }    
    // Now we have aps and awa, the relative (or apparent) wind speed/angle corrected for boat roll and pitch    
      
    // ITERATION THREE of corrections, compensating for the boat's speed over ground    
    float twa0 = awa+hdt;    
    if (twa0 > 360) {    
     twa0 = twa0-360;    
    }    
    float dwa = twa0-cmg;    
    if (dwa < 0) {    
     dwa = dwa+360;    
    }    
    float tws = sqrt(pow(sog,2)+pow(aws,2)-2*aws*sog*cos(dwa*r));    
    float rwa1 = (M_PI-atan2((aws*sin(dwa*r)),(sog-aws*cos(dwa*r))))*d;    
    if (sog == 0.0 && aws == 0.0) {    
     rwa1 = awa;    
    }    
    float twa = cmg+rwa1;    
    if (twa > 360) {    
     twa = twa-360;    
    }    
    // Now we have tws, the true windspeed corrected for how fast the boat is rolling, pitching, and yawing, and for its roll/pitch angle, and for its induced wind    
    // We also have twa, which is the corrected true wind angle of ws, and rwa, the corrected relative wind angle from the boat's bow    
      
    // MWV sentence for apparent wind   
    if (outputawv == true) {    
    char mwvSentence [30];    
    byte csm;    
    PString strm(mwvSentence, sizeof(mwvSentence));    
    strm.print("$WIMWV,");    
    strm.print(awa);    
    strm.print(",R,");    
    strm.print(aws);    
    strm.print(",N,A*");    
    csm = checksum(mwvSentence);    
    if (csm < 0x10) strm.print('0');    
    strm.print(csm, HEX);    
    Serial.println(mwvSentence);   
    }   
      
    // MWV Sentence for true wind   
    if (outputtwv == true) {   
    char mwdSentence [30];    
    byte csd;    
    PString strd(mwdSentence, sizeof(mwdSentence));    
    strd.print("$WIMWV,");    
    strd.print(twa);    
    strd.print(",T,");    
    strd.print(tws);    
    strd.print(",N,A*");    
    csd = checksum(mwdSentence);    
    if (csd < 0x10) strd.print('0');    
    strd.print(csd, HEX);    
    Serial.println(mwdSentence);  
    }   
    }    
    }    
    }   
   }   
   // ***** End Anemometer ***** //   
     
  }  

Instructions for the King Tide Sailing Arduino NMEA-0183 Multiplexer

Which Sentences to Include

Let's just work our way down from the top. First up, you will need to alter these boolean variables to select which sentences to output (boolean simple means a variable that contains either TRUE or FALSE).


It's pretty self-explanatory; if you want to print out a sentence, make sure it's boolean is "true" instead of "false." My setup has a few caveats, however. For one, I'm using the mighty Airmar DST-800 Thru-Hull Triducer Smart Sensor, which outputs quite a few sentences (hullspeed, depth, water temp, and voyage distance). Also, I'm using the GlobalSat BU-353 USB GPS Receiver following David's Guide to connect it to the Arduino. This spits out way more NMEA-0183 sentences than I care for; so I used the NMEA Library to only accept the RMC sentence (which gives you time, position, course, and speed).

The reason for this is because the MPU-9150 is running every half second; and if I accepted all the GPS sentences, it takes longer than half a second to print them all. So what happened was that the GPS Serial port would start printing the sentences, and then the MPU-9150 would cut it off and print its own. It's pretty easy to get a little too cluttered on the one output Serial port, so I recommend only using the minimum number of sentences to output. The only benefit I can see to having the other GPS sentences is if you have some application that needs to know how many satellites are in view; for instance, navigating in dangerous waters where you need to know if you can rely on your GPS position. Or perhaps certain DSC VHF radios need those other sentences. If you wish to include them, you must change the code such that instead of only including RMC sentences, it includes ALL sentences; that would require quite a bit of change, and is left as an exercise to the reader if he or she so chooses.

Vessel Information

Next up is the information specific to your vessel; geographic location for magnetic deviation, depth sounder offset, and anemometer offset.


The variable "magd" is simply your magnetic deviation. You will have to update this whenever you move somewhere where the value changes. Not sure what your magnetic deviation is? Check out this page, and it'll tell you precisely what it is. If the value is given as "East" such as 14E, then type in "magd = -14.0" since east values are considered to be negative values.

"tdo" is simply the transducer offset from either the waterline or bottom of the keel. If you want the absolute depth of water from the surface of the water, enter in the positive distance between the waterline on your boat and the location of the depth sounder. If you want the depth below the keel (like me), then enter in the distance between the depth sounder and the bottom of the keel as a negative number. Also, this is measured in meters, not feet.

"tda" is the angle offset of the depth sounder. Most people have it pointed straight down (as it should be), but if you're like me, you may have been forced to place the depth sounder at an angle from straight down. Simply enter this angle here in degrees (mine is pointing 23 degrees to starboard). This will compensate for the offset, assuming the ocean/lake/bay floor is flat.

"anvo" and "anlo" are the vertical and longitudinal offsets of the anemometer, respectively. Here is a picture of my boat pulled from http://sailboatdata.com/ with colored lines added to measure both numbers: 

The green circle represents the center of buoyancy (at least a rough estimate). It's approximately 2/3 the way forward of the keel, and I just guessed for the vertical location. The redline is the vertical offset, and the blue line is the longitudinal offset. If you know the length of the waterline of your boat, then you can do a quick comparison to figure out how long the offsets are. For mine, the anvo is approximately 53 feet, and the anlo is approximately 5 feet.

How Often to Calculate Certain Sentences


DISPLAY_INTERVAL is how often the MPU-9150 will run its script, measured in milliseconds. So a value of 500 means it will run twice per second, which is a pretty good resolution without clogging the Serial port or taxing the system. I recommend leaving it at 500, but you can tweak with this and see if you can get it working smoothly at faster rates. Keep in mind if you change this, though, you will have to change the next three variables.

"gpsworking," and "hsworking," are simply counters that tick up every time the MPU runs. Every time the GPS outputs a sentence, it resets the counter back to 0. Same thing with the hullspeed. When they reach 20 (so after 10 seconds, since the MPU runs every half second), then VDR won't calculate because you are no longer receiving GPS data or hullspeed data.

"imuworking," follows the same idea, only kind of backwards; if the IMU isn't working (the MPU-9150), then this counter ticks up each time the wind anemometer outputs a sentence. So basically, the anemometer will output 5 sentences, and if the IMU hasn't ran in that time, it will stop correcting for pitch/roll/yaw, and will only spit out the raw relative wind sentence.

Final Thoughts

This works fine, good enough for most day sailing/racing. But you may want to add some increased functionality based on your needs. What I would like to do next is output every sentence, including all the GPS sentences, in a way that doesn't clog up the Serial port. I think this is possible by using the PString library, so that anytime a sentence comes in from its respective Serial port, the script appends the sentence to a giant array of sentences instead of repeating it immediately to the output Serial port. Then at the end of the whole script, it would print the sentence every time the MPU runs. 

But it works fine now, and the next step is to writeup how to get the Raspberry Pi working to pipe the sentences wirelessly, and to get a Kindle to display the information over Wi-Fi. But I also think it's quite easy to get too deep in the weeds with this, and spend a lot of time and resources trying to perfect this sturdy little device.

Sometimes, it might just be better to go out sailing.

Wednesday, October 7, 2015

Correcting NMEA-0183 Wind for Vessel Roll, Pitch, and Yaw



EDIT July 2016: I should point out that this is mainly for fun; practically speaking, a simple dampening will work just fine. Most anemometers that use cups and the spinning arrow won't register instantaneous changes, as this code assumes. For example, as a boat rolls starboard, the arrow will gradually swing to reflect that, but the code thinks it happens immediately. Also, you have upwash from the sails which will affect the sensor. All in all, this is just a fun post that I worked on when I was deployed and had a lot of free time. Enjoy.

A simple wind anemometer only measures wind blowing orthogonal to its axis of rotation. If you take a normal anemometer and tilt it 90 degrees to the side, it will stop working because there is no more wind that can turn it (there is plenty of wind passing it, though). Imagine a water wheel at a mill, only placed so that the water hits the side of the wheel instead of its buckets.

Of course, as you allow more and more wind (or water) to hit the anemometer (or wheel), the more it will spin until it is fully lined up correctly with the wind (water) direction of travel. This follows a simple trigonometric function based on the cosine of the angle between the axis of rotation and the axis of wind (which is parallel with Earth's surface).

See the following sketch to try and visualize what I mean.


The Arduino Code to Calculate Wind Corrections for Vessel Movement in NMEA-0183

This follows the theme of recent posts and assumes you have the MPU-9150 setup and working, and are using it's roll, pitch, and yaw functions for your vessel. You must also already have your wind instrument connected and working with the Arduino, following my guide here. Here is the code, with the explanation to follow:

 ////////////////////////////////////////////////////////////////////////////  
 //  
 // This file is part of RTIMULib-Arduino  
 //  
 // Copyright (c) 2014-2015, richards-tech  
 //  
 // Permission is hereby granted, free of charge, to any person obtaining a copy of   
 // this software and associated documentation files (the "Software"), to deal in   
 // the Software without restriction, including without limitation the rights to use,   
 // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the   
 // Software, and to permit persons to whom the Software is furnished to do so,   
 // subject to the following conditions:  
 //  
 // The above copyright notice and this permission notice shall be included in all   
 // copies or substantial portions of the Software.  
 //  
 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   
 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A   
 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT   
 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION   
 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE   
 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.  
 //  
 // Further modified by King Tide Sailing (kingtidesailing.blogspot.com)  
 //  
 ////////////////////////////////////////////////////////////////////////////  
   
 #include <Wire.h>  
 #include "I2Cdev.h"  
 #include "RTIMUSettings.h"  
 #include "RTIMU.h"  
 #include "RTFusionRTQF.h"   
 #include "CalLib.h"  
 #include <EEPROM.h>  
 #include <SoftwareSerial.h>  
 #include <PString.h>  
 #include <nmea.h>  
   
 RTIMU *imu;               // the IMU object  
 RTFusionRTQF fusion;          // the fusion object  
 RTIMUSettings settings;         // the settings object  
 NMEA gps(GPRMC);            // GPS data connection to decode RMC sentence types (otherwise the serial output becomes too buggy if you decode all GPS sentences)  
 NMEA nmeaDecoder(ALL);         // NMEA data decoder for all sentence types (for the Transducer, and Anemometer)  
 SoftwareSerial windSerial(10,11,true); // RX pin, TX pin (not used), true means to invert the TTL signal (which is necessary with the MAX3232)  
   
 // User defined variables (for your own boat and geographic location):  
 float magd = -14.0;   // magnetic deviation; if it's East, it's negative (14E in San Francisco 2015)  
 float anvo = 52;    // vertical offset of the anemometer from the boat's center of rotation in feet  
 float anlo = 6;     // longitudinal offset of the anemometer from the boat's center of rotation in feet (negative is aft)  
   
 // Variables preset with recommended values (but can be changed):  
 #define DISPLAY_INTERVAL 500  // rate (in milliseconds) in which MPU9150 results are displayed  
 int gpsworking = 20;      // this ticks up everytime the MPU runs, which is defined in DISPLAY INTERVAL and is used as a   
 int hsworking = 20;       // counter so that if no GPS or Waterspeed data is received for 10 seconds, then VDR won't calculate  
 int imuworking = 20;      // Same thing but for roll/pitch/yaw corrections on wind  
   
 // Global variables:  
 unsigned long lastDisplay;  
 unsigned long lastRate;  
 int sampleCount;  
 float r = M_PI/180.0f; // degrees to radians  
 float d = 180.0f/M_PI; // radians to degrees  
   
 // Variables exchanged between instruments:  
 float sog; // speed over ground  
 float cmg; // course made good  
 float hs;  // water speed, or hull speed through the water  
 float ts;  // timestamp of GPS signal in UTC  
 float hdm; // magnetic heading  
 float hdt; // true heading   
 int roll;  // roll in degrees  
 int pitch; // pitch   
 int yaw;  // yaw (0 yaw corresponds to 270 magnetic)  
 int ror;  // rate of roll (degrees per second)  
 int rop;  // rate of pitch  
 int rot;  // rate of turn  
   
 void setup()  
 {  
   int errcode;  
   
   Serial.begin(4800);   // output port to computer  
   Serial1.begin(4800);  // from GPS  
   Serial2.begin(4800);  // from DST-800 transducer  
   windSerial.begin(4800); // from anemometer  
   Wire.begin();  
   imu = RTIMU::createIMU(&settings);  
   
   // Checking the IMU, if it fails, simply restarting the connection seems to work sometimes  
   if ((errcode = imu->IMUInit()) < 0) {  
     Serial.print("Failed to init IMU: "); Serial.println(errcode);  
   }  
   if (imu->getCalibrationValid())  
     Serial.println("Using compass calibration");  
   else  
     Serial.println("No valid compass calibration data");  
   
   lastDisplay = lastRate = millis();  
   sampleCount = 0;  
   
   // Slerp power controls the fusion and can be between 0 and 1  
   // 0 means that only gyros are used, 1 means that only accels/compass are used  
   // In-between gives the fusion mix. 0.02 recommended.  
     
   fusion.setSlerpPower(0.02);  
     
   // use of sensors in the fusion algorithm can be controlled here  
   // change any of these to false to disable that sensor  
     
   fusion.setGyroEnable(true);  
   fusion.setAccelEnable(true);  
   fusion.setCompassEnable(true);  
 }  
   
 // calculate checksum function (thanks to https://mechinations.wordpress.com)  
 byte checksum(char* str)   
 {  
   byte cs = 0;   
   for (unsigned int n = 1; n < strlen(str) - 1; n++)   
   {  
     cs ^= str[n];  
   }  
   return cs;  
 }  
   
 void loop()  
 {   
    
  // ***** BU-353 GPS ***** //  
   if (Serial1.available()) {  
    if (gps.decode(Serial1.read())) {  // if it's a valid NMEA sentence  
      Serial.println(gps.sentence()); // print the NMEA sentence  
      sog = atof(gps.term(7));  
      cmg = atoi(gps.term(8));  
      ts = atof(gps.term(1));  
      gpsworking = 0;         // resets the "gps is bad" counter back to 0 since it's good  
     }  
   }  
  // ***** End BU-353 GPS ***** //  
   
  // ***** Integrated Navigation (MPU-9150 and derivatives) ***** //  
   unsigned long now = millis();  
   unsigned long delta;  
   int loopCount = 1;  
    
   while (imu->IMURead()) {  
     // this flushes remaining data in case we are falling behind  
     if (++loopCount >= 10)  
       continue;  
     fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());  
     sampleCount++;  
     if ((delta = now - lastRate) >= 1000) {      
       sampleCount = 0;  
       lastRate = now;  
     }  
     if ((now - lastDisplay) >= DISPLAY_INTERVAL) {  
       imuworking = 0;  
       lastDisplay = now;  
       RTVector3 pose = fusion.getFusionPose();  
       RTVector3 gyro = imu->getGyro();  
       roll = pose.y()*-1*d; // degrees to radians  
       pitch = pose.x()*d;         // negative is nose down  
       yaw = pose.z()*d;  
       ror = gyro.y()*-1*d;  
       rop = gyro.x()*d;  
       rot = gyro.z()*d;          // negative is to left  
       hdm = yaw-90;            // 0 yaw = 270 magnetic; converts to mag degrees  
       if (yaw < 90 && yaw >= -179.99) {  
        hdm = yaw+270;  
       }  
       hdt = hdm-magd;           // calculate true heading  
       if (hdt > 360) {  
         hdt = hdt-360;  
       }  
       if (hdt < 0.0) {  
         hdt = hdt+360;  
       }  
       if (sog == 0.0) {          // this eliminates artifacts in cmg when you're speed over ground is zero  
        cmg = hdt;  
       }  
   
       // HDT True Heading //  
       char hdtSentence [23];   
       byte cst;  
       PString strt(hdtSentence, sizeof(hdtSentence));  
       strt.print("$HCHDT,");  
       strt.print(lround(hdt));  
       strt.print(",T*");   
       cst = checksum(hdtSentence);  
       if (cst < 0x10) strt.print('0');  
       strt.print(cst, HEX);  
       Serial.println(hdtSentence);   
      }  
    }  
    // ***** End Integrated Navigation ***** //  
      
    // ***** Anemometer ***** //  
    if (windSerial.available() > 0 ) {  
    if (nmeaDecoder.decode(windSerial.read())) {  
      float ftk = .592484;            // feet per second to knots  
      float rwa0 = atof(nmeaDecoder.term(1));   // the raw relative wind angle  
      float ws0 = atof(nmeaDecoder.term(3));   // the raw uncorrected wind speed  
      int quad = floor(rwa0/90);         // this gets which quadrant the wind is coming from (0,1,2,3)  
        
      // ITERATION ONE of corrections, for rotational velocity of pitch/roll/yaw  
      float sor = ror*r*anvo*ftk;              // this assumes your anemometer has little or no lateral offset  
      float sop = rop*r*sqrt(pow(anvo,2)+pow(anlo,2))*ftk;  
      float sot = rot*r*anlo*ftk;  
      float wx0 = ws0*sin(rwa0*r);  
      float wy0 = ws0*cos(rwa0*r);  
      float wx1 = wx0-sor-sot;            // assume starboard tack or...  
      if (rwa0 > 180.0) {                  // port  
       wx1 = wx0+sor+sot;  
      }  
      float wy1 = wy0+sop;              // assume beating upwind...  
      if (rwa0 < 270.0 && rwa0 > 90.0) {          // or downwind  
       wy1 = wy0-sop;  
      }  
      // Now we have wx1 and wy1, the x and y wind components corrected for the movement of the mast  
   
      // ITERATION TWO of corrections, for absolute roll and pitch of the boat (yaw has no effect)  
      float wx2 = wx1/cos(roll*r);  
      float wy2 = wy1/cos(pitch*r);  
      float ws1 = sqrt(pow(wx2,2)+pow(wy2,2));  
      float rwa1 = (atan2(wx2,wy2)*d)+90*quad;  
      if (wy2 == 0.0) {                   // this is for when there is no wind, and you can't divide by zero  
       rwa1 = rwa0;  
      }  
      if (quad == 1 || quad == 3) {             // we have to flip the atan function if the wind is coming from quadrant 1 or 3  
       rwa1 = (atan2(wy2,wx2)*d)+90*quad;  
       if (wx2 == 0.0) {  
        rwa1 = rwa0;  
       }  
      }  
      if (rwa1 < 360) {  
       rwa1 = rwa1+360;  
      }  
      if (rwa1 > 360) {  
       rwa1 = rwa1-360;  
      }  
      // Now we have ws1 and rwa1, the relative (or apparent) wind speed/angle corrected for boat roll and pitch  
   
      // ITERATION THREE of corrections, compensating for the boat's speed over ground  
      float twa0 = rwa1+hdt;  
      if (twa0 > 360) {  
       twa0 = twa0-360;  
      }  
      float dwa = twa0-cmg;  
      if (dwa < 0) {  
       dwa = dwa+360;  
      }  
      float ws = sqrt(pow(sog,2)+pow(ws1,2)-2*ws1*sog*cos(dwa*r));  
      float rwa2 = (M_PI-atan2((ws1*sin(dwa*r)),(sog-ws1*cos(dwa*r))))*d;  
      if (sog == 0.0 && ws1 == 0.0) {  
       rwa2 = rwa1;  
      }  
      int twa = lround(cmg+rwa2);  
      if (twa > 360) {  
       twa = twa-360;  
      }  
      int rwa = twa-hdt;  
      if (rwa < 0) {  
       rwa = rwa+360;  
      }  
      // Now we have ws, the true windspeed corrected for how fast the boat is rolling, pitching, and yawing, and for its roll/pitch angle, and for its induced wind  
      // We also have twa, which is the corrected true wind angle of ws, and rwa, the corrected relative wind angle from the boat's bow  
   
      // MWV sentence for apparent wind  
      char mwvSentence [30];   
      byte csm;  
      PString strm(mwvSentence, sizeof(mwvSentence));  
      strm.print("$WIMWV,");  
      strm.print(rwa1);  
      strm.print(",R,");  
      strm.print(ws1);  
      strm.print(",N,A*");  
      csm = checksum(mwvSentence);  
      if (csm < 0x10) strm.print('0');  
      strm.print(csm, HEX);  
      Serial.println(mwvSentence);  
   
      // MWV Sentence for true wind  
      char mwdSentence [30];  
      byte csd;  
      PString strd(mwdSentence, sizeof(mwdSentence));  
      strd.print("$WIMWV,");  
      strd.print(twa);  
      strd.print(",T,");  
      strd.print(ws);  
      strd.print(",N,A*");  
      csd = checksum(mwdSentence);  
      if (csd < 0x10) strd.print('0');  
      strd.print(csd, HEX);  
      Serial.println(mwdSentence);  
     }  
   }  
   // ***** End Anemometer ***** //  
   
 }  

First, you must figure out the vertical offset of your anemometer from the boat's center of rotation. Go to http://sailboatdata.com/ and find your boat, then use whatever methods you can come up with the estimate where the center of rotation is. In most cases, this is the center of buoyancy, which is approximately 2/3 the way forward on a fin-keeled sailboat. Where is it vertically? This is where you kind of have to guess smartly. Once you know which point the boat rotates around (and rolls, pitches, and yaws), then you must figure out the vertical offset of the anemometer from this point. Not the actual distance, since that will be slant range, but the actual offset from the same vertical point of the center of buoyancy. Then, do the same with longitudinal offset, and input both of those values (in feet) at the top.

Now, it's ready to calculate.

Iteration 1 of Corrections

The first iteration corrects for the speed of the top of the mast whipping back and forth. Think of a windmill; the entire blade is spinning at the same rate (angular velocity), but the tip of the blade is spinning much, much faster (tangential velocity). It's the same with your boat; the whole thing is rotating at the same rate, but the mast, being fifty feet away from the center of rotation, is experiencing a heck of a lot of wind. Thanks to the MPU-9150, we know how fast the boat rolls, pitches, and yaws.

The code simply reduces the wind vector into its x and y components, and then adds (or subtracts) the x wind (rate of roll and yaw) and the y wind (rate of pitch).

Iteration 2 of Corrections

Similar to Iteration 1, this simply corrects for the absolute roll and pitch of the boat. As alluded to in the first paragraph, a wind vane tilted 90 degrees to the wind won't register any wind. But a wind vane tilted X degrees to the wind will register wind depending on the angle. Same thing with pitch.

After this correction is made, we reconstruct the wind vector using our new x and y components. This new wind vector is the boat's actual apparent wind that you are feeling on your face, but your anemometer couldn't detect. Think of this as instrument error.

Iteration 3 of Corrections

This simply subtracts your speed over ground from the wind to get the true wind. It follows a very similar process as calculating the tidally induced currents I wrote about here, only now it's for wind. And this is transmitted as True Wind. If you have a chart plotter that automatically calculates True Wind based on your other instruments, you may just want to omit this sentence.

Final Thoughts

I had a wireless wind anemometer that I was using to test with, and everything came out correctly. The only problem is that the receiver stopped working, and I had to return the device before I could install it (maybe that's not a problem...). In any case, it produced desirable results, but I unfortunately couldn't test it on the water. Perhaps someone else could and share the results. I'm going to wait to lift the stick and redo the wiring and install a wired anemometer then, so expect to see some actual results on the water.

Set and Drift; or, the Speed and Direction of the Tidal Current (NMEA-0183 VDR)

Saltstraumen, Norway
Tucked away in a small fjord in Finland, the tidal current can reach 22 knots. Following the theoretical maximum hull speed formula, you'd need a boat almost 270 feet long just to make way in that current. In the San Francisco Bay, the speeds routinely exceed 4 knots (now we're looking at a much more realistic 9 foot boat to reach that speed). But when you're beating into the wind with two reefs in and maybe a little too much headsail, that current is going to push you off. It's the same thing in an airplane; if you have a 30 knot direct cross-wind from the right, you are now moving 30 knots to the left, regardless of how fast you're moving forward. Kind of a problem when you're trying to land. It's also kind of a problem when you're trying to sail a course, and your nose is pointed off by 30 degrees. Even the simple knowledge of how strong and where the current is can make or break your sail plan.

The direction and speed of the current, formally titled Set and Drift respectively, are really easy to figure out if you are given the right information. Over the past few blog posts, I've gone over how to get that information, and more importantly--how to manipulate it.

Recommended Gear

NMEA-0183 Instruments and VDR

The assumption is that you've followed all my previous posts (MPU-9150 Setup, DST-800 Setup) and have everything working swimmingly. What about the GPS, though? I followed David's writeup and soldered a wire to the chip where the output comes through, then kept the original usb cable until the very end where I cut it and ended up with a VCC, Ground, and the NMEA wire. There are probably much easier ways to do this, so... have at it, and report back.

Those are the only three instruments you need: Compass, paddlewheel  and GPS. The most difficult part of this is the paddlewheel. If you're boat is big and heavy, you'll need to haul it out and install it while on the hard. Mine was over due for a new paint job, so the timing worked out pretty well. Good time to check everything else too. Anyway, the point is that we need four numbers: heading, waterspeed (which is not the speed of the water; it is the speed of your hull through the water--sometimes I refer to it as hullspeed, but it is technically called waterspeed in the industry), course made good, and speed over ground. In more simple terms, we need the angle and magnitude of two vectors: our boat through the water, and our boat over the ground.

Here are some handy sketches to help visualize what we're trying to figure out, where
  • hdt = true heading
  • hsp = hullspeed, or speed through the water
  • cmg = course made good
  • sog = speed over ground
and the bold lines indicate the actual force vector acting upon the vessel, and each picture represents one of the three possible situations where a current can act upon your vessel (the port/starboard direction doesn't matter, because you can just mirror the image and the values will be identical).

Scenario 1: The current is pushing you back and to starboard

Scenario 2: the current is pushing your forward and to starboard

Scenario 3: same as Scenario 1, but the current is moving faster than your boat is moving 

How to Calculate the NMEA-0183 VDR Set and Drift Sentence

The math isn't that bad, plus I did it all for you. We're just doing simple trigonometry here, which is a little easier to understand by using vectors. If you really want to get down and dirty, I'll leave you to it--it'll take some time. But, given those four variables, the functions are as follows:




The astute reader may observe that you would need more than just those two equations to get the right answer under any conditions. But we'll take care of that in the actual Arduino code when we get there. For now, those two equations are the most basic representation. Set is measured in degrees relative from your heading; drift is measured in the same units as your hull speed and speed over ground (most likely knots). You would also have to operate in radians instead of degrees, in which case the 180 in the set formula becomes Pi.

What's important here is that these two equations are quite simple, and as long as you have the four data inputs, you can figure it out really quickly. And cheaply. This is the only equipment I could find that gives you similar information. And I can't even find a price; comparable items sell for $15,000 to $20,000, so that gives you an idea of what we're talking about here.

So, quite naturally, I am making the code available for free so you don't have to spend $20,000.

The Arduino Sketch to Calculate VDR

 ////////////////////////////////////////////////////////////////////////////  
 //  
 // This file is part of RTIMULib-Arduino  
 //  
 // Copyright (c) 2014-2015, richards-tech  
 //  
 // Permission is hereby granted, free of charge, to any person obtaining a copy of   
 // this software and associated documentation files (the "Software"), to deal in   
 // the Software without restriction, including without limitation the rights to use,   
 // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the   
 // Software, and to permit persons to whom the Software is furnished to do so,   
 // subject to the following conditions:  
 //  
 // The above copyright notice and this permission notice shall be included in all   
 // copies or substantial portions of the Software.  
 //  
 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   
 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A   
 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT   
 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION   
 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE   
 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.  
 //  
 // Further modified by King Tide Sailing (kingtidesailing.blogspot.com)  
 //  
 ////////////////////////////////////////////////////////////////////////////  
   
 #include <Wire.h>  
 #include "I2Cdev.h"  
 #include "RTIMUSettings.h"  
 #include "RTIMU.h"  
 #include "RTFusionRTQF.h"   
 #include "CalLib.h"  
 #include <EEPROM.h>  
 #include <SoftwareSerial.h>  
 #include <PString.h>  
 #include <nmea.h>  
   
 RTIMU *imu;               // the IMU object  
 RTFusionRTQF fusion;          // the fusion object  
 RTIMUSettings settings;         // the settings object  
 NMEA gps(GPRMC);            // GPS data connection to decode RMC sentence types (otherwise the serial output becomes too buggy if you decode all GPS sentences)  
 NMEA nmeaDecoder(ALL);         // NMEA data decoder for all sentence types (for the Transducer, and Anemometer)  
   
 // User defined variables (for your own boat and geographic location):  
 float magd = -14.0;   // magnetic deviation; if it's East, it's negative (14E in San Francisco 2015)   
   
 // Variables preset with recommended values (but can be changed):  
 #define DISPLAY_INTERVAL 500  // rate (in milliseconds) in which MPU9150 results are displayed  
   
 // Global variables:  
 unsigned long lastDisplay;  
 unsigned long lastRate;  
 int sampleCount;  
 float r = M_PI/180.0f; // degrees to radians  
 float d = 180.0f/M_PI; // radians to degrees  
   
 // Variables exchanged between instruments:  
 float ts;  // timestamp of GPS signal in UTC  
 float sog; // speed over ground  
 float cmg; // course made good  
 float hs;  // water speed, or hull speed through the water  
 float hdm; // magnetic heading  
 float hdt; // true heading  
   
 void setup()  
 {  
   int errcode;  
   
   Serial.begin(4800);   // output port to computer  
   Serial1.begin(4800);  // from GPS  
   Serial2.begin(4800);  // from DST-800 transducer  
   Wire.begin();      // from MPU-9150  
   imu = RTIMU::createIMU(&settings);  
   
   // Checking the IMU, if it fails, simply restarting the connection seems to work sometimes  
   if ((errcode = imu->IMUInit()) < 0) {  
     Serial.print("Failed to init IMU: "); Serial.println(errcode);  
   }  
   if (imu->getCalibrationValid())  
     Serial.println("Using compass calibration");  
   else  
     Serial.println("No valid compass calibration data");  
   
   lastDisplay = lastRate = millis();  
   sampleCount = 0;  
   
   // Slerp power controls the fusion and can be between 0 and 1  
   // 0 means that only gyros are used, 1 means that only accels/compass are used  
   // In-between gives the fusion mix. 0.02 recommended.  
     
   fusion.setSlerpPower(0.02);  
     
   // use of sensors in the fusion algorithm can be controlled here  
   // change any of these to false to disable that sensor  
     
   fusion.setGyroEnable(true);  
   fusion.setAccelEnable(true);  
   fusion.setCompassEnable(true);  
 }  
   
 // calculate checksum function (thanks to https://mechinations.wordpress.com)  
 byte checksum(char* str)   
 {  
   byte cs = 0;  
   for (unsigned int n = 1; n < strlen(str) - 1; n++)   
   {  
     cs ^= str[n];  
   }  
   return cs;  
 }  
   
 void loop()  
 {   
    
  // ***** BU-353 GPS ***** //  
   if (Serial1.available()) {  
    if (gps.decode(Serial1.read())) {  // if it's a valid NMEA sentence  
      Serial.println(gps.sentence()); // print the NMEA sentence  
      sog = atof(gps.term(7));    // speed over ground  
      cmg = atoi(gps.term(8));    // course made good  
      ts = atof(gps.term(1));     // timestamp  
     }  
   }  
  // ***** End BU-353 GPS ***** //  
   
  // ***** DST-800 Transducer ***** //  
   if (Serial2.available() > 0 ) {  
    if (nmeaDecoder.decode(Serial2.read())) {  
     char* title = nmeaDecoder.term(0);  
     if (strcmp(title,"VWVHW") == 0) {  
      hs = atof(nmeaDecoder.term(5));  
      }  
     Serial.println(nmeaDecoder.sentence());  
     }  
  }  
  // ***** End DST-800 Transducer ***** //  
   
  // ***** Integrated Navigation (MPU-9150 and derivatives) ***** //  
   unsigned long now = millis();  
   unsigned long delta;  
   int loopCount = 1;  
     
   while (imu->IMURead()) {  
     // this flushes remaining data in case we are falling behind  
     if (++loopCount >= 10)  
       continue;  
     fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());  
     sampleCount++;  
     if ((delta = now - lastRate) >= 1000) {      
       sampleCount = 0;  
       lastRate = now;  
     }  
     if ((now - lastDisplay) >= DISPLAY_INTERVAL) {  
       lastDisplay = now;   
       RTVector3 pose = fusion.getFusionPose();   
       RTVector3 gyro = imu->getGyro();   
       float roll = pose.y()*-1*d;   // negative is left roll   
       float pitch = pose.x()*d;    // negative is nose down   
       float yaw = pose.z()*d;     // negative is to the left of 270 magnetic   
       float rot = gyro.z()*d;     // negative is to left   
       hdm = yaw-90;       // 0 yaw = 270 magnetic; converts to mag degrees   
       if (yaw < 90 && yaw >= -179.99) {   
        hdm = yaw+270;   
       }   
       hdt = hdm-magd;      // calculate true heading   
       if (hdt > 360) {   
        hdt = hdt-360;   
       }   
       if (hdt < 0.0) {   
        hdt = hdt+360;   
       }   
       if (sog == 0.0) {        // this eliminates old artifacts in cmg when your speed over ground is zero  
        cmg = hdt;  
       }  
     
       // HDT True Heading //   
       char hdtSentence [23];    
       byte cst;   
       PString strt(hdtSentence, sizeof(hdtSentence));   
       strt.print("$HCHDT,");   
       strt.print(lround(hdt));   
       strt.print(",T*");    
       cst = checksum(hdtSentence);   
       if (cst < 0x10) strt.print('0');   
       strt.print(cst, HEX);   
       Serial.println(hdtSentence);   
         
       // VDR Set and Drift //  
        float set0 = (cmg-hdt)*r;     // gets the difference between boat's heading and cmg in radians  
        if (set0 < 0) {  
         set0 = set0+(2*M_PI);  
        }  
        float drift = sqrt(pow(hs,2)+pow(sog,2)-2*hs*sog*(cos(set0)));  
        float set1 = (M_PI-atan2((sog*sin(set0)),(hs-sog*cos(set0))))*d;  
        if (hs == 0 && sog == 0) {     // if you're not moving, then set = 0  
         set1 = set0;  
        }  
        float set = hdt+set1;  // converting to true instead of relative degrees  
        if (set > 360) {  
         set = set-360;  
        }  
        float setmag = set+magd;  
        if (setmag > 360) {   
         setmag = setmag-360;   
        }   
        if (setmag < 0.0) {   
         setmag = setmag+360;   
        }  
          
        char vdrSentence [43];   
        byte csv;  
        PString strv(vdrSentence, sizeof(vdrSentence));  
        strv.print("$INVDR,");  
        strv.print(lround(set));  
        strv.print(",T,");  
        strv.print(lround(setmag));  
        strv.print(",M,");  
        strv.print(drift);  
        strv.print(",N*");  
        csv = checksum(vdrSentence);  
        if (csv < 0x10) strv.print('0');  
        strv.print(csv, HEX);  
        Serial.println(vdrSentence);  
     }  
    }  
    // ***** End Integrated Navigation ***** //  
   
 }  

The big thing here is the VDR portion at the end, which spits out an NMEA sentence like this:

          1  2  3  4  5  6  7  
          |  |  |  |  |  |  |  
  $--VDR,x.x,T,x.x,M,x.x,N*hh  
 1. Course, True  
 2. T = True  
 3. Course, Magnetic  
 4. M = Magnetic  
 5. Knots  
 6. N = Knots  
 7. Checksum  
   

I messed around with this for a while, and realized that Magnetic degrees is kind of useless. So I changed it to relative degrees, and took my yacht out for some testing. It took a little while to get it totally correct, but...

The Results

Here's a screenshot of OpenCPN, where I found a forecasted current point and posted up to see how close I was.


As you can see, near this point in the San Francisco Bay, the tidally induced current is forecasted to be flowing towards 211 degrees true (its course), at a speed of 2.5 knots. This is a solid forecast using known techniques that produce good results.

So what did the King Tide Multiplexer say the current was at this point? Here's a screen shot shortly after:


It was saying the current was flowing towards 237 degrees at 1.89 knots, a difference of 25 degrees and half a knot. What gives? Well, a few things:

  • The point at which the current was forecasted is only at that particular point; as anyone who sails in the Bay knows, the current can change drastically and quite quickly over a small distance. So while I tried to get as accurate measurements as possible, the point at which I took the above screenshot isn't the same as the forecast point
  • Variations of water level can change how fast a current is moving based on the nearby bathymetry. I do not believe the current models account for the reduced inflow into the Bay due to the drought
  • I hadn't calibrated my heading sensor on my boat; it was only calibrated at my dining room table, which is a lot different than inside my nav desk
  • The heading sensor was pointing to port by 15 degrees (oops)
Here's another sample output, where I added the cross- and head-component of the current:


I edited the code to have the current direction reflect where it's coming from, as opposed to where it's flowing to. So right at that geographic point in the Bay, the current is coming from 041 (212 relative from my boat's heading, or from my port/aft quarter) at 2.57 knots. This also says that the current is pushing me 1.57 knots to starboard, and 2.17 knots forward. I just need a useful way of displaying this information now.

In any case, I will be taking it out for some more testing after I recalibrate the compass, and mount it more securely to the nav desk. And I'll be using a data logger in conjunction with OpenCPN and pass by the point precisely to see how close my math works out.

What to do next? Definitely find a plugin for OpenCPN that plots the current vector just like the CMG and HDT vector as shown. That way, I'll get a quick visual reference of what precisely the current is doing at a particular moment at a particular point.

EDIT: I have become aware that this is neglecting one important factor, called leeway, or the boat's drift due to being blown sideways by the wind. Unfortunately, the only way to figure out your leeway is through ops testing and deriving its value that way based on your heel angle, the relative wind direction, and speed. Maybe in a perfect world, I'd have all these things... but for now, this will do.

Sunday, September 27, 2015

How to Make an NMEA-0183 Tilt-Compensated Compass and Rate-Of-Turn Indicator

This simple, analogue compass costs $10. If we want to use this for electronic chart-plotting applications, then we need an electronic compass that takes your magnetic heading and converts it into a digital signal. This compass does that, for the low, low price of $774.95. Why does an electronic compass, based on technology over four thousand years old, cost so much? This guide will show you how to build your own for under $100.

Recommended Gear

Now Why Is a Marine Compass so Expensive?

Aside from the obvious "it has the word 'marine' in the title, so it costs X times more," there's a valid reason it's more expensive than just a regular electronic compass. Any compass application for a moving vehicle needs to be securely mounted such that magnetic north aligns exactly with the vehicle longitudinal axis. In more basic terms, it needs to be completely flat and lined up with forward. When you take an analogue compass (like I linked to in the first paragraph) and tilt it just a little bit, the needle quickly jams on its pivot and it becomes unusable. So, in a rocking, rolling, listing sailboat, you need a tilt-compensated compass.

One way to do this is to put the compass on a two-axis gimbal (kind of like how some stoves are). This way, as the boat rolls and pitches, it remains flat relative to gravity, and will continue to work just fine. However, that requires you to build a two-axis gimbal, which isn't exactly a quick job if you want to do it correctly.

Fortunately, there's an easier way. Just get three compasses, and line them up so that one is flat against each axis. Lay one done so it's flat with respect to the horizon (Z), another with respect to the boat's longitude (X), and another with respect to the boat's latitude (Y). Then you just filter each compass's output through a complicated formula, and you get your magnetic heading. Fortunately, the MPU-9150 has taken care of all that hard work.

This guide assumes you've followed my previous post and set up the versatile MPU-9150 9-Axis Accelerometer, Gyro, and Compass. It should be fully working, and putting out valid data as demonstrated at the end of that post. If you have problems, comment at the bottom and I'll respond as soon as I can.

It also assumes you have gone through my other post and you have a good understanding of how to create an NMEA-0183 sentence with an Arduino. In fact, if you've gone through both of those posts and understand them, this is a very simple and quick job.

The Sketch for NMEA-0183 True Heading

 ////////////////////////////////////////////////////////////////////////////  
 //  
 // This file is part of RTIMULib-Arduino  
 //  
 // Copyright (c) 2014-2015, richards-tech  
 //  
 // Permission is hereby granted, free of charge, to any person obtaining a copy of   
 // this software and associated documentation files (the "Software"), to deal in   
 // the Software without restriction, including without limitation the rights to use,   
 // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the   
 // Software, and to permit persons to whom the Software is furnished to do so,   
 // subject to the following conditions:  
 //  
 // The above copyright notice and this permission notice shall be included in all   
 // copies or substantial portions of the Software.  
 //  
 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   
 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A   
 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT   
 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION   
 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE   
 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.  
 //  
 // Further modified by King Tide Sailing (kingtidesailing.blogspot.com)  
 //  
 ////////////////////////////////////////////////////////////////////////////  
   
 #include <Wire.h>  
 #include "I2Cdev.h"  
 #include "RTIMUSettings.h"  
 #include "RTIMU.h"  
 #include "RTFusionRTQF.h"   
 #include "CalLib.h"  
 #include <EEPROM.h>  
 #include <PString.h> 
   
 RTIMU *imu;               // the IMU object  
 RTFusionRTQF fusion;          // the fusion object  
 RTIMUSettings settings;         // the settings object  
  
 // User defined variables (for your own boat and geographic location):  
 float magd = -14.0;     // magnetic deviation; if it's East, it's negative (14E in San Francisco 2015)  
   
 // if you don't want to include a sentence, then just make the appropriate variable false  
 // it is recommended that you print only the minimum sentences desired to keep the Serial Port from getting too busy  
 boolean outputhdm = false; // magnetic heading  
 boolean outputhdt = true;  // true heading  
 boolean outputrot = true;  // rate of turn  
 boolean outputshr = true;  // pitch and roll  
   
 // Variables preset with recommended values (but can be changed):  
 #define DISPLAY_INTERVAL 500  // rate (in milliseconds) in which MPU9150 results are displayed  
   
 // Global variables:  
 unsigned long lastDisplay;  
 unsigned long lastRate;  
 int sampleCount;  
 float r = M_PI/180.0f; // degrees to radians  
 float d = 180.0f/M_PI; // radians to degrees  
   
 void setup()  
 {  
   int errcode;  
   
   Serial.begin(4800);   // output port to computer  
   Wire.begin();  
   imu = RTIMU::createIMU(&settings);  
   
   // Checking the IMU, if it fails, simply restarting the connection seems to work sometimes  
   if ((errcode = imu->IMUInit()) < 0) {  
     Serial.print("Failed to init IMU: "); Serial.println(errcode);  
   }  
   if (imu->getCalibrationValid())  
     Serial.println("Using compass calibration");  
   else  
     Serial.println("No valid compass calibration data");  
   
   lastDisplay = lastRate = millis();  
   sampleCount = 0;  
   
   // Slerp power controls the fusion and can be between 0 and 1  
   // 0 means that only gyros are used, 1 means that only accels/compass are used  
   // In-between gives the fusion mix. 0.02 recommended.  
     
   fusion.setSlerpPower(0.02);  
     
   // use of sensors in the fusion algorithm can be controlled here  
   // change any of these to false to disable that sensor  
     
   fusion.setGyroEnable(true);  
   fusion.setAccelEnable(true);  
   fusion.setCompassEnable(true);  
 }  
   
 // calculate checksum function (thanks to https://mechinations.wordpress.com)  
 byte checksum(char* str)   
 {  
   byte cs = 0;   
   for (unsigned int n = 1; n < strlen(str) - 1; n++)   
   {  
     cs ^= str[n];  
   }  
   return cs;  
 }  
   
 void loop()  
 {   
   unsigned long now = millis();  
   unsigned long delta;  
   int loopCount = 1;  
    
   while (imu->IMURead()) {  
     if (++loopCount >= 10)  
       continue;  
     fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());  
     sampleCount++;  
     if ((delta = now - lastRate) >= 1000) {      
       sampleCount = 0;  
       lastRate = now;  
     }  
     if ((now - lastDisplay) >= DISPLAY_INTERVAL) {  
       lastDisplay = now;  
       RTVector3 pose = fusion.getFusionPose();  
       RTVector3 gyro = imu->getGyro();  
       float roll = pose.y()*-1*d;     // negative is left roll  
       float pitch = pose.x()*d;       // negative is nose down  
       float yaw = pose.z()*d;         // negative is to the left of 270 magnetic  
       float rot = gyro.z()*d;         // negative is to left  
       float hdm = yaw-90;             // 0 yaw = 270 magnetic; converts to mag degrees  
       if (yaw < 90 && yaw >= -179.99) {  
        hdm = yaw+270;  
       }  
       float hdt = hdm-magd;           // calculate true heading  
       if (hdt > 360) {  
         hdt = hdt-360;  
       }  
       if (hdt < 0.0) {  
         hdt = hdt+360;  
       }  
   
       // HDM Magnetic Heading //  
       if (outputhdm = true){  
       char hdmSentence [23];   
       byte csm;  
       PString strm(hdmSentence, sizeof(hdmSentence));  
       strm.print("$HCHDM,");  
       strm.print(lround(hdm));  // lround simply rounds out the decimal, since a single degree is fine enough of a resolution
       strm.print(",M*");   
       csm = checksum(hdmSentence);  
       if (csm < 0x10) strm.print('0');  
       strm.print(csm, HEX);  
       Serial.println(hdmSentence);  
       }  
   
       if (outputhdt = true){  
       // HDT True Heading //  
       char hdtSentence [23];   
       byte cst;  
       PString strt(hdtSentence, sizeof(hdtSentence));  
       strt.print("$HCHDT,");  
       strt.print(lround(hdt));  
       strt.print(",T*");   
       cst = checksum(hdtSentence);  
       if (cst < 0x10) strt.print('0');  
       strt.print(cst, HEX);  
       Serial.println(hdtSentence);  
       }  
   
       if (outputrot = true){  
       // ROT Rate of Turn //  
       char rotSentence [18];   
       byte csr;  
       PString strr(rotSentence, sizeof(rotSentence));  
       strr.print("$TIROT,");  
       strr.print(lround(rot)*60);    // multiply by 60, since ROT is measured in degrees per minute
       strr.print(",A*");   
       csr = checksum(rotSentence);  
       if (csr < 0x10) strr.print('0');  
       strr.print(csr, HEX);  
       Serial.println(rotSentence);  
       }  
   
       if (outputshr = true){  
       // SHR Pitch and Roll (no heave... yet...)//  
       char shrSentence [50];   
       byte csp;  
       PString strp(shrSentence, sizeof(shrSentence));  
       strp.print("$INSHR,,");  
       strp.print(lround(hdt));  
       strp.print(",T,");  
       strp.print(lround(roll));  
       strp.print(",");  
       strp.print(lround(pitch));  
       strp.print(",,,,,1,0*");  
       csp = checksum(shrSentence);  
       if (csp < 0x10) strp.print('0');  
       strp.print(csp, HEX);  
       Serial.println(shrSentence);  
       }  
      }  
    }  
 }  

So what's going on here? As long as you mount the MPU-9150 completely flat with respect to the boat (parallel with the water line, forward pointing towards the bow), then it will give you valid heading data. You must adjust a few variables at the top, mainly the magnetic deviation for where you're boat is. If you're having trouble finding that, it may help to look up nearby airports, since magnetic deviation is important for aviation.

The variables just below that dictate what gets printed. For the most part, marine applications use true heading (as do aviation applications), but if your chart plotter incorporates a geodetic magnetic deviation model, then you may just want to print the magnetic sentence.

I also have the lround function in there, which gets rid of the decimal place for the headings. A single degree is fine enough of a resolution (101.5 degrees means the same to me as 102 degrees). If you wish to keep the decimals, then just remove the lround functions.

The HDM and HDT sentences are self-explanatory enough, but what about ROT? Simple: it's Rate of Turn, how many degrees per second your boat is turning. And SHR? That's the roll and pitch of the boat, which you can end up doing some pretty cool things with if you know how to use them.

That's really all there is to say about this. It's a simple sketch, and it's with some pretty low cost parts. In fact, if you tally it up, you get a tilt-compensated compass for about ~$100. Now that's more like 2015.