diff --git a/ServoHolder.scad b/ServoHolder.scad
index d093e66..05c70e5 100644
--- a/ServoHolder.scad
+++ b/ServoHolder.scad
@@ -1,6 +1,6 @@
 $fn = 30; // number of faces in screw holes
 
-thickness = 5; // thickness
+thickness = 5; // thickness, this doesnt work
 
 // main body
 width = 70;
@@ -8,7 +8,7 @@ depth = 70;
 height = 32;
 
 // servo
-sWidth = 14;
+sWidth = 15;
 sDepth = 32;
 sHeight = 5;
 
@@ -20,7 +20,7 @@ sDiameter = 5; // screw diameter
 difference()
 {
     // main
-    cube([width, depth, 5]);    
+    cube([width, depth, thickness]);    
     
       union()
     {
@@ -38,22 +38,21 @@ difference()
         
         // screw holes
         
-        // NOTE: it might be worth getting rid of the 
-        // top lower holes, we dont need both them and 
-        // the side faces holes
-        
-        // lower left
-        translate([cWidth/2, cWidth/2+thickness, 0])
-        cylinder(h=thickness, d=sDiameter);
         // upper left
         translate([cWidth/2, width-cWidth/2, 0])
         cylinder(h=thickness, d=sDiameter);
-        // lower right
-        translate([depth-cWidth/2, cWidth/2+thickness, 0])
-        cylinder(h=thickness, d=sDiameter);
         // upper right
         translate([depth-cWidth/2, width-cWidth/2, 0])
         cylinder(h=thickness, d=sDiameter);
+        
+        /*
+        // lower right
+        translate([depth-cWidth/2, cWidth/2+thickness, 0])
+        cylinder(h=thickness, d=sDiameter);
+        // lower left
+        translate([cWidth/2, cWidth/2+thickness, 0])
+        cylinder(h=thickness, d=sDiameter);
+        */
     }
 }
 
diff --git a/example-control-system.ino b/example-control-system.ino
deleted file mode 100644
index d13b0e1..0000000
--- a/example-control-system.ino
+++ /dev/null
@@ -1,463 +0,0 @@
-/*
- This file is part of the AberSailbot minimum viable control system (AMVCS).
- AMVCS is free software: you can
- redistribute it and/or modify it under the terms of the GNU General Public
- License as published by the Free Software Foundation, version 2.
-
- This program is distributed in the hope that it will be useful, but WITHOUT
- ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
- FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
- details.
-
- You should have received a copy of the GNU General Public License along with
- this program; if not, write to the Free Software Foundation, Inc., 51
- Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
-
- Copyright Colin Sauze
-*/
-
-#include <stdio.h>
-#include <Servo.h>
-#include <Wire.h>
-#include <SoftwareSerial.h>
-#include <math.h>
-#include "Time.h"
-#include "TinyGPS.h"
-
-#define HMC6343_ADDRESS 0x19
-#define HMC6343_HEADING_REG 0x50
-#define GPS_ENABLE_PIN 12
-
-#define GPS_READ_INTERVAL 15 //how many seconds to leave between GPS reads
-#define WP_THRESHOLD 15 //how close (in metres) should we get before we change waypoint?
-
-#define rad2deg(x) (180/M_PI) * x
-#define deg2rad(x) x * M_PI/180
-
-Servo rudderServo; // create servo object to control a servo
-SoftwareSerial myDebug(7, 8);
-
-TinyGPS gps;
-
-#define HEADING 0
-#define WIND_DIR 1
-#define ROLL 2
-#define PITCH 3
-#define RUDDER 4
-#define SAIL 5
-#define LAT 6
-#define LON 7
-#define TIME 8
-
-#define DEBUG_CRITICAL 1 //really important messages that we don't want to ignore and are prepared to sacrifice execution speed to see
-#define DEBUG_IMPORTANT 2 //fairly important messages that we probably want to see, but might cause issues with execution speed
-#define DEBUG_MINOR 3 //less important messages that we can safely turn off to improve execution speed
-
-#define DEBUG_THRESHOLD DEBUG_IMPORTANT //set to 0 to show no debugging messages
-
-byte ledState=0;
-
-  struct Data{
-    uint16_t heading;
-    uint16_t wind_dir;
-    int8_t roll;
-    int8_t pitch;
-    int8_t rudder;
-    byte sail;
-    float lat;
-    float lon;
-    long unixtime;
-  } 
-  state;
-
-
-//make printf work
-static FILE uartout = {
-  0} 
-;
-
-static int uart_putchar (char c, FILE *stream)
-{
-  myDebug.write(c) ;
-  return 0 ;
-}
-
-static void say(byte level, char* msg)
-{
-  if(level<=DEBUG_THRESHOLD)
-  {
-    myDebug.print("Debug");
-    myDebug.print(level);
-    myDebug.print(": [Ctrl] ");
-    myDebug.println(msg);
-  }
-}
-
-//debugging printf that prepends "Debug:" to everything and can be easily turned off
-void dprintf(byte level, const char *fmt, ...)
-{      
-  
-  if(level<=DEBUG_THRESHOLD)
-  {
-    printf("Debug%d: [Ctrl] ",level);
-    va_list ap;
-    va_start(ap, fmt);
-    
-    vprintf(fmt, ap);
-    va_end(ap);
-  }
-}
-
-void dprintf2(const char *fmt, ...)
-{      
-  
-    //printf("Debug%d: [Ctrl] ",level);
-    va_list ap;
-    va_start(ap, fmt);
-    
-    printf(fmt, ap);
-    va_end(ap);
-}
-
-
-void setup() 
-{
-  //Serial.begin(9600); //baud rate makes no difference on 32u4
-
-  Serial.begin(4800); //for GPS
-
-  myDebug.begin(4800); //debug UART
-
-  say(DEBUG_CRITICAL,"Control system start up");
-
-  //required for printf  
-  fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
-  stdout = &uartout ;
-  dprintf(DEBUG_IMPORTANT,"Printf configured \r\n");
-
-  delay(5000);
-
-  say(DEBUG_IMPORTANT,"Setting up servos...");
-  //Use .attach for setting up connection to the servo
-  rudderServo.attach(5, 1060, 1920); // Attach, with the output limited
-  // between 1000 and 2000 ms
-  rudderServo.writeMicroseconds(1500);
-  say(DEBUG_IMPORTANT,"Done");
-
-  say(DEBUG_IMPORTANT,"Setting up I2C...");
-  Wire.begin(); // Initialise i2c for compass
-  say(DEBUG_IMPORTANT,"Done");
-
-  say(DEBUG_IMPORTANT,"Setting up GPS...");
-  pinMode(GPS_ENABLE_PIN, OUTPUT); //GPS on/off line
-  //setup GPS
-  digitalWrite(GPS_ENABLE_PIN,1);
-  delay(1000);
-  //turn off VTG
-  Serial.println("$PSRF103,05,00,00,01*21\r");
-  //turn off RMC
-  Serial.println("$PSRF103,04,00,00,01*20\r");
-
-  //turn off GSV
-  Serial.println("$PSRF103,03,00,00,01*27\r");
-
-  //turn off GSA
-  Serial.println("$PSRF103,02,00,00,01*26\r");
-
-  //turn off GLL
-  Serial.println("$PSRF103,01,00,00,01*25\r");
-
-  //turn off GGA
-  Serial.println("$PSRF103,00,00,00,01*24\r");
-  delay(1000);
-
-  //leave GPS on to get its initial fix
-  //digitalWrite(GPS_ENABLE_PIN,0);
-  say(DEBUG_IMPORTANT,"Done");
-
-  say(DEBUG_IMPORTANT,"Setup Complete\n");
-}
-
-//computes an NMEA checksum
-byte compute_checksum(byte *data,byte length)
-{                
-  byte computed_checksum=0;
-
-  for (byte i = 0; i < length; i++)
-  {
-    computed_checksum = (byte)computed_checksum ^ data[i];
-  }
-
-  return computed_checksum;
-}
-
-//reads heading from HMC6343 compass
-int readCompass() {
-  byte buf[6];
-
-  Wire.beginTransmission(HMC6343_ADDRESS); // Start communicating with the HMC6343 compasss
-  Wire.write(HMC6343_HEADING_REG); // Send the address of the register that we want to read
-  //Wire.write(0x55); // Send the address of the register that we want to read
-  Wire.endTransmission();
-
-  Wire.requestFrom(HMC6343_ADDRESS, 6); // Request six bytes of data from the HMC6343 compasss
-  for(int i=0;i<6;i++)
-  {
-    while(Wire.available() < 1); // Busy wait while there is no byte to receive
-    buf[i]=Wire.read();
-    //printf("buf[%d]=%d\r\n",i,buf[i]);
-  }
-  int heading = ((buf[0] << 8) + buf[1]); // the heading in degrees
-  int pitch =   ((buf[2] << 8) + buf[3]); // the pitch in degrees
-  int roll = ((buf[4] << 8) + buf[5]); // the roll in degrees*/
-  
-   heading=heading/10;
-   roll=roll/10;
-   pitch=pitch/10;
-   
-   //myDebug.print("Heading = ");
-   //myDebug.print(heading);
-
-  state.roll=(int8_t)roll;
-  state.pitch=(int8_t)pitch;
-  state.heading=(uint16_t)heading;
-  
-  //dprintf(DEBUG_IMPORTANT,"Heading: %d Roll: %d Pitch: %d\r\n",state.heading,state.roll,state.pitch);
-  //printf("heading=%d\r\n",heading);
-  //dprintf2("heading=%d\r\n",heading);
-
-  delay(100);
-  
-  return (int)heading; // Print the sensor readings to the serial port.
-}
-
-void readGPS() {
-  unsigned long fix_age=9999,time,date;
-
-  say(DEBUG_MINOR,"About to read GPS");
-  digitalWrite(GPS_ENABLE_PIN,1); //turn the GPS on
-  delay(1000);
-
-  while(fix_age == TinyGPS::GPS_INVALID_AGE||fix_age>3000) //make sure the GPS has a fix, this might cause a wait the first time, but it should be quick any subsequent time
-  {
-    Serial.println("$PSRF103,04,01,00,01*21\r");
-    dprintf(DEBUG_MINOR,"NMEA string: ");
-    unsigned long start = millis();
-    while(millis()<start+2000)
-    {
-      if(Serial.available())
-      {
-        int c = Serial.read();
-        gps.encode(c);
-        if(DEBUG_THRESHOLD>=DEBUG_MINOR)
-        {
-          myDebug.write(c);
-        }
-        if(c=='\n')
-        {
-          break;
-        }
-      }
-    }
-    gps.get_datetime(&date,&time,&fix_age);
-
-    dprintf(DEBUG_MINOR,"fix age = %ld\r\n",fix_age);
-    if(fix_age == TinyGPS::GPS_INVALID_AGE)
-    {
-      dprintf(DEBUG_IMPORTANT,"Invalid fix, fix_age=%ld\r\n",fix_age);
-      say(DEBUG_IMPORTANT,"No GPS fix");
-    }
-  }
-
-  digitalWrite(GPS_ENABLE_PIN,0); //turn the GPS off
-   
-
-  gps.get_datetime(&date,&time,&fix_age);
-  gps.f_get_position(&state.lat,&state.lon,&fix_age);
-   
-
-  if(fix_age == TinyGPS::GPS_INVALID_AGE)
-  {
-    say(DEBUG_IMPORTANT,"Invalid fix");
-  }
-
-  else
-  {
-    
-    say(DEBUG_IMPORTANT,"Fix Valid");
-    dprintf(DEBUG_IMPORTANT,"lat=%ld lon=%ld\r\n",(long)(state.lat*1000),(long)(state.lon*1000));
-
-    int year;
-    byte month,day,hour,min,sec;
-    unsigned long age;
-      
-    gps.crack_datetime(&year,&month,&day,&hour,&min,&sec,NULL,&age);
-    
-    setTime(hour,min,sec,day,month,year); //sets the time in the time library, lets us get unix time
-
-  }
-
-}
-
-int mod(int value){ //keeps angles betweeen 0 and 360
-  int newValue;
-  if(value < 0){
-    newValue = value + 360;
-  }
-  else if(value >= 360){
-    newValue = value - 360;
-  }
-  else{
-    newValue = value;
-  }
-  return newValue;
-}
-
-//calculates difference between two headings taking wrap around into account
-int get_hdg_diff(int heading1,int heading2)
-{
-  int result;
-
-  result = heading1-heading2;
-
-  if(result<-180)
-  {
-    result = 360 + result;
-    return result;
-  } 
-
-  if(result>180)
-  {
-    result = 0 - (360-result);
-  }
-
-  return result;
-}
-
-void loop()
-{
-  unsigned long last_gps_read=0;
-  unsigned long last_time=0,time_now=0;
-  int wp_hdg=0;
-  float wp_dist=0.0;
-  int wp_num=0;
-
-  float igain=0.01;
-  float pgain=0.1;
-  float running_err=0.0;
-  int hdg_err=0;
-  int relwind;
-  
-  long last_telemetry=0;
-  #define TELEMETRY_INTERVAL 10
-  #define TARGET_LOOP_INTERVAL 100 //number of milliseconds between loop intervals
-
-  #define NUM_OF_WAYPOINTS 1
-
-
-  float wp_lats[NUM_OF_WAYPOINTS]; 
-  float wp_lons[NUM_OF_WAYPOINTS];
-
-  wp_lats[0]=52.4;
-  wp_lons[0]=-4.4;
-
-
-  while(1)
-  {
-    
-    //make loop execute at constant speed
-    time_now=millis();
-
-    if(time_now-last_time>0&&time_now-last_time<TARGET_LOOP_INTERVAL)
-    {
-      delay(TARGET_LOOP_INTERVAL-(time_now-last_time));
-    }
-
-    last_time=millis();
-
-    readCompass();
-
-    //state.wind_dir=getTrueWind();
-    //no wind sensor, so just use a fixed wind direction
-    state.wind_dir=270;
-
-    relwind=mod(state.wind_dir - state.heading);
-
-    if(millis()-last_gps_read>(GPS_READ_INTERVAL*1000)||millis()<last_gps_read) //read the GPS at the specified interval or whenever the millis count wraps around 
-    {
-      say(DEBUG_MINOR,"Reading GPS");
-      readGPS();
-      wp_hdg = (int) TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
-      wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
-      if(wp_dist<WP_THRESHOLD)
-      {       
-        wp_num++;      
-        if(wp_num==NUM_OF_WAYPOINTS) //reached last waypoint already
-        {
-          wp_num--;          
-        }
-        else //reached new waypoint
-        {
-          wp_hdg = (int) TinyGPS::course_to(state.lat, state.lon,wp_lats[wp_num],wp_lons[wp_num]);
-          wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
-        }
-      }
-      last_gps_read=millis();
-    }
-
-    //sail logic
-    //sailLogic(relwind);
-
-    //rudder logic
-    hdg_err = get_hdg_diff(wp_hdg,state.heading);
-
-    running_err = running_err + (float)hdg_err;
-    if (abs(running_err > 4000))
-    {
-      running_err = 4000; // limit integral component
-    }
-    running_err = running_err * 0.9;
-
-    /*dprintf("hdg_err = %d running_err = ",hdg_err);
-    myDebug.println(running_err);*/
-
-    state.rudder = (int) round((pgain * (float)hdg_err) + (igain * running_err));
-    if(state.rudder<-5)
-    {
-      state.rudder=-5;
-    }
-    else if(state.rudder>5)
-    {
-      state.rudder=5;
-    }
-
-    rudderServo.writeMicroseconds(1500+(state.rudder*100));
-
-    if(last_telemetry+(TELEMETRY_INTERVAL*1000)<millis())
-    {
-      dprintf(DEBUG_CRITICAL,"time=%ld hdg=%d hdg_err=%d roll=%d pitch=%d truewind=%d relwind=%d sail=%d rudder=%d wp_num=%d wp_hdg=%d wp_dist=%ld ",now(),state.heading,hdg_err,state.roll,state.pitch,state.wind_dir,relwind,state.sail,state.rudder,wp_num,wp_hdg,(long)wp_dist);
-      
-      //time=181734082 hdg=-14836 hdg_err=9217 roll=-22526 pitch=24182 truewind=-25261 relwind=27648 sail=-6656 rudder=-7937 wp_hdg=2768 wp_dist=-284423467 lat=52.41648 lon=-4.06522 wplat=52.40000 wplon=-4.40000 running_err=966.57
-      
-      // time=1398700112 hdg=158 hdg_err=107 roll=-26 pitch=-32 truewind=302 relwind=144 sail=4 rudder=5 Debug1: [Ctrl] wp_hdg=265 wp_dist=22842 lat=52.41666 lon=-4.06445 wplat=52.40000 wplon=-4.40000 running_err=963.96
-
-      if(DEBUG_THRESHOLD>=DEBUG_CRITICAL)
-      {
-        myDebug.print("lat=");
-        myDebug.print(state.lat,5);
-        myDebug.print(" lon=");
-        myDebug.print(state.lon,5);
-        myDebug.print(" wplat=");
-        myDebug.print(wp_lats[wp_num],5);
-        myDebug.print(" wplon=");
-        myDebug.print(wp_lons[wp_num],5);
-        myDebug.print(" running_err=");
-        myDebug.println(running_err);
-      }
-
-      //transmit_data();
-      last_telemetry=millis();
-    }
-
-  }
-}
diff --git a/rudder.scad b/rudder.scad
new file mode 100644
index 0000000..7e360bf
--- /dev/null
+++ b/rudder.scad
@@ -0,0 +1,2 @@
+linear_extrude(height = 2)
+import("rudderProfile.svg");
\ No newline at end of file
diff --git a/rudderProfile.svg b/rudderProfile.svg
new file mode 100644
index 0000000..778e05d
--- /dev/null
+++ b/rudderProfile.svg
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+   width="106mm"
+   height="20.5mm"
+   viewBox="0 0 106 20.5"
+   version="1.1"
+   id="svg131"
+   sodipodi:docname="rudderProfile.svg"
+   inkscape:version="1.1.2 (0a00cf5339, 2022-02-04)"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:svg="http://www.w3.org/2000/svg">
+  <defs
+     id="defs135" />
+  <sodipodi:namedview
+     id="namedview133"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageshadow="2"
+     inkscape:pageopacity="0.0"
+     inkscape:pagecheckerboard="0"
+     inkscape:document-units="mm"
+     showgrid="false"
+     inkscape:zoom="2.2963934"
+     inkscape:cx="302.21303"
+     inkscape:cy="-58.570104"
+     inkscape:window-width="2182"
+     inkscape:window-height="1440"
+     inkscape:window-x="0"
+     inkscape:window-y="0"
+     inkscape:window-maximized="1"
+     inkscape:current-layer="g121" />
+  <title
+     id="title89">Airfoil plot (user-000) NACA 0010 Airfoil M=0.0% P=0.0% T=10.0% by AirfoilTools.com</title>
+  <g
+     transform="translate(3,8)"
+     id="g121">
+    <path
+       fill="none"
+       stroke="#ff0000"
+       stroke-width="0.1"
+       d="M 100,-0.11 99.85,-0.12 99.38,-0.18 98.62,-0.27 97.55,-0.39 96.19,-0.54 94.55,-0.72 92.63,-0.93 90.45,-1.16 88.02,-1.41 85.36,-1.68 82.47,-1.95 79.39,-2.24 76.12,-2.54 72.7,-2.83 69.13,-3.12 65.45,-3.41 61.67,-3.69 57.82,-3.95 53.92,-4.19 50,-4.41 46.08,-4.6 42.18,-4.76 38.33,-4.88 34.55,-4.96 30.87,-5 l -3.57,0.01 -3.42,0.07 -3.27,0.11 -3.08,0.17 -2.89,0.22 -2.66,0.27 L 9.55,-3.84 7.37,-3.48 5.45,-3.07 3.81,-2.63 2.45,-2.16 1.38,-1.65 0.62,-1.13 0.15,-0.57 0,0 l 0.15,0.57 0.47,0.56 0.76,0.52 1.07,0.51 1.36,0.47 1.64,0.44 1.92,0.41 2.18,0.36 2.43,0.31 2.66,0.27 2.89,0.22 3.08,0.17 3.27,0.11 L 27.3,4.99 30.87,5 34.55,4.96 38.33,4.88 42.18,4.76 46.08,4.6 50,4.41 53.92,4.19 57.82,3.95 61.67,3.69 65.45,3.41 69.13,3.12 72.7,2.83 76.12,2.54 79.39,2.24 82.47,1.95 85.36,1.68 88.02,1.41 90.45,1.16 92.63,0.93 94.55,0.72 96.19,0.54 97.55,0.39 98.62,0.27 99.38,0.18 99.85,0.12 100,0.11"
+       id="path111" />
+  </g>
+</svg>
diff --git a/sppControlSystem.ino b/sppControlSystem.ino
index 30da59c..de8d215 100644
--- a/sppControlSystem.ino
+++ b/sppControlSystem.ino
@@ -28,13 +28,13 @@
 #define HMC6343_HEADING_REG 0x50
 #define GPS_ENABLE_PIN 12
 
-#define GPS_READ_INTERVAL 15 //how many seconds to leave between GPS reads
-#define WP_THRESHOLD 15 //how close (in metres) should we get before we change waypoint?
+#define GPS_READ_INTERVAL 15  //how many seconds to leave between GPS reads
+#define WP_THRESHOLD 15       //how close (in metres) should we get before we change waypoint?
 
-#define rad2deg(x) (180/M_PI) * x
-#define deg2rad(x) x * M_PI/180
+#define rad2deg(x) (180 / M_PI) * x
+#define deg2rad(x) x *M_PI / 180
 
-Servo rudderServo; // create servo object to control a servo
+Servo rudderServo;  // create servo object to control a servo
 SoftwareSerial myDebug(7, 8);
 
 TinyGPS gps;
@@ -49,43 +49,39 @@ TinyGPS gps;
 #define LON 7
 #define TIME 8
 
-#define DEBUG_CRITICAL 1 //really important messages that we don't want to ignore and are prepared to sacrifice execution speed to see
-#define DEBUG_IMPORTANT 2 //fairly important messages that we probably want to see, but might cause issues with execution speed
-#define DEBUG_MINOR 3 //less important messages that we can safely turn off to improve execution speed
+#define DEBUG_CRITICAL 1   //really important messages that we don't want to ignore and are prepared to sacrifice execution speed to see
+#define DEBUG_IMPORTANT 2  //fairly important messages that we probably want to see, but might cause issues with execution speed
+#define DEBUG_MINOR 3      //less important messages that we can safely turn off to improve execution speed
 
-#define DEBUG_THRESHOLD DEBUG_IMPORTANT //set to 0 to show no debugging messages
+#define DEBUG_THRESHOLD DEBUG_IMPORTANT  //set to 0 to show no debugging messages
 
-byte ledState=0;
+byte ledState = 0;
 
-  struct Data{
-    uint16_t heading;
-    uint16_t wind_dir;
-    int8_t roll;
-    int8_t pitch;
-    int8_t rudder;
-    byte sail;
-    float lat;
-    float lon;
-    long unixtime;
-  } 
-  state;
+struct Data {
+  uint16_t heading;
+  uint16_t wind_dir;
+  int8_t roll;
+  int8_t pitch;
+  int8_t rudder;
+  byte sail;
+  float lat;
+  float lon;
+  long unixtime;
+} state;
 
 
 //make printf work
 static FILE uartout = {
-  0} 
-;
+  0
+};
 
-static int uart_putchar (char c, FILE *stream)
-{
-  myDebug.write(c) ;
-  return 0 ;
+static int uart_putchar(char c, FILE *stream) {
+  myDebug.write(c);
+  return 0;
 }
 
-static void say(byte level, char* msg)
-{
-  if(level<=DEBUG_THRESHOLD)
-  {
+static void say(byte level, char *msg) {
+  if (level <= DEBUG_THRESHOLD) {
     myDebug.print("Debug");
     myDebug.print(level);
     myDebug.print(": [Ctrl] ");
@@ -94,66 +90,62 @@ static void say(byte level, char* msg)
 }
 
 //debugging printf that prepends "Debug:" to everything and can be easily turned off
-void dprintf(byte level, const char *fmt, ...)
-{      
-  
-  if(level<=DEBUG_THRESHOLD)
-  {
-    printf("Debug%d: [Ctrl] ",level);
+void dprintf(byte level, const char *fmt, ...) {
+
+  if (level <= DEBUG_THRESHOLD) {
+    printf("Debug%d: [Ctrl] ", level);
     va_list ap;
     va_start(ap, fmt);
-    
+
     vprintf(fmt, ap);
     va_end(ap);
   }
 }
 
-void dprintf2(const char *fmt, ...)
-{      
-  
-    //printf("Debug%d: [Ctrl] ",level);
-    va_list ap;
-    va_start(ap, fmt);
-    
-    printf(fmt, ap);
-    va_end(ap);
+void dprintf2(const char *fmt, ...) {
+
+  //printf("Debug%d: [Ctrl] ",level);
+  va_list ap;
+  va_start(ap, fmt);
+
+  printf(fmt, ap);
+  va_end(ap);
 }
 
 
-void setup() 
-{
+void setup() {
   //Serial.begin(9600); //baud rate makes no difference on 32u4
 
   //for GPS, we communicate with the GPS through the Serial Port
   //so, using Serial.println() will actually send messages to the GPS
   Serial.begin(4800);
 
-  myDebug.begin(4800); //debug UART
+  myDebug.begin(4800);  //debug UART
 
-  say(DEBUG_CRITICAL,"Control system start up");
+  say(DEBUG_CRITICAL, "Control system start up");
 
-  //required for printf  
-  fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
-  stdout = &uartout ;
-  dprintf(DEBUG_IMPORTANT,"Printf configured \r\n");
+  //required for printf
+  fdev_setup_stream(&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
+  stdout = &uartout;
+  dprintf(DEBUG_IMPORTANT, "Printf configured \r\n");
 
   delay(5000);
 
-  say(DEBUG_IMPORTANT,"Setting up servos...");
+  say(DEBUG_IMPORTANT, "Setting up servos...");
   //Use .attach for setting up connection to the servo
-  rudderServo.attach(5, 1060, 1920); // Attach, with the output limited
+  rudderServo.attach(5, 1060, 1920);  // Attach, with the output limited
   // between 1000 and 2000 ms
   rudderServo.writeMicroseconds(1500);
-  say(DEBUG_IMPORTANT,"Done");
+  say(DEBUG_IMPORTANT, "Done");
 
-  say(DEBUG_IMPORTANT,"Setting up I2C...");
-  Wire.begin(); // Initialise i2c for compass
-  say(DEBUG_IMPORTANT,"Done");
+  say(DEBUG_IMPORTANT, "Setting up I2C...");
+  Wire.begin();  // Initialise i2c for compass
+  say(DEBUG_IMPORTANT, "Done");
 
-  say(DEBUG_IMPORTANT,"Setting up GPS...");
-  pinMode(GPS_ENABLE_PIN, OUTPUT); //GPS on/off line
+  say(DEBUG_IMPORTANT, "Setting up GPS...");
+  pinMode(GPS_ENABLE_PIN, OUTPUT);  //GPS on/off line
   //setup GPS
-  digitalWrite(GPS_ENABLE_PIN,1);
+  digitalWrite(GPS_ENABLE_PIN, 1);
   delay(1000);
   //turn off VTG
   Serial.println("$PSRF103,05,00,00,01*21\r");
@@ -175,18 +167,16 @@ void setup()
 
   //leave GPS on to get its initial fix
   //digitalWrite(GPS_ENABLE_PIN,0);
-  say(DEBUG_IMPORTANT,"Done");
+  say(DEBUG_IMPORTANT, "Done");
 
-  say(DEBUG_IMPORTANT,"Setup Complete\n");
+  say(DEBUG_IMPORTANT, "Setup Complete\n");
 }
 
 //computes an NMEA checksum
-byte compute_checksum(byte *data,byte length)
-{                
-  byte computed_checksum=0;
+byte compute_checksum(byte *data, byte length) {
+  byte computed_checksum = 0;
 
-  for (byte i = 0; i < length; i++)
-  {
+  for (byte i = 0; i < length; i++) {
     computed_checksum = (byte)computed_checksum ^ data[i];
   }
 
@@ -197,185 +187,168 @@ byte compute_checksum(byte *data,byte length)
 int readCompass() {
   byte buf[6];
 
-  Wire.beginTransmission(HMC6343_ADDRESS); // Start communicating with the HMC6343 compasss
-  Wire.write(HMC6343_HEADING_REG); // Send the address of the register that we want to read
+  Wire.beginTransmission(HMC6343_ADDRESS);  // Start communicating with the HMC6343 compasss
+  Wire.write(HMC6343_HEADING_REG);          // Send the address of the register that we want to read
   //Wire.write(0x55); // Send the address of the register that we want to read
   Wire.endTransmission();
 
-  Wire.requestFrom(HMC6343_ADDRESS, 6); // Request six bytes of data from the HMC6343 compasss
-  for(int i=0;i<6;i++)
-  {
-    while(Wire.available() < 1); // Busy wait while there is no byte to receive
-    buf[i]=Wire.read();
+  Wire.requestFrom(HMC6343_ADDRESS, 6);  // Request six bytes of data from the HMC6343 compasss
+  for (int i = 0; i < 6; i++) {
+    while (Wire.available() < 1)
+      ;  // Busy wait while there is no byte to receive
+    buf[i] = Wire.read();
     //printf("buf[%d]=%d\r\n",i,buf[i]);
   }
-  int heading = ((buf[0] << 8) + buf[1]); // the heading in degrees
-  int pitch =   ((buf[2] << 8) + buf[3]); // the pitch in degrees
-  int roll = ((buf[4] << 8) + buf[5]); // the roll in degrees*/
-  
-   heading=heading/10;
-   roll=roll/10;
-   pitch=pitch/10;
-   
-   //myDebug.print("Heading = ");
-   //myDebug.print(heading);
+  int heading = ((buf[0] << 8) + buf[1]);  // the heading in degrees
+  int pitch = ((buf[2] << 8) + buf[3]);    // the pitch in degrees
+  int roll = ((buf[4] << 8) + buf[5]);     // the roll in degrees*/
+
+  heading = heading / 10;
+  roll = roll / 10;
+  pitch = pitch / 10;
+
+  //myDebug.print("Heading = ");
+  //myDebug.print(heading);
+
+  state.roll = (int8_t)roll;
+  state.pitch = (int8_t)pitch;
+  state.heading = (uint16_t)heading;
 
-  state.roll=(int8_t)roll;
-  state.pitch=(int8_t)pitch;
-  state.heading=(uint16_t)heading;
-  
   //dprintf(DEBUG_IMPORTANT,"Heading: %d Roll: %d Pitch: %d\r\n",state.heading,state.roll,state.pitch);
   //printf("heading=%d\r\n",heading);
   //dprintf2("heading=%d\r\n",heading);
 
   delay(100);
-  
-  return (int)heading; // Print the sensor readings to the serial port.
+
+  return (int)heading;  // Print the sensor readings to the serial port.
 }
 
 void readGPS() {
-  unsigned long fix_age=9999,time,date;
+  unsigned long fix_age = 9999, time, date;
 
-  say(DEBUG_MINOR,"About to read GPS");
-  digitalWrite(GPS_ENABLE_PIN,1); //turn the GPS on
+  say(DEBUG_MINOR, "About to read GPS");
+  digitalWrite(GPS_ENABLE_PIN, 1);  //turn the GPS on
   delay(1000);
 
-  while(fix_age == TinyGPS::GPS_INVALID_AGE||fix_age>3000) //make sure the GPS has a fix, this might cause a wait the first time, but it should be quick any subsequent time
+  while (fix_age == TinyGPS::GPS_INVALID_AGE || fix_age > 3000)  //make sure the GPS has a fix, this might cause a wait the first time, but it should be quick any subsequent time
   {
     Serial.println("$PSRF103,04,01,00,01*21\r");
-    dprintf(DEBUG_MINOR,"NMEA string: ");
+    dprintf(DEBUG_MINOR, "NMEA string: ");
     unsigned long start = millis();
-    while(millis()<start+2000)
-    {
-      if(Serial.available())
-      {
+    while (millis() < start + 2000) {
+      if (Serial.available()) {
         int c = Serial.read();
         gps.encode(c);
-        if(DEBUG_THRESHOLD>=DEBUG_MINOR)
-        {
+        if (DEBUG_THRESHOLD >= DEBUG_MINOR) {
           myDebug.write(c);
         }
-        if(c=='\n')
-        {
+        if (c == '\n') {
           break;
         }
       }
     }
-    gps.get_datetime(&date,&time,&fix_age);
+    gps.get_datetime(&date, &time, &fix_age);
 
-    dprintf(DEBUG_MINOR,"fix age = %ld\r\n",fix_age);
-    if(fix_age == TinyGPS::GPS_INVALID_AGE)
-    {
-      dprintf(DEBUG_IMPORTANT,"Invalid fix, fix_age=%ld\r\n",fix_age);
-      say(DEBUG_IMPORTANT,"No GPS fix");
+    dprintf(DEBUG_MINOR, "fix age = %ld\r\n", fix_age);
+    if (fix_age == TinyGPS::GPS_INVALID_AGE) {
+      dprintf(DEBUG_IMPORTANT, "Invalid fix, fix_age=%ld\r\n", fix_age);
+      say(DEBUG_IMPORTANT, "No GPS fix");
     }
   }
 
-  digitalWrite(GPS_ENABLE_PIN,0); //turn the GPS off
-   
+  digitalWrite(GPS_ENABLE_PIN, 0);  //turn the GPS off
 
-  gps.get_datetime(&date,&time,&fix_age);
-  gps.f_get_position(&state.lat,&state.lon,&fix_age);
-   
 
-  if(fix_age == TinyGPS::GPS_INVALID_AGE)
-  {
-    say(DEBUG_IMPORTANT,"Invalid fix");
+  gps.get_datetime(&date, &time, &fix_age);
+  gps.f_get_position(&state.lat, &state.lon, &fix_age);
+
+
+  if (fix_age == TinyGPS::GPS_INVALID_AGE) {
+    say(DEBUG_IMPORTANT, "Invalid fix");
   }
 
-  else
-  {
-    
-    say(DEBUG_IMPORTANT,"Fix Valid");
-    dprintf(DEBUG_IMPORTANT,"lat=%ld lon=%ld\r\n",(long)(state.lat*1000),(long)(state.lon*1000));
+  else {
+
+    say(DEBUG_IMPORTANT, "Fix Valid");
+    dprintf(DEBUG_IMPORTANT, "lat=%ld lon=%ld\r\n", (long)(state.lat * 1000), (long)(state.lon * 1000));
 
     int year;
-    byte month,day,hour,min,sec;
+    byte month, day, hour, min, sec;
     unsigned long age;
-      
-    gps.crack_datetime(&year,&month,&day,&hour,&min,&sec,NULL,&age);
-    
-    setTime(hour,min,sec,day,month,year); //sets the time in the time library, lets us get unix time
 
+    gps.crack_datetime(&year, &month, &day, &hour, &min, &sec, NULL, &age);
+
+    setTime(hour, min, sec, day, month, year);  //sets the time in the time library, lets us get unix time
   }
-
 }
 
-int mod(int value){ //keeps angles betweeen 0 and 360
+int mod(int value) {  //keeps angles betweeen 0 and 360
   int newValue;
-  if(value < 0){
+  if (value < 0) {
     newValue = value + 360;
-  }
-  else if(value >= 360){
+  } else if (value >= 360) {
     newValue = value - 360;
-  }
-  else{
+  } else {
     newValue = value;
   }
   return newValue;
 }
 
 //calculates difference between two headings taking wrap around into account
-int get_hdg_diff(int heading1,int heading2)
-{
+int get_hdg_diff(int heading1, int heading2) {
   int result;
 
-  result = heading1-heading2;
+  result = heading1 - heading2;
 
-  if(result<-180)
-  {
+  if (result < -180) {
     result = 360 + result;
     return result;
-  } 
+  }
 
-  if(result>180)
-  {
-    result = 0 - (360-result);
+  if (result > 180) {
+    result = 0 - (360 - result);
   }
 
   return result;
 }
 
-void loop()
-{
-  unsigned long last_gps_read=0;
-  unsigned long last_time=0,time_now=0;
-  int wp_hdg=0; // heading to the next waypoint, in degrees
-  float wp_dist=0.0; // distance to next waypoint
-  int wp_num=0; // number of waypoints
+void loop() {
+  unsigned long last_gps_read = 0;
+  unsigned long last_time = 0, time_now = 0;
+  int wp_hdg = 0;       // heading to the next waypoint, in degrees
+  float wp_dist = 0.0;  // distance to next waypoint
+  int wp_num = 0;       // number of waypoints
 
-  float igain=0.01; // i and p gain are multipliers for the rudder servo, they fix issues
-  float pgain=0.1;
-  float running_err=0.0;
-  int hdg_err=0;
-  int relwind; // wind direction relative to us
-  
-  long last_telemetry=0;
-  #define TELEMETRY_INTERVAL 10
-  #define TARGET_LOOP_INTERVAL 100 //number of milliseconds between loop intervals
+  float igain = 0.01;  // i and p gain are multipliers for the rudder servo, they fix issues
+  float pgain = 0.1;
+  float running_err = 0.0;
+  int hdg_err = 0;
+  int relwind;  // wind direction relative to us
 
-  #define NUM_OF_WAYPOINTS 1
+  long last_telemetry = 0;
+#define TELEMETRY_INTERVAL 10
+#define TARGET_LOOP_INTERVAL 100  //number of milliseconds between loop intervals
+
+#define NUM_OF_WAYPOINTS 1
 
 
-  float wp_lats[NUM_OF_WAYPOINTS]; 
+  float wp_lats[NUM_OF_WAYPOINTS];
   float wp_lons[NUM_OF_WAYPOINTS];
 
-  wp_lats[0]=52.4;
-  wp_lons[0]=-4.4;
+  wp_lats[0] = 52.4;
+  wp_lons[0] = -4.4;
 
 
-  while(1)
-  {
-    
+  while (1) {
+
     //make loop execute at constant speed
-    time_now=millis();
+    time_now = millis();
 
-    if(time_now-last_time>0&&time_now-last_time<TARGET_LOOP_INTERVAL)
-    {
-      delay(TARGET_LOOP_INTERVAL-(time_now-last_time));
+    if (time_now - last_time > 0 && time_now - last_time < TARGET_LOOP_INTERVAL) {
+      delay(TARGET_LOOP_INTERVAL - (time_now - last_time));
     }
 
-    last_time=millis();
+    last_time = millis();
 
     //get external values
     /*
@@ -387,30 +360,28 @@ void loop()
     readCompass();
     //state.wind_dir=getTrueWind();
     //no wind sensor, so just use a fixed wind direction
-    state.wind_dir=270;
+    state.wind_dir = 270;
 
-    relwind=mod(state.wind_dir - state.heading);
+    relwind = mod(state.wind_dir - state.heading);
 
-    if(millis()-last_gps_read>(GPS_READ_INTERVAL*1000)||millis()<last_gps_read) //read the GPS at the specified interval or whenever the millis count wraps around 
+    if (millis() - last_gps_read > (GPS_READ_INTERVAL * 1000) || millis() < last_gps_read)  //read the GPS at the specified interval or whenever the millis count wraps around
     {
-      say(DEBUG_MINOR,"Reading GPS");
+      say(DEBUG_MINOR, "Reading GPS");
       readGPS();
-      wp_hdg = (int) TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
-      wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
-      if(wp_dist<WP_THRESHOLD)
-      {       
-        wp_num++;      
-        if(wp_num==NUM_OF_WAYPOINTS) //reached last waypoint already
+      wp_hdg = (int)TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]);
+      wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]);
+      if (wp_dist < WP_THRESHOLD) {
+        wp_num++;
+        if (wp_num == NUM_OF_WAYPOINTS)  //reached last waypoint already
         {
-          wp_num--;          
-        }
-        else //reached new waypoint
+          wp_num--;
+        } else  //reached new waypoint
         {
-          wp_hdg = (int) TinyGPS::course_to(state.lat, state.lon,wp_lats[wp_num],wp_lons[wp_num]);
-          wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
+          wp_hdg = (int)TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]);
+          wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]);
         }
       }
-      last_gps_read=millis();
+      last_gps_read = millis();
     }
 
     //sail logic
@@ -424,29 +395,25 @@ void loop()
     * then we use the heading and the error to set what position the rudder should be in, clamp it then also store that value in state for reference later (state holds all the data the arduino needs, see its definition at like 60~)
     */
 
-    hdg_err = get_hdg_diff(wp_hdg,state.heading);
+    hdg_err = get_hdg_diff(wp_hdg, state.heading);
 
     running_err = running_err + (float)hdg_err;
-    if (abs(running_err > 4000))
-    {
-      running_err = 4000; // limit integral component
+    if (abs(running_err > 4000)) {
+      running_err = 4000;  // limit integral component
     }
     running_err = running_err * 0.9;
 
     /*dprintf("hdg_err = %d running_err = ",hdg_err);
     myDebug.println(running_err);*/
 
-    state.rudder = (int) round((pgain * (float)hdg_err) + (igain * running_err));
-    if(state.rudder<-5)
-    {
-      state.rudder=-5;
-    }
-    else if(state.rudder>5)
-    {
-      state.rudder=5;
+    state.rudder = (int)round((pgain * (float)hdg_err) + (igain * running_err));
+    if (state.rudder < -5) {
+      state.rudder = -5;
+    } else if (state.rudder > 5) {
+      state.rudder = 5;
     }
 
-    rudderServo.writeMicroseconds(1500+(state.rudder*100));
+    rudderServo.writeMicroseconds(1500 + (state.rudder * 100));
 
     // print debug info
     /*
@@ -454,31 +421,28 @@ void loop()
     * we mostly print info about where we are and where the waypoint is
     */
 
-    if(last_telemetry+(TELEMETRY_INTERVAL*1000)<millis())
-    {
-      dprintf(DEBUG_CRITICAL,"time=%ld hdg=%d hdg_err=%d roll=%d pitch=%d truewind=%d relwind=%d sail=%d rudder=%d wp_num=%d wp_hdg=%d wp_dist=%ld ",now(),state.heading,hdg_err,state.roll,state.pitch,state.wind_dir,relwind,state.sail,state.rudder,wp_num,wp_hdg,(long)wp_dist);
-      
+    if (last_telemetry + (TELEMETRY_INTERVAL * 1000) < millis()) {
+      dprintf(DEBUG_CRITICAL, "time=%ld hdg=%d hdg_err=%d roll=%d pitch=%d truewind=%d relwind=%d sail=%d rudder=%d wp_num=%d wp_hdg=%d wp_dist=%ld ", now(), state.heading, hdg_err, state.roll, state.pitch, state.wind_dir, relwind, state.sail, state.rudder, wp_num, wp_hdg, (long)wp_dist);
+
       //time=181734082 hdg=-14836 hdg_err=9217 roll=-22526 pitch=24182 truewind=-25261 relwind=27648 sail=-6656 rudder=-7937 wp_hdg=2768 wp_dist=-284423467 lat=52.41648 lon=-4.06522 wplat=52.40000 wplon=-4.40000 running_err=966.57
-      
+
       // time=1398700112 hdg=158 hdg_err=107 roll=-26 pitch=-32 truewind=302 relwind=144 sail=4 rudder=5 Debug1: [Ctrl] wp_hdg=265 wp_dist=22842 lat=52.41666 lon=-4.06445 wplat=52.40000 wplon=-4.40000 running_err=963.96
 
-      if(DEBUG_THRESHOLD>=DEBUG_CRITICAL)
-      {
+      if (DEBUG_THRESHOLD >= DEBUG_CRITICAL) {
         myDebug.print("lat=");
-        myDebug.print(state.lat,5);
+        myDebug.print(state.lat, 5);
         myDebug.print(" lon=");
-        myDebug.print(state.lon,5);
+        myDebug.print(state.lon, 5);
         myDebug.print(" wplat=");
-        myDebug.print(wp_lats[wp_num],5);
+        myDebug.print(wp_lats[wp_num], 5);
         myDebug.print(" wplon=");
-        myDebug.print(wp_lons[wp_num],5);
+        myDebug.print(wp_lons[wp_num], 5);
         myDebug.print(" running_err=");
         myDebug.println(running_err);
       }
 
       //transmit_data();
-      last_telemetry=millis();
+      last_telemetry = millis();
     }
-
   }
-}
+}
\ No newline at end of file