Friday, February 26, 2016

Connect the GlobalSat BU-353 USB GPS to a Raspberry Pi


There isn't much to say in this post. Following the previous one here about connecting NMEA-0183 devices to a Raspberry Pi, you should plug in the BU-353 S4 USB GPS and get its USB address. Then, change the address in the following script to start receiving info. For my purposes, I only need the RMC sentence. I don't really care about any of the other ones.

As with my previous post with fault protection, if it doesn't receive an RMC sentence for 10 seconds, it starts printing out to the UDP port an XDR sentence saying it has failed an needs to be reset. How to reset it? Well, that would probably just involve turning off and on the whole Raspberry Pi. An example of that:

$IIXDR,GPS_FAIL,.3*4A

meaning it has failed for .3 minutes.
I won't be updating this pasted code here, so check the github page for the most recent script here.

The gps.py script:

 import sys  
 import serial  
 import math  
 import operator  
 import time  
 import socket  
   
 GPS_IP = "127.0.0.3"  
 GPS_PORT = 5005  
   
 t_gps = 0  
 t_fail = 0.0  
 t_print = time.time()  
   
 ser = serial.Serial('/dev/ttyUSB1', 4800, timeout=1)  
   
 while True:  
   
   hack = time.time()  
   
   gps_raw = ser.readline()  
   if "*" in gps_raw:  
     gps_split = gps_raw.split('*')  
     gps_sentence = gps_split[0].strip('$')  
     cs0 = gps_split[1][:-2]  
     cs1 = format(reduce(operator.xor,map(ord,gps_sentence),0),'X')  
     if len(cs1) == 1:  
       cs1 = "0" + cs1  
     if cs0 == cs1:  
   
       gps_vars = gps_sentence.split(',')  
       title = gps_vars[0]  
   
       if title == "GPRMC":  
         t_fail = 0.0  
         t_gps = hack  
         sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
         sock.sendto(gps_raw, (GPS_IP, GPS_PORT))  
   
   if (hack - t_gps) > 10.0:  
     if (hack - t_print) > 1.0:  
       t_fail += 1.0  
       gps_sentence = "IIXDR,GPS_FAIL," + str(round(t_fail / 60, 1))  
       cs = format(reduce(operator.xor,map(ord,gps_sentence),0),'X')  
       if len(cs) == 1:  
         cs = "0" + cs  
       gps_sentence = "$" + gps_sentence + "*" + cs  
       sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
       sock.sendto(gps_sentence, (GPS_IP, GPS_PORT))  
       t_gps = hack  
   

Connect Any NMEA-1083 Device to a Raspberry Pi


Just a quick update on my previous post about NMEA-0183 on the Arduino. Since I'm no longer using the Arduino, here's a quick down and dirty on how to interface with any NMEA-0183 device. The protocols in that post remain the same (RS-232, 422, 485, etc...), and the easiest way to connect them to a Raspberry Pi is through a USB to RS-422 adapter. This is the one I'm using, and I also have this one here for a later project. It's real self-explanatory: Plug in NMEA Out + into B, NMEA Out -  into A, and it goes (quick disclaimer: I haven't actually tried it with an NMEA device yet--I've been using my Arduino to simulate my depth sounder, and so those wires may actually have to be swapped with an actual device).

Once it's connected, you need to know the USB port's address in order to find that information. This step is really really easy. Make sure that you have it unplugged, and type this into the terminal:

ls /dev/tty*

and that will list a bunch of lines. You're main focus will be at the end of the list, with something like ttyUSB0 or something like that. Next, plug in your USB device (the NMEA-0183 device need not be powered on) and type in

ls /dev/tty* 

You should see an additional USB line at the end. Obviously, that's your NMEA's USB address. Make note of this.

Before we go on, a quick word on these addresses: if you have more than one USB device plugged in, and you unplug one of them assigned to USB0 or USB1, then the other USB's address will shift down by 1--such that your script won't work. So basically, keep everything plugged in that you'll be using. If you unplug something, you'll have to rework the script to reflect the new address.

The script for my DST-800 plugged into my Raspberry Pi is below, but it can pretty much work for everything. Here's the basic architecture:

It reads a single line coming in from the USB port, extracts the actual sentence by splitting it by the * symbol and removing the $ sign. It then calculates it's checksum, and compares that to the given checksum. If it's valid (meaning it's a valid NMEA sentence), it splits the sentence into its comma separated values, and runs specific code depending on the title.

For example, I convert the temperature from Celsius to Fahrenheit, and add a .5 depth sounder offset to that sentence.

Whenever it receives a sentence, it runs its specific code, then it sends it to a UDP port to another script for further processing and compilation. Now, one of the reasons why I wanted to switch from the Arduino to the Raspberry Pi was for fault protection. I'm checking for a valid NMEA sentence, but what if the sensor wigs out and stops sending data?

If it goes for more than 10 seconds without receiving a depth sentence, it assumes the DST-800 has failed (or circuit breaker popped, or whatever). In this case, it creates a new NMEA sentence to let me know. Example:

$IIXDR,DST_FAIL,4.5*7F

which then goes through the gonkulator script (more on that to follow), through kplex, and into OpenCPN or whatever device I'm using since this is a valid NMEA sentence, and it tells me that the DST hasn't sent a sentence in 4.5 minutes. That will continue to tick up forever. If I see that, then I know I need to go reset the circuit breaker or something like that.

More to follow on what happens next.


The dst.py script:


 import sys  
 import serial  
 import math  
 import operator  
 import time  
 import socket  
   
 DST_IP = "127.0.0.4"  
 DST_PORT = 5005  
   
 vlwfirst = 1  
 vlwinit = 0.0  
 t_dpt = 0  
 t_print = time.time()  
 t_fail = 0.0  
   
 sddpt = ''  
 mtw = ''  
 yxmtw = ''  
 vwvhw = ''  
 vlw = ''  
 vwvlw = ''  
   
 ser = serial.Serial('/dev/ttyUSB0', 4800, timeout=1)  
   
 while True:  
   hack = time.time()  
   
   dst_raw = ser.readline()  
   
   if "*" in dst_raw:  
     dst_split = dst_raw.split('*')  
     dst_sentence = dst_split[0].strip('$')  
     cs0 = dst_split[1][:-2]  
     cs = format(reduce(operator.xor,map(ord,dst_sentence),0),'X')  
     if len(cs) == 1:  
       cs = "0" + cs  
   
     if cs0 == cs:  
   
       sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
       dst_vars = dst_sentence.split(',')  
       title = dst_vars[0]  
   
       if title == "SDDPT":  
         sddpt = dst_raw[:-2]  
         sock.sendto(sddpt, (DST_IP, DST_PORT))  
         t_dpt = hack  
         t_fail = 0.0  
   
       if title == "YXMTW":  
         mtw = dst_vars[0] + "," + str(float(dst_vars[1]) * 9 / 5 + 32) + ",F"  
         cs = format(reduce(operator.xor,map(ord,mtw),0),'X')  
         if len(cs) == 1:  
           cs = "0" + cs  
         yxmtw = "$" + mtw + "*" + cs  
         sock.sendto(yxmtw, (DST_IP, DST_PORT))  
   
       if title == "VWVHW":  
         vwvhw = dst_raw[:-2]  
         sock.sendto(vwvhw, (DST_IP, DST_PORT))  
   
       if title == "VWVLW":  
         if vlwfirst == 1:  
           vlwinit = dst_vars[1]  
           vlwfirst = 0  
         trip = float(dst_vars[1]) - float(vlwinit)  
         vlw = "VWVLW," + dst_vars[1] + ",N," + str(trip) + ",N"  
         cs = format(reduce(operator.xor,map(ord,mtw),0),'X')  
         if len(cs) == 1:  
           cs = "0" + cs  
         vwvlw = "$" + vlw + "*" + cs  
         sock.sendto(vwvlw, (DST_IP, DST_PORT))  
   
   if (hack - t_dpt) > 10.0:  
     if (hack - t_print > 1.0):  
       t_fail += 1.0  
       dst_sentence = "IIXDR,DST_FAIL," + str(round(t_fail / 60, 1))  
       cs = format(reduce(operator.xor,map(ord,dst_sentence),0),'X')  
       if len(cs) == 1:  
         cs = "0" + cs  
       dst_sentence = "$" + dst_sentence + "*" + cs  
       sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
       sock.sendto(dst_sentence, (DST_IP, DST_PORT))  
       t_print = hack  
   

Thursday, February 25, 2016

How to Setup the MPU-9250 on a Raspberry Pi

Step 1: Get the Right Gear

You'll need to get the associated wires, jumper cables, solder, what-have you, in order to properly connect the MPU-9250 to the Raspberry Pi. Keep in mind, I'm using i2c, and those cables can't be too long otherwise the signal gets corrupted (No longer than a foot or so).

Not only that, but you'll have to mount the sensor correctly. I had some leftover redwood lumber that I fashioned into a housing for the sensor, which makes it easier to work with at home, and then easy to install on the boat. The housing is goofy shaped because I'll be installing it inside a cabinet, but the bottom is at a 13 degree angle from level (hence the sloped housing).

Do not use any ferrous metal (or any metal, really... maybe brass would be okay) to construct the housing. I waved a stainless steel screw over the sensor and it swiftly changed the heading by 30 degrees. So I used some old fashioned wood glue to hold it together.

Step 2: Download and Install the Right Programs

For my system, I keep all scripts and files in its own folder in the /home/pi directory called "kts".

mkdir kts
cd kts

Next, get yourself the right i2c programs.

sudo apt-get install i2c-tools

and connect your MPU-9250 (remember to attach it to the 3.3V pin, not the 5V pin). Here's a pinout diagram, with the top of the image representing the side away from the USB ports. See my previous post for help if you get stuck.


  • One thing you must do: Click the Menu in the top Left Corner, go to Prefrences->Raspberry Pi Configuration->Interfaces and enable i2c (this will require a reboot).
  • Go ahead and restart now.

To make sure it's connected, type in

sudo i2cdetect -y 1

and you should see the number 68 in the grid that displays. 68 is the sensor's default address.

Next, download and install cmake, which is used to build the sensor's library:

sudo apt-get install cmake

(and enter "y" went prompted). You'll also have to install some python tools, since that is the language of the scripts we use:

sudo apt-get install python-dev

If you want to want to do the Ellipsoid Fit Calibration (strongly recommended), then you'll also need the octave program:

sudo apt-get install octave

Next, get the RTIMULib Library from Github (website here):

git clone https://github.com/richards-tech/RTIMULib2.git

All we really need is the RTIMULibCal program, and the Linux/python scripts. So if you want all the demo programs and whatnot, follow the instructions here at the github page. Otherwise, proceed as follows:

cd RTIMULib2/Linux/RTIMULibCal
make -j4
sudo make install

The last thing we have to do before we run the program is setup our working directory, which I have creatively called "scripts" and copy the RTEllipsoidFit folder into folder just above this directory:

cp -r /home/pi/kts/RTIMULib2/RTEllipsoidFit/ /home/pi/kts/
cd /home/pi/kts/RTEllipsoidFit/

It's important to navigate to inside RTEllipsoidFit folder before proceeding, otherwise the calibration won't work. 

These next few edits I believe are optional, but are dictated on the RTIMULib's github page, so here we go. First, enter

sudo nano /etc/modules

and make sure that both these lines are uncommented in this file:

i2c-dev
i2c-bcm2708

Hit control+x, then y, then enter to save the changes.

Then:

sudo nano /etc/udev/rules.d/90-i2c.rules

and add:

KERNEL=="i2c-[0-7]",MODE="0666"

And one more:

sudo nano /boot/config.txt

and add at the bottom:

dtparam=i2c1_baudrate=400000

then

sudo reboot

Step 3: Calibrating the MPU-9250 on the Raspberry Pi

In the RTEllipsoidFit folder, run this in the terminal:

RTIMULibCal

Just type that in and hit enter. The instructions that display are pretty straight forward. Hit m to begin calibrating the magnometer's min/max values (rotate the sensor along all axis), then hit "s" to save it. Next, hit "e" to do the ellipsoid fit. You basically imagine the sensor sitting on a globe--and then you move the sensor all over the surface of the globe, as if it were a ship sailing along the ocean, keeping the bottom of the sensor pointed towards the center as you traverse all quadrants. This might take a while, and it will automatically stop when it gets enough data. If you get the following error:

error: ellipsoide fit returned 32512 - aborting

then that means you aren't inside the RTEllipsoidFit directory. Navigate back into first and then try again. If you get this error:

error /bin/sh: 1: octave: not found

that means you haven't installed octave (see above, or just quick run sudo apt-get install octave). If you get this error:

I2C read error from 104, 114 - Failed to read fifo count

then one of your cords probably became unplugged :/ (it happened to me).

Finally, we calibrate the accelerometers by pressing a, then holding the sensor along the x axis (so x is pointed straight up). Begin, hit 'e' to activate the axis, wiggle it around gently, hit 'd' to disable it, rotate it so the other end of that axis is pointed down, then hit 'e' to reactivate it, then hit 'd' and space to change to the next axis. Then repeat this for y, and z, and then his s to save it all, and finally x to exit.

Now we have the calibration data. The important thing is to make sure the RTEllipsoidFit folder is one folder above our working directory. And just copy the "RTIMULib.ini" file to the working directory, otherwise none of it will work. To copy it over, do this:

cp RTIMULib.ini /home/pi/kts/scripts/

See the next step for more.

The MPU-9250 Python Script

As opposed to my previous endeavors, I only want true heading, and roll, pitch, rate of roll, rate of pitch, and rate of yaw (those last three are strictly for gee-whiz data logging, so I can go back and say "Wow, I was rolling really fast right there!").

This script extracts that information and runs as fast as it possibly can. Mine clocks in around 4 milliseconds, or 250 times per second. But I don't need a new NMEA sentence printed that quickly. So I only print it once per second. But for some reason, whenever I change the sample rate (poll_interval), it works really goofy. Sometimes it delays up to 5 seconds. Strange. Anyway, I wrote in the code to run at its normal rate, take a reading 10 times per second, dampen out the heading over three seconds, dampen out roll for one second, and then print it once per second.

It prints it out pretty much only for debugging purposes. But it also sends it to a UDP port, which goes to another script for more fun and filtering of other data inputs. Make a file called "imu.py" in the scripts folder, and run it from the terminal by typing "python imu.py" (if you run it like this, be sure to uncomment the "print imu_sentence" line to you actually see the output).

Or to make it easy, just mosey on over to this github page right here: https://github.com/OldCC/kts and download the imu.py script. I've copy and pasted it here for ease, but I won't be updating the code below (you'll have to go to github for that).

Be sure that the RTIMULib.ini calibration file generated in the previous step is in the same directory as this script, and that the RTEllipsoidFit folder is in the directory above this script. For example, the RTEllipsoidFit folder is located here:

/home/pi/kts/

and this script (imu.py) as well as the calibration file (RTIMULib.ini) are both located in:

/home/pi/kts/scripts/

Something worth noting: If it fails to initialize, it sends an NMEA sentence saying that. If it fails after it's been working, it sends an NMEA sentence that will go all the way through to my chartplotter or phone that lets me know its failed, and for how long it has been failed.

Example sentence:

$IIXDR,IMU_FAIL,1.2*6F


that lets me know it failed 1.2 minutes ago, and something's wrong. Usually it would mean one of the wires came loose.

The imu.py script:

 import sys, getopt  
   
 sys.path.append('.')  
 import RTIMU  
 import os.path  
 import time  
 import math  
 import operator  
 import socket  
   
 IMU_IP = "127.0.0.2"  
 IMU_PORT = 5005  
   
 SETTINGS_FILE = "RTIMULib"  
   
 s = RTIMU.Settings(SETTINGS_FILE)  
 imu = RTIMU.RTIMU(s)  
   
 # timers  
 t_print = time.time()  
 t_damp = time.time()  
 t_fail = time.time()  
 t_fail_timer = 0.0  
 t_shutdown = 0  
   
 if (not imu.IMUInit()):  
   hack = time.time()  
   imu_sentence = "$IIXDR,IMU_FAILED_TO_INITIALIZE*7C"  
   if (hack - t_print) > 1.0:  
     sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
     sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))  
     t_print = hack  
     t_shutdown += 1  
     if t_shutdown > 9:  
       sys.exit(1)  
   
 imu.setSlerpPower(0.02)  
 imu.setGyroEnable(True)  
 imu.setAccelEnable(True)  
 imu.setCompassEnable(True)  
   
 poll_interval = imu.IMUGetPollInterval()  
   
 # data variables  
 roll = 0.0  
 pitch = 0.0  
 yaw = 0.0  
 heading = 0.0  
 rollrate = 0.0  
 pitchrate = 0.0  
 yawrate = 0.0  
 magnetic_deviation = -13.7  
   
   
 # dampening variables  
 t_one = 0  
 t_three = 0  
 roll_total = 0.0  
 roll_run = [0] * 10  
 heading_cos_total = 0.0  
 heading_sin_total = 0.0  
 heading_cos_run = [0] * 30  
 heading_sin_run = [0] * 30  
   
 # sentences produces by the imu  
 iihdt0 = "$IIHDT,,T*0C"  
 iixdr0 = "$IIXDR,A,,D,ROLL,A,,D,PTCH,A,,D,RLLR,A,,D,PTCR,A,,D,YAWR*51"  
 iihdt = iihdt0  
 iixdr = iixdr0  
 freq = 1  
   
 while True:  
  hack = time.time()  
   
  # if it's been longer than 5 seconds since last print  
  if (hack - t_damp) > 5.0:  
      
    if (hack - t_fail) > 1.0:  
      t_one = 0  
      t_three = 0  
      roll_total = 0.0  
      roll_run = [0] * 10  
      heading_cos_total = 0.0  
      heading_sin_total = 0.0  
      heading_cos_run = [0] * 30  
      heading_sin_run = [0] * 30  
      t_fail_timer += 1  
      imu_sentence = "IIXDR,IMU_FAIL," + str(round(t_fail_timer / 60, 1))  
      cs = format(reduce(operator.xor,map(ord,imu_sentence),0),'X')  
      if len(cs) == 1:  
         cs = "0" + cs  
      imu_sentence = "$" + imu_sentence + "*" + cs  
      sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
      sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))  
      t_fail = hack  
      t_shutdown += 1  
   
  if imu.IMURead():  
   data = imu.getIMUData()  
   fusionPose = data["fusionPose"]  
   Gyro = data["gyro"]  
   t_fail_timer = 0.0  
   
   if (hack - t_damp) > .1:  
     roll = round(math.degrees(fusionPose[0]), 1)  
     pitch = round(math.degrees(fusionPose[1]), 1)  
     yaw = round(math.degrees(fusionPose[2]), 1)  
     rollrate = round(math.degrees(Gyro[0]), 1)  
     pitchrate = round(math.degrees(Gyro[1]), 1)  
     yawrate = round(math.degrees(Gyro[2]), 1)  
     if yaw < 90.1:  
       heading = yaw + 270 - magnetic_deviation  
     else:  
       heading = yaw - 90 - magnetic_deviation  
     if heading > 360.0:  
       heading = heading - 360.0  
         
     # Dampening functions  
     roll_total = roll_total - roll_run[t_one]  
     roll_run[t_one] = roll  
     roll_total = roll_total + roll_run[t_one]  
     roll = roll_total / 10  
     heading_cos_total = heading_cos_total - heading_cos_run[t_three]  
     heading_sin_total = heading_sin_total - heading_sin_run[t_three]  
     heading_cos_run[t_three] = math.cos(math.radians(heading))  
     heading_sin_run[t_three] = math.sin(math.radians(heading))  
     heading_cos_total = heading_cos_total + heading_cos_run[t_three]  
     heading_sin_total = heading_sin_total + heading_sin_run[t_three]  
     heading = round(math.degrees(math.atan2(heading_sin_total/30,heading_cos_total/30)),1)  
     if heading < 0.1:  
       heading = heading + 360.0  
   
     t_damp = hack  
     t_one += 1  
     if t_one == 10:  
       t_one = 0  
     t_three += 1  
     if t_three == 30:  
       t_three = 0  
    
     if (hack - t_print) > 1:  
       hdt = "IIHDT," + str(round(heading))[:-2] + ",T"  
       hdtcs = format(reduce(operator.xor,map(ord,hdt),0),'X')  
       if len(hdtcs) == 1:  
         hdtcs = "0" + hdtcs  
       iihdt = "$" + hdt + "*" + hdtcs  
       
       xdr = "IIXDR,A," + str(int(round(roll))) + ",D,ROLL,A," + str(int(round(pitch))) + ",D,PTCH,A," + str(int(round(rollrate))) + ",D,RLLR,A," + str(int(round(pitchrate))) + ",D,PTCR,A," + str(int(round(yawrate))) + ",D,YAWR"  
       xdrcs = format(reduce(operator.xor,map(ord,xdr),0),'X')  
       if len(xdrcs) == 1:  
         xdrcs = "0" + xdrcs  
       iixdr = "$" + xdr + "*" + xdrcs  
   
       imu_sentence = iihdt + '\r\n' + iixdr  
   
       sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
       sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))  
   
       t_print = hack  
       
   time.sleep(poll_interval*1.0/1000.0)  

Tuesday, February 23, 2016

An Arduino NMEA-0183 Multiplexer - Final Update



Long story short, I've decided to abandon the Arduino. The main reasons are because it can't hold good calibration data for the IMU (leading to bad compass data), it locks up whenever the IMU freezes (or if a wire wiggles loose), and it's a whole lot of unnecessary wires and parts.

But I'm not giving up entirely. I've simply decided to move everything to the Raspberry Pi! I've been using it already to suck the data stream from the Arduino, and then pump that wirelessly to my laptop for OpenCPN. I've already got it up and working, complete with total calibration of the imu, and fault protection. Expect an update on that sometime soon.

Even though I'm done with the Arduino (I'm still going to use it for something.. probably the wind instrument over here), I'm still going to post my final code so that if someone else wants to pick and pull different sections for their own projects, it's available here.

As always, this is written for someone who knows next to nothing about programming--so this isn't the most efficient code out there. It is far from perfect, but it's easy for simpletons like me to understand.

The Whole Arduino NMEA-0183 Code

 ////////////////////////////////////////////////////////////////////////////  
 //  
 // 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.  
 //  
 // BME280 License:  
 // Marshall Taylor @ SparkFun Electronics  
 // https://github.com/sparkfun/SparkFun_BME280_Arduino_Library  
 // This code is released under the [MIT License](http://opensource.org/licenses/MIT).  
 // Please review the LICENSE.md file included with this example. If you have any questions   
 // or concerns with licensing, please contact techsupport@sparkfun.com.  
 // Distributed as-is; no warranty is given.  
 //  
 // Further modified by King Tide Sailing (kingtidesailing.blogspot.com)  
 // Thanks to https://mechinations.wordpress.com for the checksum function  
 //  
 ////////////////////////////////////////////////////////////////////////////  
   
 #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 <stdint.h>  
 #include <SparkFunBME280.h>  
 #include <nmea.h>  
   
 RTIMU *imu;               // the IMU object  
 RTFusionRTQF fusion;          // the fusion object  
 RTIMUSettings settings;         // the settings object  
 BME280 atmo;       // the BME-280 Atmospheric Sensor  
 NMEA nmeaDecoder(ALL);         // NMEA data decoder for all sentence types (for the Transducer, and Anemometer)  
    
 // NMEA-0183 Sentences to Print:    
 boolean outputgps = true; // gps  
 boolean outputvhw = true; // waterspeed and heading (speed of vessel relative to water)  
 boolean outputdpt = true; // depth    
 boolean outputmtw = true; // mean temperature of water  
 boolean outputvlw = true; // distance through water   
 boolean outputhdm = true; // magnetic heading    
 boolean outputhdt = true; // true heading    
 boolean outputrot = true; // rate of turn    
 boolean outputxdr = true; // pitch and roll (and rate of pitch and roll)  
 boolean outputvdr = true; // set and drift   
 boolean outputmda = true; // meteorological composite  
   
 // User defined variables (for your own boat, geographic location, and preferences):  
 float magd = -13.73; // magnetic deviation; if it's East, it's negative (13.73E in San Francisco 2015)    
 float tdo = .5;   // transducer offset from waterline (positive) or from bottom of keel (negative) (in meters)    
 float tda = 23.0;  // triducer angle from straight down (in degrees, positive to starboard)  
 float pof = -.07;  // pitch offset from neutral  
 float rof = .39;   // roll offset from neutral  
   
 // Dampening variables  
 const int five = 5;  
 const int ten = 10;  
 const int thirty = 30;  
 int tgps = 0;  
 int tvhw = 0;  
 int t1 = 0;  
 int tp = 0;  
 int tr = 0;  
 int t3 = 0;  
 int t10 = 0;  
 float sogrun[five];  
 float sogtotal = 0;  
 float cogxrun[five];  
 float cogxtotal = 0;  
 float cogyrun[five];  
 float cogytotal = 0;  
 float vhwrun[five];  
 float vhwtotal = 0;  
 float hdmxrun[thirty];  
 float hdmyrun[thirty];  
 float hdmxtotal = 0;  
 float hdmytotal = 0;  
 float rollrun[ten];  
 float rolltotal = 0;  
 float pitchrun[ten];  
 float pitchtotal = 0;  
 float rrlrun[ten];  
 float rrltotal = 0;  
 float rphrun[ten];  
 float rphtotal = 0;  
 float rywrun[ten];  
 float rywtotal = 0;  
 float vdrxrun[ten];  
 float vdryrun[ten];  
 float vdrdrun[ten];  
 float vdrxtotal = 0;  
 float vdrytotal = 0;  
 float drifttotal = 0;  
   
 // Global variables:  
 #define DISPLAY_INTERVAL 100  // rate (in milliseconds) in which MPU9150 results are displayed. This also drives universal counters for ***ref above  
 int gpsworking = 100;      // this ticks up everytime the MPU runs, which is defined in DISPLAY INTERVAL and is used as a counter  
 int wspworking = 100;      // so that if no GPS or Waterspeed data is received for 10 seconds (wspworking*DISPLAY_INTERVAL), then VDR won't calculate  
 int mtwworking = 100;  
 unsigned long lastDisplay;  
 unsigned long lastRate;  
 int sampleCount;  
 boolean vlwfirst = true; // used one time for the VLW sentence  
 float tcd0;       // so is this  
 float r = M_PI / 180.0f; // degrees to radians  
 float d = 180.0f / M_PI; // radians to degrees  
 int t60 = 49;      // used for the temperature sensor  
   
 // Variables exchanged between instruments:  
 float sog; // speed over ground  
 float cog; // course over ground  
 float wsp; // water speed, or hull speed through the water  
 float mtw; // mean temp of water  
 float hdm; // magnetic heading  
 float hdt; // true heading  
 float roll; // roll in degrees  
 float pitch; // pitch  
 float yaw; // yaw (0 yaw corresponds to 270 magnetic)  
 float ror; // rate of roll (degrees per second, negative to port)  
 float rop; // rate of pitch  
 float roy; // rate of yaw  
   
 void setup()  
 {  
  int errcode;  
   
  Serial.begin(57600);  // output port to computer  
  Serial1.begin(4800);  // from GPS  
  Serial2.begin(4800);  // from DST-800 transducer  
  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);  
   
  // Setup the dampening functions  
  for (int i = 0; i < five; i++) {  
   sogrun[i] = 0;  
   cogxrun[i] = 0;  
   cogyrun[i] = 0;  
   vhwrun[i] = 0;  
  }  
  for (int i = 0; i < thirty; i++) {  
   hdmxrun[i] = 0;  
   hdmyrun[i] = 0;  
  }  
  for (int i = 0; i < ten; i++) {  
   rollrun[i] = 0;  
   pitchrun[i] = 0;  
   rrlrun[i] = 0;  
   rphrun[i] = 0;  
   rywrun[i] = 0;  
   vdrxrun[i] = 0;  
   vdryrun[i] = 0;  
   vdrdrun[i] = 0;  
  }  
   
  // BME-280 Setup  
  atmo.settings.commInterface = I2C_MODE;  
  atmo.settings.I2CAddress = 0x77;  
  atmo.settings.runMode = 3;  
  atmo.settings.tStandby = 0;  
  atmo.settings.filter = 0;  
  atmo.settings.tempOverSample = 1;  
  atmo.settings.pressOverSample = 1;  
  atmo.settings.humidOverSample = 1;  
  atmo.begin();  
 }  
   
 // 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;  
 }  
   
 // Dampen Function  
 float dampen(float array[], int arraysize, float data, float &total, int t){  
  total = total - array[t];  
  array[t] = data;  
  total = total + array[t];  
  return total / arraysize;  
 }  
   
 void loop()  
 {  
    
  // ***** BU-353 GPS ***** //   
   if (Serial1.available()) {   
   if (nmeaDecoder.decode(Serial1.read())) {  
    char* title = nmeaDecoder.term(0);  
    if (strcmp(title,"GPRMC") == 0) {  
    gpsworking = 0; // resets the "gps is bad" counter back to 0 since it's good  
    sog = nmeaDecoder.term_decimal(7);   
    cog = nmeaDecoder.term_decimal(8);  
    if (sog == 0.0) { // this removes artifacts in cog when sog = 0  
     cog = hdt;   
    }  
   
    // Dampen the last 5 readings  
    float cogxave = dampen(cogxrun, five, cos(cog*r), cogxtotal, tgps);  
    float cogyave = dampen(cogyrun, five, sin(cog*r), cogytotal, tgps);  
    cog = atan2(cogyave,cogxave)*d;  
    if (cog<0.0){  
     cog=360+cog;  
    }  
    sog = dampen(sogrun, five, sog, sogtotal, tgps);  
    tgps++;  
    if (tgps>=five){  
     tgps=0;  
    }  
    if (outputgps == true) {  
     char rmcSentence [71];    
     byte csg;    
     PString strg(rmcSentence, sizeof(rmcSentence));    
     strg.print("$GPRMC,");   
     strg.print(nmeaDecoder.term_decimal(1));   
     strg.print(",");   
     strg.print(nmeaDecoder.term(2));   
     strg.print(",");   
     strg.print(nmeaDecoder.term_decimal(3));   
     strg.print(",");   
     strg.print(nmeaDecoder.term(4));   
     strg.print(",");   
     strg.print(nmeaDecoder.term_decimal(5));   
     strg.print(",");   
     strg.print(nmeaDecoder.term(6));   
     strg.print(",");   
     strg.print(sog);  
     strg.print(",");   
     strg.print(cog);   
     strg.print(",");   
     strg.print(nmeaDecoder.term_decimal(9));   
     strg.print(",,,");   
     strg.print(nmeaDecoder.term(12));   
     strg.print("*");   
     csg = checksum(rmcSentence);    
     if (csg < 0x10) strg.print('0');    
     strg.print(csg, HEX);    
     Serial.println(rmcSentence);    
    }  
    }  
    else {  
    Serial.println(nmeaDecoder.sentence());   
    }  
   }  
   }   
  // ***** End BU-353 GPS ***** //  
   
  // ***** DST-800 Transducer ***** //    
   if (Serial2.available() > 0 ) {    
   if (nmeaDecoder.decode(Serial2.read())) {    
    char* title = nmeaDecoder.term(0);  
   
    // 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);  
   
     // Corrections for heel and speed  
     float wsp1 = wsp0;  
     if (wsp0 > 15){  
      wsp1 = 15;  
     }  
     float pwa = abs(tda - roll); // 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;  
     wsp = dampen(vhwrun, five, wsp, vhwtotal, tvhw); //dampen over the past 5 readings  
     tvhw++;  
     if (tvhw>=five){  
      tvhw=0;  
     }  
       
     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.15078);    
      strw.print(",M*");    
      csw = checksum(vhwSentence);    
      if (csw < 0x10) strw.print('0');    
      strw.print(csw, HEX);    
      Serial.println(vhwSentence);    
      }    
    }  
   
   // DPT Depth Sentence //  
   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-roll)*r));    
     strd.print(",");    
     strd.print(tdo);    
     strd.print("*");    
     csd = checksum(dptSentence);    
     if (csd < 0x10) strd.print('0');    
     strd.print(csd, HEX);    
     Serial.println(dptSentence);    
    }    
   }  
   
   // MTW Mean Temperature of Water Sentence //  
   if (strcmp(title,"YXMTW") == 0) {  
    mtwworking = 0;  
    mtw = nmeaDecoder.term_decimal(1)*(9/5)+32;  
    if (outputmtw == true) {   
     char mtwSentence [19];  
     byte csf;  
     PString strf(mtwSentence, sizeof(mtwSentence));    
     strf.print("$YXMTW,");    
     strf.print(mtw);    
     strf.print(",F*");    
     csf = checksum(mtwSentence);    
     if (csf < 0x10) strf.print('0');    
     strf.print(csf, HEX);    
     Serial.println(mtwSentence);    
    }    
   }  
   
   // VLW Voyage Log Sentence //  
   if (outputvlw == true) {    
    if (strcmp(title,"VWVLW") == 0) {    
     float tcd = nmeaDecoder.term_decimal(1);    
     if (vlwfirst == true) {    
      tcd0 = tcd;    
      vlwfirst = false;    
     }    
     float dsr = tcd-tcd0;    
     char vlwSentence [25];    
     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 Instrumentation (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) {  
      
    if (gpsworking < 100) {  
     gpsworking++;  
    }  
    if (wspworking < 100) {  
     wspworking++;  
    }  
    if (mtwworking < 100) {  
     mtwworking++;  
    }  
    lastDisplay = now;  
    RTVector3 pose = fusion.getFusionPose();  
    RTVector3 gyro = imu->getGyro();  
    roll = pose.y() * -1 * d - rof; // negative is port  
    pitch = pose.x() * d - pof;    // negative is nose down  
    yaw = pose.z() * d;    // zero yaw is 270 magnetic  
    ror = gyro.y() * -1 * d;  
    rop = gyro.x() * d;  
    roy = gyro.z() * d;  
    hdm = yaw - 90;  
    if (yaw < 90 && yaw >= -179.99) {  
     hdm = yaw + 270;  
    }  
   
    // Dampen everything out over the last second  
    float hdmxave = dampen(hdmxrun, thirty, cos(hdm*r), hdmxtotal, t3);  
    float hdmyave = dampen(hdmyrun, thirty, sin(hdm*r), hdmytotal, t3);  
    hdm = atan2(hdmyave,hdmxave)*d;  
    if (hdm<0.0){  
     hdm=360+hdm;  
    }  
    pitch = dampen(pitchrun, ten, pitch, pitchtotal, t1);  
    roll = dampen(rollrun, ten, roll, rolltotal, t1);  
    ror = dampen(rrlrun, ten, ror, rrltotal, t1);  
    rop = dampen(rphrun, ten, rop, rphtotal, t1);  
    roy = dampen(rywrun, ten, roy, rywtotal, t1);  
      
    hdt = hdm - magd; // calculate true heading  
    if (hdt > 360.0) {  
     hdt = hdt - 360;  
    }  
    if (hdt < 0.0) {  
     hdt = hdt + 360;  
    }  
   
    // HDM Magnetic Heading //  
    if (outputhdm == true){  
     if (t1 == 9){  
      char hdmSentence [23];  
      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) {  
     if (t1 == 9){  
      char hdtSentence [23];  
      byte cst;  
      PString strt(hdtSentence, sizeof(hdtSentence));  
      strt.print("$IIHDT,");  
      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) {  
     if (t1 == 9){  
      char rotSentence [18];  
      byte csr;  
      PString strr(rotSentence, sizeof(rotSentence));  
      strr.print("$TIROT,");  
      strr.print(roy*cos(pitch)*cos(roll)*60);  
      strr.print(",A*");  
      csr = checksum(rotSentence);  
      if (csr < 0x10) strr.print('0');  
      strr.print(csr, HEX);  
      Serial.println(rotSentence);  
     }  
    }  
   
    // XDR Pitch and Roll //  
    if (outputxdr == true) {  
     if (t1 == 9){  
      char xdrSentence [89];  
      byte csx;  
      PString strp(xdrSentence, sizeof(xdrSentence));  
      strp.print("$IIXDR,A,");  
      strp.print(pitch);  
      strp.print(",D,PTCH,A,");  
      strp.print(roll);  
      strp.print(",D,ROLL,A,");  
      strp.print(rop);  
      strp.print(",D,RPTC,A,");  
      strp.print(ror);  
      strp.print(",D,RROL,A,");  
      strp.print(roy);  
      strp.print(",D,RYAW*");  
      csx = checksum(xdrSentence);  
      if (csx < 0x10) strp.print('0');  
      strp.print(csx, HEX);  
      Serial.println(xdrSentence);  
        
      /*  
      char bdrSentence [37];  
      byte cs3;  
      PString str3(bdrSentence, sizeof(bdrSentence));  
      str3.print("$IIXDR,P,");  
      str3.print(atmo.readFloatPressure()*.00001,5);  
      str3.print(",B,Barometer*");  
      cs3 = checksum(bdrSentence);  
      if (cs3 < 0x10) str3.print('0');  
      str3.print(cs3, HEX);  
      Serial.println(bdrSentence);  
   
      char tdrSentence [37];  
      byte cs4;  
      PString str4(tdrSentence, sizeof(tdrSentence));  
      str4.print("$IIMTA,");  
      str4.print(atmo.readTempF());  
      str4.print(",F*");  
      cs4 = checksum(tdrSentence);  
      if (cs4 < 0x10) str4.print('0');  
      str4.print(cs4, HEX);  
      Serial.println(tdrSentence);  
      */  
        
     }  
    }  
      
     // VDR Set & Drift //   
     if (outputvdr == true) {  
      if (gpsworking < 100 && wspworking < 100) {  
      if (t1 == 9) {  
      float set0 = (cog-hdt)*r; // gets the difference between hdt and cog in radians   
      if (set0 < 0) {   
       set0 = set0+(2*M_PI);   
      }  
      float drift = sqrt(pow(wsp,2)+pow(sog,2)-2*wsp*sog*(cos(set0)));   
      float setr = (M_PI-atan2((sog*sin(set0)),(wsp-sog*cos(set0))))*d;   
      if (wsp == 0 && sog == 0) { // if you're not moving, then set = 0   
       setr = set0;  
      }  
      float set = hdt+setr;   // converting to true instead of relative degrees   
      if (set > 360) {   
       set = set-360;   
      }  
   
      // Dampen out VDR over the last 10 seconds  
      float vdrxave = dampen(vdrxrun, 10, cos(set*r), vdrxtotal, t10);  
      float vdryave = dampen(vdryrun, 10, sin(set*r), vdrytotal, t10);  
      set = atan2(vdryave,vdrxave)*d;  
      if (set<0.0){  
       set=180*(set+1);  
      }  
      drift = dampen(vdrdrun, 10, drift, drifttotal, t10);  
        
      char vdrSentence [37];    
      byte csv;   
      PString strv(vdrSentence, sizeof(vdrSentence));   
      strv.print("$IIVDR,A,");   
      strv.print(set);   
      strv.print(",T,");   
      strv.print(setr);   
      strv.print(",R,");  
      strv.print(drift);   
      strv.print(",N*");   
      csv = checksum(vdrSentence);   
      if (csv < 0x10) strv.print('0');   
      strv.print(csv, HEX);   
      Serial.println(vdrSentence);  
      }  
     }   
     }  
   
     // MDA Meteorological Composite //  
     if (outputmda == true) {  
      if (t1 == 9){  
       float temp = atmo.readTempC();  
       float tempk = atmo.readTempC()+273.15;  
       float pres = atmo.readFloatPressure()*.00001;  
       float rh = atmo.readFloatHumidity();  
       float dewp = (tempk*sqrt(1+(tempk*log(rh/100))/((3.167485-.00244*tempk)*1000000/461.5))-273.15);//*(9/5)+32;  
       char mdaSentence [69];  
       byte csa;  
   
       PString strm(mdaSentence, sizeof(mdaSentence));  
       strm.print("$IIMDA,");  
       strm.print(atmo.readFloatPressure()*.0002953);  
       strm.print(",I,");  
       strm.print(pres);  
       strm.print(",B,");  
       strm.print(temp,1);  
       strm.print(",C,");  
       if (mtwworking<10){  
        strm.print(mtw);  
        strm.print(",C,");  
       }  
       else {  
        strm.print(",C,");  
       }  
       strm.print(rh,0);  
       strm.print(",,");  
       strm.print(dewp);  
       strm.print(",C,,T,,M,,N,,M*");  
       csa = checksum(mdaSentence);  
       if (csa < 0x10) strm.print('0');  
       strm.print(csa, HEX);  
       Serial.println(mdaSentence);  
       t60=0;  
      }  
     }  
   
     // Timers tick up  
     t1++;  
     tp++;  
     tr++;  
     t3++;  
     if(t1>9){  
     t1=0;  
     tp=0;  
     tr=0;  
     t10++;  
     t60++;  
     if (t10>9) {  
      t10 = 0;  
     }  
     if (t60>59){  
       t60=0;  
      }  
     }  
     if (t3>=29){  
      t3=0;  
     }  
   }  
  }  
  // ***** End Integrated Navigation ***** //  
   
 }  

The Arduino Pre-Setup

Most of it is pretty self-explanatory. If not, just browse my previous posts to get a more clear picture. A few changes, though:
  • I added a pitch and roll offset (pof and rof). I left my boat sitting in its slip for a minute, and recorded the average pitch and roll. That's what you see there. Of course, getting down to tenths of a degree is pretty absurd because it doesn't really change anything.
  • Transducer offset. My triducer (this one here: Airmar DST-800 Thru-Hull Triducer Smart Sensor) is mounted on the right side of my boat, at an angle of 23 degrees from straight down.
  • Dampening variables. A few posts ago, I decided to dampen everything. That's a good idea, but pretty much just for heading and VDR. Why's that? Well, the boat is large enough that any speed changes won't take effect for a couple of seconds, thereby negating most dampening benefits. Also, I looked at my "undampened" logs, and the speeds don't really change enough to warrant the extra code. Oh well.
Most everything else in the pre-setup are just timers and variables exchanged between instruments.

Setup()

Self-explananotry, and/or already covered. The differences here are the initialization of the dampening variables, which is necessary for it to work properly, and the addition of the BMP-280 Atmospheric Sensor, which I picked up from sparkfun.com for about $12. It measures temperature, humidity, and pressure, all of which are useful boat numbers.

The Loop()

No need to go over what I've already covered. I dampen out the GPS info, which is totally unnecessary, and whenever my groundspeed is zero, I change the course to my true heading (just a pet peeve when I'm sitting in my slip trying to troubleshoot and my chart plotter is spinning all over).

Next up, the triducer. In this post (http://kingtidesailing.blogspot.com/2015/12/how-to-calibrate-water-speed-for-vessel.html), I described how to correct the paddle wheel/hullspeed/waterspeed data for sensor heel and speed. That's pretty much all that's going on there. Oh, I also add my heading to the sentence. That's kind of useful, since now I only need to log this sentence and the RMC sentence for heading/waterspeed, and course/groundspeed.

I also add a .5 meter offset to the depth sounding to give me depth of the water (my sensor is .5 meters down from the surface), as well as correct it for boat heel and pitch (technically, a boat heeled over at 90 degrees will give a depth sounding reading to the moon).

Temp of the water? I convert it to F, instead of C.

With the VLW sentence, I simply run a quick bit of code to give me the total distance travelled since initialization, since that's impossible otherwise. This is just a bit of gee-whiz info.

The IMU

On to the heart of the code (and it's downfall). The IMU, which I've already explained in this post, runs at 10 times per second, but only prints a sentence once per second. I dampen out pretty much everything (unnecessarily so), and reformatted the XDR sentence to be compatible with NKE marine instruments. 

I've also added two additional sentences for Air Temp and Barometer, which are supposedly compatible with NKE, but I couldn't get them to work. Or with OpenCPN. Which is why they are commented out.

Then we're on to Set and Drift, which was explained here (http://kingtidesailing.blogspot.com/2015/10/set-and-drift-or-speed-and-direction-of.html), and then finally I've implemented a quick routine for the BMP-280 Atmospheric Sensor.

NMEA-0183 MDA Meteorological Composite

I dusted off my old college textbooks, and figure out how to derive the dewpoint from temperature, pressure, and humidity, and compiled it all in the MDA sentence. The MDA sentence, however, is no longer supported by most things. I think it worked with OpenCPN, but it was giving me such a headache that I kind of just quit.

It goes without saying that the barometer is of utmost importance to the sailor, but for me, I mainly want to know the humidity inside the cabin. I'm tired of re-oiling all my woodwork.

Conclusion of the Arduino

I haven't given up on getting calibrated, good data from my boat. I've just given up on using the Arduino because of its limitations.

Like I said earlier, I already have the Raspberry Pi up and working using some python scripts (which I had to learn), but until I get that posted here, I highly suggest you check out the OpenPlotter project, which is essentially what I was trying to do (until I discovered the OpenPlotter project).

Go check it out over here (http://www.sailoog.com/en), since most of that works right out of the box, and it's cheap, and, well, who wants to duplicate effort?


Thursday, February 11, 2016

How to Dampen/Smooth Degrees Between 360 and 0 for Heading, Course, Speeds and Other Sailboat Instruments



I believe one of the issues I'm facing is the resolution of the readings from my boat's sensors. The MPU can output a reading incredibly fast (at speeds up to every 51 milliseconds). But that's kind of overkill for my chartplotter. Further more, the only application that I can think of that would need a heading reading of greater than once per second is radar stabilization, in which case you need 10 readings per second. Which is no problem for the MPU.

But when I print out a heading sentence once per second, it's simply taking the heading at that very instant. And as we all know, boat's sway back and forth, and that heading might not be very accurate. It's the same thing with speeds too--the second you take a reading, it might be unusually fast or slow, and then between readings it returns to its normal speed.

The way to fix this is to dampen or smooth out the sensor readings.

Dampening on an Arduino

Velocities are easy to dampen out. I adapted the Arduino tutorial found here, and came up with a quick Arduino C/C++ function to dampen out readings.

The idea is really simple. Take the average of, say, the last 10 readings, and the result is your dampened reading. This is pretty easy to implement in an Arduino code. In fact, it's a good idea, I've realized, to dampen out pretty much everything.

Since I'm only taking one reading per second for most things, I want that reading to reflect the average over the last second. So, let's start with something like roll, or vessel heel.

Before we even begin, we need to declare a total number of readings variable (this doesn't change at all), a timer variable (think of the "i" in a for loop), an array to store each individual reading, and then finally a variable that contains the sum total of all the raw readings

 // Dampening variables   
 const int ten = 10;  // the total number of readings we will dampen
 int t1 = 0;          // the timer variable that ticks up every cycle of the MPU
 float rollrun[ten];  // the array that stores each individual raw reading
 float rolltotal = 0; // the sum total of all readings

Next, we have to "initialize" the dampening variables. We do this inside the void setup() function of the code:

 for (int i = 0; i < ten; i++) {  
   rollrun[i] = 0;  
  }  

Finally, since I'm going to be doing this over and over again for pretty much every reading, I decide to declare it as a function to save on code space and to make it easier. Here's the function code, which you declare outside of anything else (don't include it in the setup or loop portions of the code).

 // Dampen Function  
 float dampen(float array[], int arraysize, float data, float &total, int t){  
  total = total - array[t];  // subtract the oldest reading from the total
  array[t] = data;           // redeclare the oldest reading in the array as the latest
  total = total + array[t];  // add the latest reading to the total
  return total / arraysize;  // divide the sum total by the total number of readings
 }  

You can refer to this or this for the entire code, or wait for an update in a couple of weeks in which I post the fully updated code file. For this, we're only going to look at the roll of the boat (for now). Here's how we call this function:

 roll = pose.y() * -1 * 180.0f / M_PI;             // declares roll in degrees
 roll = dampen(rollrun, ten, roll, rolltotal, t1); // redeclares roll as a dampened value
   
 t1++;      // t1 ticks up once every iteration, which is set to 10hz (10 times/second)
 if(t1>9){  // if t1 = 9 (one second has elapsed), reset it to 0
     t1=0;  
 }  

dampen(rollrun, ten, roll, rolltotal, t1). "rollrun" is the array in which all values are stored, "ten" is the total number of readings, "roll"is the newest/latest reading, "rolltotal" is the sum total of all values, and "t1" is the timer that ticks up every iteration. Why do I have t1 reset when it equals 9 instead of 10? Because it starts at 0, not 1. And this is important because the array has a position for point "0". An array with 10 total values is actually an array from point 0 to point 9.

The resulting output is the running average over the last second. It doesn't matter that the roll might fluctuate between -6 degrees (negative is to the left) and +5 degrees (positive is to the right). The average of those will make sense and give a more accurate reading than without dampening.

But what about degrees of heading that pass between 360 and 0 degrees?

Dampening Headings and Courses between 360 and 0 degrees

The average of a 355 heading and a 005 heading is due north, either 360 or 0 (I prefer to use heading 360 in this case, since a heading of 0 degrees just doesn't sound right). But if you plug that into the function about, it will give you a dampened value of 180 degrees--the complete opposite. It's actually really easy to fix it, and as usual, I've done all the ground work for you.

The key is to split the heading into its cosine and a sine, and take the dampened value of those. You have to use both, however. You can't just take the cosine and dampen it out. The cosine of 10 degrees is .98, while the cosine of 350 degrees is also .98. It's an identical value. So if you just dampened out the cosine, when you turn it back into a degree value, it could either be 10 degrees, or 350 degrees.

But if you incorporate the sine as well, you can figure it out. The secret lies in the atan2() function on the Arduino. I've used this before here when I derived the speed and direction of the current. Using the magic of math and other stuff, if you take the sine of the heading, and the cosine, and call atan2(sine,cosine), it will give you the correct info.

So let's dampen out our heading.

We have to declare two dampening sets, one for the sine and the cosine.

 // dampening variables  
 const int thirty = 30;  
 int t3 = 0;  
 float hdgxrun[thirty];  
 float hdgyrun[thirty];  
 float hdgxtotal = 0;  
 float hdgytotal = 0;  

However, I want to dampen the heading over the past three seconds. Since the MPU runs at 10hz, that means I'll have 30 total readings to dampen.

Then initialize the variables in the setup():

 for (int i = 0; i < thirty; i++) {  
   hdgxrun[i] = 0;  
   hdgyrun[i] = 0;  
  }  

We've already declared the dampen function, so here's the code to dampen out the heading.

 hdm = yaw - 90;
 if (yaw < 90 && yaw >= -179.99) {
   hdm = yaw + 270;  
 }  

 // Dampen it out  
 float hdgxave = dampen(hdgxrun, thirty, cos(hdm * M_PI / 180.0f), hdgxtotal, t3);  
 float hdgyave = dampen(hdgyrun, thirty, sin(hdm * M_PI / 180.0f), hdgytotal, t3);  
 hdm = atan2(hdgyave,hdgxave) * 180.0f / M_PI;
 if (hdm<0.0){  
  hdm=360+hdm;  
 }

 t3++;
 if(t3>29){
     t3=0;  
 } 

Please note that the mathematical functions use radians, instead of degrees. So I convert the heading to radians before I get the cosine or sine, and then reconvert it back into degrees after I use the atan2 function. Also, since sometimes that spits out negative degrees, I have to add 360 if needed.

Supposedly, if you try to do some sort of atan2 function or dampen between 090 and 270, it will return an error. I haven't ops tested that, and I wouldn't worry too much, since if you can move your boat 180 degrees in less than 3 seconds, you probably have bigger things on your mind (such as you massive bank account to afford such an incredible boat).

If you are trying to dampen out something like the wind, which has a degree value and a velocity, then you use the cosine  and sine routine above for the angle, but the velocity value itself can be dampened out no problem like I did for the roll.

Dampened and Smoothed Sailboat Sensors

My new code dampens out my GPS course over ground / speed over ground over the last 5 seconds, the boat speed through the water over the last 5 seconds, the heading over the last 3 seconds, the roll, pitch, rate of roll, rate of pitch, and rate of yaw over the last 1 second (why not yaw itself? Well, I do dummy, it's called heading!), and tidally induced current set/drift over the last 10 seconds.

The only thing left to do is dampen out heading as a function of rate of turn, but that might be a bit beyond me for now. I think three seconds is fine enough.