Still working on the Autopilot

It’s been a few months since I’ve made any tangible progress on the autopilot project.  Work and life has a way of interfering with my recreational activities. But this evening I decided to start to work on the Arduino side of the CPN Pi-lot project. Hopefully this post doesn’t get too technical…

The Arduino side of the system will need to be a “smart” component. Simply stated, given a desired destination heading, it will adjust the rudder angle accordingly so that the boat will eventually reach the destination.

First, to receive the chartplotter destination, it will receive and parse an RMB NMEA message, which is produced by OpenCPN, and which is defined as so:

$GPRMB,A,0.66,L,003,004,4917.24,N,12309.57,W,001.3,052.5,000.5,V*20
where:
           RMB          Recommended minimum navigation information
           A            Data status A = OK, V = Void (warning)
           0.66,L       Cross-track error (nautical miles, 9.99 max),
                                steer Left to correct (or R = right)
           003          Origin waypoint ID
           004          Destination waypoint ID
           4917.24,N    Destination waypoint latitude 49 deg. 17.24 min. N
           12309.57,W   Destination waypoint longitude 123 deg. 09.57 min. W
           001.3        Range to destination, nautical miles (999.9 max)
           052.5        True bearing to destination
           000.5        Velocity towards destination, knots
           V            Arrival alarm  A = arrived, V = not arrived
           *20          checksum

By combining the range and bearing to the destination, with the current heading, the Arduino will be able to determine how much rudder to adjust. This algorithm will be an Arduino implementation of a common PID Controller, but will be the subject of a future article, after it gets implemented.

If you read more on PID loops, you will see that the algorithm requires a feedback loop (PV or process variable in the equations)… as in, an actual measurement of the item being controlled. In an autopilot situation, this feedback is the current heading of the boat, received either from a GPS, compass or other device. After some research and communication with people that have built autopilots for things like quad-copters, it has become clear that GPS simply doesn’t have the resolution necessary to effectively steer a vehicle, even at the relatively slow speeds of a trawler (roughly six knots through the water).

So the Arduino component will need to receive the current heading via a digital compass, and that is what I worked on today.

A few weeks ago, I purchased a LSM303DLHC 3-axis Accelerometer and Magnetometer board … aka a digital compass. And tonight I wired it up to an Arduino, and was able to receive a fairly reliable compass heading. I hooked up a small LCD to it that displayed the heading as well as the cardinal direction it correlated to. Then I turned it on in my truck, and drove around town comparing what the ardino claimed was the direction to the direction on the GPS and dashboard compass. Thankfully, they were very close; the differences are attributed to the lack of tilt-compensation in the calculation.

The Arduino Code, as it stands today is copied here:

//Autopilot.h
typedef struct TiltCompass_s
{
	float Heading;
	float Pitch;
	float Roll;

	float X;
	float Y;
	float Z;

	float MaxX;
	float MaxY;
	float MaxZ;
	float MinX;
	float MinY;
	float MinZ;

	float XMean;
	float YMean;
	float ZMean;
	float XLeng;
	float YLeng;
	float ZLeng;
} TiltCompass;

//Autopilot.ino

#include <Wire.h>
#include <SoftwareSerial.h>
#include <Print.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include "Autopilot.h"

SoftwareSerial lcd(3,2); // pin 2 = TX, pin 3 (unused)
const int LOOP_DELAY = 500;

TiltCompass Compass;
Adafruit_LSM303_Mag_Unified Sensor = Adafruit_LSM303_Mag_Unified(12345);

///Initiation Method
void setup() {
  Serial.begin(9600);
  Wire.begin();
  InitCompass();

  lcd.begin(9600); // set up serial port for 9600 baud
  delay(500); // wait for display to boot up
}

///Main Program Loop
void loop() {

	GetHeading();
		
	//LCD_WriteOutput();
  
	delay(LOOP_DELAY);
}

///Initialize the Compass
void InitCompass(void)
{
	//Start LSM303 Module
	if(!Sensor.begin())
	{
		Serial.println("Invalid sensor!");
		LCD_Clear();
		lcd.write("Invalid Sensor!");
		while(1);
	}

	//Calibrate sensore values to values discovered for the sensor
	SetCalibrationValues();
}

///Method to get the Compass heading from the LSM303DLHC module
void GetHeading()
{
	sensors_event_t magEvent; 
	Sensor.getEvent(&magEvent);

    AdjustCalibration(magEvent);

	float computedX = (magEvent.magnetic.x - Compass.XMean)/Compass.XLeng; // normalized x-component of acceleration
	float computedY = (magEvent.magnetic.y - Compass.YMean)/Compass.YLeng; // normalized y-component of acceleration
	float computedZ = (magEvent.magnetic.z - Compass.ZMean)/Compass.ZLeng; // normalized z-component of acceleration
	//float acceleration = sqrt(pow(computedX,2)+pow(computedY,2)+pow(computedZ,2)); 
    
	// Calculate the angle of the vector y,x
	Compass.Heading = (atan2(computedY, computedX) * 180) / PI;
	if (Compass.Heading < 0)
	{
		Compass.Heading += 360;
	}

	//Output 
	Serial.print("Compass: ");
	Serial.print(Compass.Heading);
	Serial.print(" - ");
	Serial.println(GetCompassRoseDirection());
}

///Method to convert a Compass direction to a cardinal direction
String GetCompassRoseDirection()
{
	if (Compass.Heading > 11.25 & Compass.Heading < 33.75)
	{
		return "NNE";
	}if (Compass.Heading > 33.75 & Compass.Heading < 56.25)
	{
		return "NE";
	}if (Compass.Heading > 56.25 & Compass.Heading < 78.75)
	{
		return "ENE";
	}if (Compass.Heading > 78.75 & Compass.Heading < 101.25)
	{
		return "E";
	}if (Compass.Heading > 101.25 & Compass.Heading < 123.75)
	{
		return "ESE";
	}if (Compass.Heading > 123.75 & Compass.Heading < 145.25)
	{
		return "SE";
	}if (Compass.Heading > 145.25 & Compass.Heading < 168.75)
	{
		return "SSE";
	}if (Compass.Heading > 168.75 & Compass.Heading < 191.25)
	{
		return "S";
	}if (Compass.Heading > 191.25 & Compass.Heading < 213.75)
	{
		return "SSW";
	}if (Compass.Heading > 213.75 & Compass.Heading < 236.25)
	{
		return "SW";
	}if (Compass.Heading > 236.25 & Compass.Heading < 258.75)
	{
		return "WSW";
	}if (Compass.Heading > 258.75 & Compass.Heading < 281.25)
	{
		return "W";
	}if (Compass.Heading > 281.25 & Compass.Heading < 303.75)
	{
		return "WNW";
	}if (Compass.Heading > 303.75 & Compass.Heading < 326.35)
	{
		return "NW";
	}if (Compass.Heading > 326.35 & Compass.Heading < 348.75)
	{
		return "NNW";
	}

	return "N";
	
}

///Take into account new values for calibration
void AdjustCalibration(sensors_event_t &magEvent)
{
  if (magEvent.magnetic.x < Compass.MinX)
  {
	  Serial.println("Resetting MinX");
	  Compass.MinX = magEvent.magnetic.x;
	  SetNormalizationValues();
  }
  if (magEvent.magnetic.x > Compass.MaxX)
  {
	  Serial.println("Resetting MaxX");
	  Compass.MaxX = magEvent.magnetic.x;
	  SetNormalizationValues();
  }  
  
  if (magEvent.magnetic.y < Compass.MinY)
  {
	  Serial.println("Resetting MinY");
	  Compass.MinY = magEvent.magnetic.y;
	  SetNormalizationValues();
  }
  if (magEvent.magnetic.y > Compass.MaxY)  
  {
	  Serial.println("Resetting MaxY");
	  Compass.MaxY = magEvent.magnetic.y;
	  SetNormalizationValues();
  }

  if (magEvent.magnetic.z < Compass.MinZ)  
  {
	  Serial.println("Resetting MinZ");
	  Compass.MinZ = magEvent.magnetic.z;
	  SetNormalizationValues();
  } 
  if (magEvent.magnetic.z > Compass.MaxZ)  
  {
	  Serial.println("Resetting MaxZ");
	  Compass.MaxZ = magEvent.magnetic.z;
	  SetNormalizationValues();
  } 
}

///Method to display information on the LCD screen
void LCD_WriteOutput()
{
	LCD_Clear();
	lcd.write("Heading: ");
	lcd.print(round(Compass.Heading));
	lcd.write(254); // move cursor to beginning of first line
	lcd.write(192);
	lcd.write("Direction: ");
	lcd.print(GetCompassRoseDirection());
}

///Method to clear the LCD scren
void LCD_Clear()
{
	lcd.write(254); // move cursor to beginning of first line
	lcd.write(128);
	lcd.write("                "); // clear display
	lcd.write("                ");
	lcd.write(254); // move cursor to beginning of first line
	lcd.write(128);
}

///Set calibrations values
///  - Initial hardcoded numbers come from Calibration example project
void SetCalibrationValues()
{
	//Set Calibration Values from examples
	Compass.MinX = -60.45;
	Compass.MinY = -58.64;
	Compass.MinZ = -52.14;
	Compass.MaxX = 44.00;
	Compass.MaxY = 50.18;
	Compass.MaxZ = 78.06;
	SetNormalizationValues();
}

///Set values uses to normalize measured accelerations
void SetNormalizationValues()
{
	//Set Normalization Parameters
	Compass.XMean = (Compass.MaxX + Compass.MinX)/2;
	Compass.YMean = (Compass.MaxY + Compass.MinY)/2;
	Compass.ZMean = (Compass.MaxZ + Compass.MinZ)/2;
	Compass.XLeng = (Compass.MaxX - Compass.MinX)/2;
	Compass.YLeng = (Compass.MaxY - Compass.MinY)/2;
	Compass.ZLeng = (Compass.MaxZ - Compass.MinZ)/2;
}

References:

Advertisements