I've been experimenting with a gyro sensor based compass. There has been a few surprises in the process, and I'd like to share the program and some experiences with you.
I'm using the HiTechnic Gyro Sensor and LeJOS.
First surprise is how amazing precise this sensor actualy is. It can actually work as a pretty precise compass that doesn't get affected by magnetic fields or weather it's level.
The sensor does however drift a bit, and I've used some simple tricks to avoid it in the sample below (Does anyone know how to attach a Java program?)
I have heard that a "Kalman filter" could help improve the accuracy of the sensor readings. Does anyone here have experiences with such a filter? Some usable source code perhaps...? (NO this is NOT for a school project!
)Regarding the enclosed code: Use and experiment as you find fit, and please post your experiences here... Please note that it's experimental, and comes with absolutely no warranty, so please do not use it for missile guidance or something...
Kind regards
Povl
Code: Select all
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.GyroSensor;
/**
* This is an experimental test class to study the HiTechnic Gyro Sensor for use as a gyro compass..
*
* The class is not in a finished state, and is only meant for experiments.
*
* @author kvols
*
*/
public class GyroCompassTest implements Runnable {
public static final int SAMPLE_RATE= 5;
private final GyroSensor gs;
// Heading in 1/1000 degrees
private int heading = 0;
// Sensor offset in 1/1000 degrees/s
int sensorOffset;
public GyroCompassTest(GyroSensor gs) throws InterruptedException {
this.gs = gs;
// Find a reasonable offset
for (int i = 0; i < 20; i++) {
sensorOffset += gs.readValue();
Thread.sleep(3);
}
sensorOffset *= 50;
// Start the sensor sampler
Thread t = new Thread(this);
t.setDaemon(true);
t.start();
}
/**
* @throws InterruptedException
*/
public static void main(String[] args) throws InterruptedException {
GyroSensor gs = new GyroSensor(SensorPort.S1);
GyroCompassTest gt = new GyroCompassTest(gs);
while (!Button.ESCAPE.isPressed()) {
if (Button.ENTER.isPressed()) // Set to zero
gt.heading = 0;
Thread.sleep(25);
LCD.clear();
// Display the heading rounded to nearest degree:
LCD.drawInt(gt.sensorOffset, 5, 5);
// Display the current estimated sensor offset (1000nds):
LCD.drawInt((gt.heading+500)/1000, 5, 3);
}
}
@Override
public void run() {
int av = 0;
int oldAv;
int err = 0;
int errCount = 0;
long time = System.currentTimeMillis();
try {
Thread.sleep(SAMPLE_RATE);
for (;;) {
oldAv = av;
av = gs.readValue() * 1000 - sensorOffset;
// Detect no movement and fine tune the offset
if (av >= -1000 && av <= 1000) {
err += av;
errCount++;
// Tune the offset if still for some time
if (errCount == 30) {
// Let the offset converge to prevent over-compensation:
sensorOffset += err / (errCount*2);
// Adjust the current heading, to prevent idle drifting:
av = -err;
errCount = 0;
err = 0;
}
} else {
errCount = 0;
err = 0;
}
// Calculate the absolute rotation
int dt = (int) (System.currentTimeMillis() - time);
time += dt;
heading += ((oldAv + av) * dt) / 2000;
// Sleep or some time
Thread.sleep(SAMPLE_RATE);
}
} catch (InterruptedException e) { // Just stop if interrupted!
}
}
}
