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;

// 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 ;
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 ) {
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
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 ;
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 ;
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.