The following is a driver for the Sparkfun 9DoF Razor IMU. The Razor runs a custom version of the AHRS firmware that waits for a query before outputting data. That way, I’m not dealing with a wasting time parsing constant stream of data.
I want to see if I can get even more efficient than that, though.
Anybody have any suggestions?
/*
* Sparkfun 9DoF AHRS Razor IMU Driver
* Coded by Chris Seto August 2010
* <chris@ chrisseto.com>
*
* Note: This driver requires a slight modification to the IMU firmware.
* Please use the output.pde file that comes bundled with this driver to include
* the mod in your IMU's firmware.
*
* This software is released under the Apache 2.0 license.
* Copyright Chris Seto, 2010
*
* Chris Seto: Initial release (1.0)
* Chris Seto: Removed angle remapping
* Chris Seto: Support for accel readings (1.1)
*
* */
using System.IO.Ports;
using System.Text;
using System.Threading;
using Microsoft.SPOT;
namespace Razor_9DoF_API
{
class Razor_9DoF
{
/// <summary>
/// Serial Port handle for the Razor
/// </summary>
private SerialPort _Razor;
/// <summary>
/// Yaw degrees (-180 to 180)
/// </summary>
public double Yaw
{
get
{
return GetValue(2);
}
}
/// <summary>
/// Pitch degrees (-180 to 180)
/// </summary>
public double Pitch
{
get
{
return GetValue(1);
}
}
/// <summary>
/// Roll degrees (-180 to 180)
/// </summary>
public double Roll
{
get
{
return GetValue(0);
}
}
/// <summary>
/// Accel X
/// </summary>
public double Accel_X
{
get
{
return GetValue(3);
}
}
/// <summary>
/// Accel Y
/// </summary>
public double Accel_Y
{
get
{
return GetValue(4);
}
}
/// <summary>
/// Accel Z
/// </summary>
public double Accel_Z
{
get
{
return GetValue(5);
}
}
/// <summary>
/// Keep the values here until we process them
/// </summary>
private double[] Raws = new double[6];
/// <summary>
/// Construct, set the recv event, bind the serial handle
/// </summary>
/// <param name="razorPort"></param>
public Razor_9DoF(string razorPort)
{
// Bind the serial port
_Razor = new SerialPort(razorPort, 57600);
// Options..
_Razor.ReadTimeout = 0;
// Open the port
_Razor.Open();
}
/// <summary>
/// Get a certain word
/// </summary>
/// <param name="word"></param>
/// <returns></returns>
private double GetValue(int word)
{
ReadRazor();
return Raws[word];
}
/// <summary>
/// Check for valid data, parse
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void ReadRazor()
{
// Capital P
_Razor.Write(new byte[] {80}, 0, 1);
Thread.Sleep(25);
// Create the buffer
byte[] buffer = new byte[_Razor.BytesToRead];
// Read the serial buffer
_Razor.Read(buffer, 0, buffer.Length);
_Razor.DiscardInBuffer();
Debug.Print(new string(System.Text.UTF8Encoding.UTF8.GetChars(buffer)));
// Get the tokens in one sentence (Yes, I know, FUGLY!)
string[] recv = new string(System.Text.UTF8Encoding.UTF8.GetChars(buffer)).Split('\n')[0].Split(',');
// If it's complete, process it
if (recv.Length > 7)
{
// Loop through the values in the sentence, parse
// them and add them to the raws array
for (int i = 1; i <= 6; i++)
Raws[i - 1] = double.Parse(recv[i]);
}
}
}
}
