Heading hold

Projects General Guidance
1

Hi.
I an puting together RoV ( remotley operated vehicle ) comms and sensor bits.
I have CMPS03 compass which gives me a reading of 0-3599 for a full circle, representing 0-359.9 degrees. So no problembs with bit.
I am trying to work out how to implement a compass hold.
I have browsed the forum and found bits but nothing that's of any use or ideas.
The best I have found is here http://arduino.cc/forum/index.php/topic,106683.0.html
I am looking for pointers rather than code as I am still in the learning stages.

Regards Antony

2

Hi.
After playing I have A excuse for Code.
Some will cry some will say "its a try"
So this is what I have at the moment.

#include "Wire.h"
#include "CMPS03.h"
#include <Servo.h>
CMPS03 cmps03;
#define SERVO  2

Servo    myservo;


int  desiredheading = 55;
int  error;

int val;

void setup()
{
  Serial.begin(9600);
  myservo.attach(SERVO);
  Wire.begin();
}

void loop()
{

  error = desiredheading - (cmps03.read());                                     
  if(error < - 1800) error =(3600 + error);                
  else if(error > 1800) error =(- 3600 - error);       // output here is between -1800 +1800

  val = map(error, -1800, 1800, 0, 180);         // scale it to use it with the servo (value between 0 and 180) 
  myservo.write(val);                     

  Serial.print(" error =  ");
  Serial.print(error);
  Serial.print("                        Servo = ");
  Serial.println(val);
  delay(200);
  
}

The issue I face ATM is if my true heading is 310 degrees and I need to turn to 55 degrees via the shortest turn..this works until it goes past North 0 360.
At this point the servo runs away and I have no idea on how to overcome this issue.
Any thoughts on this would be really helpful and encouraging.

Regards Antony.

3

had a look at your code. Did a quick refactor , find the 7 differences ;).

One bug was that the desired heading should be in 1/10th of degrees too.

Code compiles => Give it a try.

//
//    FILE: compass.pde
//  AUTHOR: 
//    DATE: 2012-06-04
//
// PUPROSE: keeping heading
//

#include "Wire.h"
#include "CMPS03.h"
#include <Servo.h>

CMPS03 cmps03;

#define SERVO  2

Servo    myservo;

int  desiredheading = 550;  // must be in 1/10th degree as compass is also in 1/10th
int val = 90;

void setup()
{
  Serial.begin(115200);
  myservo.attach(SERVO);
  Wire.begin();
  
  myservo.write(val);    
}

void loop()
{
  // average 4 readings to remove noise.
  int heading = 0;
  for (int i=0; i<4; i++) heading += cmps03.read();
  heading /= 4;

  int error = desiredheading - heading;
 
  if (abs(error) >= 20) // NOTE: |error| < 20 will not pass the map function!
  {
    if (error < -1800) error += 3600; 
    else if (error > 1800) error -= 3600;
    
    val = map(error, -1800, 1800, 0, 180);         // scale it to use it with the servo (value between 0 and 180)
    // shorter formula
    // val = error/20 + 90;
    
    myservo.write(val);                     
  }
  
  Serial.print("  heading =  ");
  Serial.print(heading);
  Serial.print("  desiredheading =  ");
  Serial.print(desiredheading );
  Serial.print("  error =  ");
  Serial.print(error);
  Serial.print("  Servo = ");
  Serial.println(val);
  delay(200);
}
4

Hi Rob.
I picked up the first one 1/10 degrees. This helped
But the read the compass 4 times then devide bu 4 gives me what I wanted...THANKS.
I have the test rig with Arduino uno Cmps03 and a micro servo mounted in a board.
when I turn the board the Servo(rudder) correct's the error.
Thats all I wanted from this sketch...

Thanks Antony.

5

Almost every sensor I know has noise, some more than others. By oversampling / averaging the noise is made less.

Good to hear the sketch is working, however as I pointed out small deviations less than 20 are currently not corrected for.

The first way to handle this is to introduce proper rounding.
Another - heuristic - approach is to make an estimate of the sum of the error e.g. 10 seconds @ -0.5 degree (which cannot be done by the servo) could be corrected by 4 seconds @ -1 degree (which can be done by the servo). The numbers can be found by some triangel / sine() math or by trial and error...

6

Hi Rob.
The output from this sketch will be feed into the thrusters to correct headings.
As the output is at the moment it's perfect.
Just for people's intrest:-
It works like this If I move rhe RoV around then don't move the joystick the Arduino will take a heading and this will be it goal to hold.
Thrusters are controlled by Esc(revirseable).
Also the Depth will have a Auto hold.
Again thank You.

Regards Antony.