/9dofAhrs/RazorAhrs.cs

/*

 * This program 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; either version 2 of the License, or

 * (at your option) any later version.

 *

 * 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.,

 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

 *

 *

 * Project: 9dof: Razor 9DoF AHRS firmware and NETMF drivers

 * Description:  This is a collection of files that allows for easy

 * NETMf integration with the  * Sparkfun 9DoF Razor IMU.

 *

 * Author(s):

 * Chris Seto <chris@chrisseto.com>

 * Christian Völlinger <zerov83@googlemail.com>

 * 

 */



using System;

using System.IO.Ports;

using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;

using NineDofAhrs.Extensions;



namespace NineDofAhrs

{

	/// <summary>

	/// Class to parse serial output of Sparkfun 9DOF Razor AHRS

	/// Call ParseData() before you want to use new data!

	/// </summary>

	public class RazorAhrs : IAhrs

	{

		/// <summary>

		/// Serial port handle for the 9DoF

		/// </summary>

		private readonly SerialPort serialHandle;



		/// <summary>

		/// USed to reset the 9DoF

		/// </summary>

		private readonly Cpu.Pin dtr;



		/// <summary>

		/// 

		/// </summary>

		private readonly byte[] receiveBuffer = new byte[80];



		/// <summary>

		/// 

		/// </summary>

		private readonly char[] receivedChars = new char[80];



		/// <summary>

		/// 

		/// </summary>

		private readonly char[] parseBuffer = new char[80];



		/// <summary>

		/// 

		/// </summary>

		private int receiveIndex = 0;



		/// <summary>

		/// 

		/// </summary>

		private bool haveToParse = false;



		/// <summary>

		/// 

		/// </summary>

		private ParserState imuState = ParserState.FindPre;



		/// <summary>

		/// 

		/// </summary>

		private static readonly char[] pree = { '!', 'A', 'N', 'G', ':' };



		/// <summary>

		/// 

		/// </summary>

		private int preeIndex = 0;



		/// <summary>

		/// 

		/// </summary>

		private const string analogsPree = ",AN:";



		/// <summary>

		/// 

		/// </summary>

		private const string productSring = "Sparkfun 9DOF Razor AHRS";



		/// <summary>

		/// 

		/// </summary>

		public bool HasValidData = false;



		/// <summary>

		/// 

		/// </summary>

		public float Roll;



		/// <summary>

		/// 

		/// </summary>

		public float Pitch;



		/// <summary>

		/// 

		/// </summary>

		public float Yaw;



		/// <summary>

		/// AccX, AccY, AccZ, AnX, AnY, AnZ

		/// </summary>

		public int[] Offsets = new int[6];



		/// <summary>

		/// 

		/// </summary>

		public int AccX;



		/// <summary>

		/// 

		/// </summary>

		public int AccY;



		/// <summary>

		/// 

		/// </summary>

		public int AccZ;



		/// <summary>

		/// 

		/// </summary>

		public int AnX;



		/// <summary>

		/// 

		/// </summary>

		public int AnY;



		/// <summary>

		/// 

		/// </summary>

		public int AnZ;



		/// <summary>

		/// 

		/// </summary>

		public int MagX;



		/// <summary>

		/// 

		/// </summary>

		public int MagY;



		/// <summary>

		/// 

		/// </summary>

		public int MagZ;



		/// <summary>

		/// Are we going to parse analog outputs?

		/// </summary>

		private readonly bool ParseAnalogs = false;



		/// <summary>

		/// Parser phases

		/// </summary>

		private enum ParserState

		{

			Start,

			FindOffsets,

			FindPre,

			ParseEuler

		}



		/// <summary>

		/// 

		/// </summary>

		public int dataReceived { get; set; }



		/// <summary>

		/// 

		/// </summary>

		private bool ApplyOffsets = true;



		/// <summary>

		/// 

		/// </summary>

		/// <param name="comPort">Com port. "COM1"</param>

		/// <param name="parseAnalogs">true, if the analogdata of the gyros should also be parsed</param>

		public RazorAhrs(string comPort, bool parseAnalogs)

		{

			ParseAnalogs = parseAnalogs;

			serialHandle = new SerialPort(comPort, 57600, Parity.None, 8, StopBits.One);

			Initialize();

		}



		/// <summary>

		/// 

		/// </summary>

		/// <param name="com"></param>

		/// <param name="parseAnalogs"></param>

		/// <param name="dtr"></param>

		public RazorAhrs(string com, bool parseAnalogs, Cpu.Pin dtr) : this(com, parseAnalogs)

		{

			dtr = dtr;

		}



		/// <summary>

		/// 

		/// </summary>

		/// <param name="reset"></param>

		private void Initialize(bool reset = true)

		{

			serialHandle.ReadTimeout = 0;

			

			if (reset)

				Reset();



			// If you open the port after you set the event you will endup with problems

			serialHandle.Open();

			serialHandle.DiscardInBuffer();



			// Subscribe to RX events

			serialHandle.DataReceived += PortDataReceived;

			serialHandle.ErrorReceived += PortErrorReceived;

		}



		/// <summary>

		/// Reset the 9DoF

		/// </summary>

		private void Reset()

		{

			OutputPort dtr = new OutputPort(dtr, false);

			System.Threading.Thread.Sleep(1);



			dtr.Write(true);

			dtr.Dispose();

		}



		/// <summary>

		/// Parse recived data

		/// </summary>

		/// <param name="sender"></param>

		/// <param name="e"></param>

		void PortDataReceived(object sender, SerialDataReceivedEventArgs e)

		{

			int bufferIndex = 0;

			byte offsetIndex = 0;

			int dataReceived;



			dataReceived = serialHandle.Read(receiveBuffer, 0, receiveBuffer.Length);



			while (bufferIndex < dataReceived)

			{

				switch (imuState)

				{

					// Looking for IMU startup

					case ParserState.Start:

						if (this.receiveBuffer[bufferIndex] == productSring[preeIndex])

						{

							preeIndex++;



							if (this.preeIndex >= productSring.Length)

							{

								imuState = ParserState.FindOffsets;

								preeIndex = 0;

							}

						}



						break;



					// 

					case ParserState.FindOffsets:

						receivedChars[receiveIndex] = (char)receiveBuffer[bufferIndex];



						if (this.receivedChars[receiveIndex] == '\r' || receivedChars[receiveIndex] == '\n')

						{

							// Full line



							// More than \r\n ?

							if (this.receiveIndex > 2)

							{

								Debug.Print("X: " + receivedChars[receiveIndex]);

								Array.Copy(receivedChars, parseBuffer, receiveIndex);



								ParseOffset(out Offsets[offsetIndex++], parseBuffer);

								receiveIndex = 0;



								// Received all 6 offsets

								if (offsetIndex >= 6)

									imuState = ParserState.FindPre;

							}

						}

						else

							receiveIndex++;



						break;



					//

					case ParserState.FindPre:

						if (receiveBuffer[bufferIndex] == pree[preeIndex])

						{

							preeIndex++;

							if (preeIndex >= pree.Length)

							{

								imuState = ParserState.ParseEuler;

								preeIndex = 0;

							}

						}

						else

							preeIndex = 0;



						break;



					//

					case ParserState.ParseEuler:

						receivedChars[receiveIndex] = (char)receiveBuffer[bufferIndex];



						if (receivedChars[receiveIndex] == '\r' || receivedChars[receiveIndex] == '\n')

						{

							// Full Line

							if (receiveIndex > 10)

							{

								// Parse the Data!

								lock (parseBuffer)

								{

									Array.Copy(receivedChars, parseBuffer, receiveIndex);

									HasValidData = true;

								}



								haveToParse = true;

							}



							receiveIndex = 0;

							imuState = ParserState.FindPre;



						}

						else

						{

							receiveIndex++;



							if (receiveIndex >= receivedChars.Length)

							{

								Debug.Print("Sentence is too long!");

								receiveIndex = 0;

								imuState = ParserState.FindPre;

							}

						}



						break;

				}



				bufferIndex++;

			}

		}



		/// <summary>

		/// 

		/// </summary>

		/// <param name="sender"></param>

		/// <param name="e"></param>

		private static void PortErrorReceived(object sender, SerialErrorReceivedEventArgs e)

		{

			Debug.Print("COM Error: " + e.EventType);

		}



		private static void ParseOffset(out int offsetVar, char[] data)

		{

			int i = 0;

			offsetVar = FastNumberParse.ParseInt(data, ref i);

		}



		/// <summary>

		/// Parses the received data

		/// Call it just before you use the data

		/// </summary>

		public void ParseData()

		{

			if (!haveToParse)

				return;



			lock (parseBuffer)

			{

				haveToParse = false;

				int i = 0;

				Roll = FastNumberParse.ParseFloat(parseBuffer, ref i);

				i++;

				Pitch = FastNumberParse.ParseFloat(parseBuffer, ref i);

				i++;

				Yaw = FastNumberParse.ParseFloat(parseBuffer, ref i);



				if (ParseAnalogs)

				{

					// + ',AN:'

					i += analogsPree.Length;



					if (ApplyOffsets)

					{

						AnX = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[0];

						i++;

						AnY = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[1];

						i++;

						AnZ = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[2];

						i++;

						AccX = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[3];

						i++;

						AccY = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[4];

						i++;

						AccZ = FastNumberParse.ParseInt(parseBuffer, ref i) - Offsets[5];

					}

					else

					{

						AnX = FastNumberParse.ParseInt(parseBuffer, ref i);

						i++;

						AnY = FastNumberParse.ParseInt(parseBuffer, ref i);

						i++;

						AnZ = FastNumberParse.ParseInt(parseBuffer, ref i);

						i++;

						AccX = FastNumberParse.ParseInt(parseBuffer, ref i);

						i++;

						AccY = FastNumberParse.ParseInt(parseBuffer, ref i);

						i++;

						AccZ = FastNumberParse.ParseInt(parseBuffer, ref i);

					}



					i++;

					MagX = FastNumberParse.ParseInt(parseBuffer, ref i);

					i++;

					MagY = FastNumberParse.ParseInt(parseBuffer, ref i);

					i++;

					MagZ = FastNumberParse.ParseInt(parseBuffer, ref i);

				}

			}

		}



		/// <summary>

		/// Get compensated attitude output

		/// </summary>

		/// <param name="roll"></param>

		/// <param name="pitch"></param>

		/// <param name="yaw"></param>

		void IAhrs.GetAttitude(out double roll, out double pitch, out double yaw)

		{

			ParseData();

			roll = Roll;

			pitch = Pitch;

			yaw = Yaw;

		}



		/// <summary>

		/// Get analog output

		/// </summary>

		/// <param name="x"></param>

		/// <param name="y"></param>

		/// <param name="z"></param>

		void IAhrs.GetAnalogs(out double x, out double y, out double z)

		{

			ParseData();

			x = AnX;

			y = AnY;

			z = AnZ;

		}



		/// <summary>

		/// Get accelerometer output

		/// </summary>

		/// <param name="x"></param>

		/// <param name="y"></param>

		/// <param name="z"></param>

		void IAhrs.GetAcc(out double x, out double y, out double z)

		{

			ParseData();

			x = AccX;

			y = AccY;

			z = AccZ;

		}

	}

}