Arduino autopilot Help

Hi i have been working on this project for some time now i am using a Ardunino R3 and a honey well HMC6343 digital compass.
I would like to add GPS to this set up as the compass is not keeping the Kontiki on its bearing, it will dirft while under power to about 10 metres off course .I am looking at buying the Adafruit Ultimate GPS Breakout .It will be under water as it goes under breaking waves.
So i was wondering if anyone could help me to add a gps to my setup and how i can add it to my code that i have it is a very basic system as when i point the Kontiki in the direction of travel and switch on the Arduino the compass locks onto a bearing and adjusts the servo to keep the craft on its bearing.One other problem i am having is i need to adjust the servo from its center point to allow for propeller torque say 1 to 2 degrees to the right but i am unsure how to add this to my code.i have attach a picture for the people who are wondering what a kontiki is .I have added my code below

#include <Wire.h>
#include <Serial.h>
#include <Servo.h>

#define compassAddress 0x32 >> 1
#define adjustment 15 //Degress to adjust rudder
#define readDelay 2500 //Time between compass reads

Servo rudder; //Rudder servo object
int rudderAngle = 0; //current angle of the rudder servo
int rudderPosition = 0; //New position for rudder

unsigned int compassHeading = 0;
unsigned int initialHeading = 0;
unsigned int heading = 0;

int offset = 0;
int tempOffset = 0;
int headingShift = 0;

byte compassHeadingBytes[2]; //Heading read bytes from compass
int i = 0;

void setup() {
Serial.begin(115200); //Start the serial port for testing

Wire.begin(); //Start i2c

rudder.attach(9); //Rudder servo on pin 9

while( millis() < 500 ) { //Wait for the compass to initialise
delay(10);
}

rudderAngle = rudder.read(); //Get initial angle of rudder servo
readCompass(); //Get heading from compass

setPosition(); //translate into direction segment
setOffset(); //Set offset to correct heading
adjustPosition(); // Adjust heading to new position

initialHeading = heading; //Save heading as target direction
}

void loop() {

readCompass();
setPosition();
adjustPosition();
setServo();

delay(readDelay);

}

void readCompass() {
//Instruct compass to read echoes
Wire.beginTransmission(compassAddress); // transmit to device
// the address specified in the datasheet is 66 (0x42)
// but i2c adressing uses the high 7 bits so it's 33
Wire.write(0x50); // Send a "Post Heading Data" (0x50) command to the HMC6343
Wire.endTransmission(); // stop transmitting

//Wait for readings
delay(2); // datasheet suggests at least 1 ms

//Request heading reading from compass
Wire.requestFrom(compassAddress, 2); // request 2 bytes from slave device #33

//Receive heading reading from compass
if(2 <= Wire.available()) // if 2 bytes were received
{
for(int i = 0; i<2; i++) {
compassHeadingBytes = Wire.read();
}
}
compassHeading = ((int)compassHeadingBytes[0]<<8) | ((int)compassHeadingBytes[1]); // heading MSB and LSB
compassHeading = compassHeading * 0.1; //Translate heading to degress
}
// Translate Compass reading to directional segment
// LSB = N, MSB = NNE.
void setPosition(){
if ( compassHeading >= 349 || compassHeading <= 11 ) {
heading = 1;
}
else if ( compassHeading >= 12 && compassHeading <= 33 ) {
heading = 2;
}
else if ( compassHeading >= 34 && compassHeading <= 56 ) {
heading = 4;
}
else if ( compassHeading >= 57 && compassHeading <= 78 ) {
heading = 8;
}
else if ( compassHeading >= 79 && compassHeading <= 101 ) {
heading = 16;
}
else if ( compassHeading >= 102 && compassHeading <= 123 ) {
heading = 32;
}
else if ( compassHeading >= 124 && compassHeading <= 146 ) {
heading = 64;
}
else if ( compassHeading >= 147 && compassHeading <= 168 ) {
heading = 128;
}
else if ( compassHeading >= 169 && compassHeading <= 192 ) {
heading = 256;
}
else if ( compassHeading >= 193 && compassHeading <= 213 ) {
heading = 512;
}
else if ( compassHeading >= 214 && compassHeading <= 236 ) {
heading = 1024;
}
else if ( compassHeading >= 237 && compassHeading <= 258 ) {
heading = 2048;
}
else if ( compassHeading >= 259 && compassHeading <= 281 ) {
heading = 4096;
}
else if ( compassHeading >= 282 && compassHeading <= 303 ) {
heading = 8192;
}
else if ( compassHeading >= 304 && compassHeading <= 326 ) {
heading = 16384;
}
else if ( compassHeading >= 327 && compassHeading <= 348 ) {
heading = 32768;
}
}
//Ofset based on segment seting, so initial heading is 256.
void setOffset() {
switch ( heading ) {
case 1:
offset = 1;
break;
case 2:
offset = 2;
break;
case 4:
offset = 3;
break;
case 8:
offset = 4;
break;
case 16:
offset = 5;
break;
case 32:
offset = 6;
break;
case 64:
offset = 7;
break;
case 128:
offset = 8;
break;
case 256:
offset = 9;
break;
case 512:
offset = 10;
break;
case 1024:
offset = 11;
break;
case 2048:
offset = 12;
break;
case 4096:
offset = 13;
break;
case 8192:
offset = 14;
break;
case 16384:
offset = 15;
break;
case 32768:
offset = 16;
break;
}
offset = 8 - offset;
}
// Adjust heading by offset
void adjustPosition(){
unsigned int headingTemp = heading;
if ( offset >= 0 ) {
tempOffset = offset;
while ( tempOffset > 0 ) {
headingShift = headingTemp << 1;
headingTemp = headingShift;
if ( headingTemp == 0 ) {
headingTemp = 1;
}
tempOffset = tempOffset - 1;
}
}
else {
switch (offset) {
case -1:
tempOffset = 1;
break;
case -2:
tempOffset = 2;
break;
case -3:
tempOffset = 3;
break;
case -4:
tempOffset = 4;
break;
case -5:
tempOffset = 5;
break;
case -6:
tempOffset = 6;
break;
case -7:
tempOffset = 7;
break;
case -8:
tempOffset = 8;
break;
}
while ( tempOffset > 0 ) {
headingShift = headingTemp >> 1;
headingTemp = headingShift;
if ( headingTemp == 0 ) {
headingTemp = 32768;
}
tempOffset = tempOffset - 1;
}
}
heading = headingTemp;
}
void setServo() {
unsigned int i,j;
rudderPosition = rudderAngle;
i = heading;
if ( heading < 128 ) {
while ( i < 128){
rudderPosition = rudderPosition - adjustment;
j = i << 1;
i = j;
}
}
if ( heading > 128 ) {
while ( i > 128 ) {
rudderPosition = rudderPosition + adjustment;
j = i >> 1;
i = j;
}
}
rudder.write(rudderPosition);
}
kontiki 001.JPG

Are you trying to keep the bow pointing in the right direction, or control the direction of travel? (obviously with varying wind or sea conditions, these are different things).

Hi PeterH

i am trying to keep the bow aiming in a sedirectionon , when i turn on the unit the compass gets a heading or bearing and should try to hold it there using the servo tcompensatetmovementnt (wind swell),I have put the compass in the rear to keep it away from the motor,the motor iin-caseded in a aluminum body

Cheers

Paul

Is it drifting off-course because it's overshooting the target heading? For example, say it's pointing 90 deg (East) and the target heading is 0 deg (North). Does it turn anti-clockwise but go too far?

boatbandit2:
Hi PeterH

i am trying to keep the bow aiming in a sedirectionon , when i turn on the unit the compass gets a heading or bearing and should try to hold it there using the servo tcompensatetmovementnt (wind swell),I have put the compass in the rear to keep it away from the motor,the motor iin-caseded in a aluminum body

Cheers

Paul

The compass is all you should need to keep the bow pointing in the right direction - GPS gives you no indication of which way you're pointing.

GPS would only be required if you wanted to control which direction you were moving, to stay on a specified track.

Can you put [ code ] [ /code ] tags round your code please? Otherwise it gets mangled by the forum software because some bits of code are interpreted as forum display codes.

Hi Boatbandit,

I am working on west coast of North Island NZ and due to currents a compass only would not work! The tide would carry kontiki beyond 45 degrees even with compass which would just keep it on same bearing and not make good on any ground (sic) lost.

If you are in strong current area you will need gps also.

Nice looking kontiki, puts mine to shame.

Sailors build in a course correction to allow for tides. See for example "The Complete Yachtmaster" by Tom Cunliffe.

...R

yeah