GPS data for X, Y, Z and related speeds

[Luis]

Hello,
I am using UM7 under NetMF. I used a driver for getting data from SPI port (reproduced below for convenience).
The thing is that I get correct eulers, accels and GPS lat/long/height data, but when recovering X,Y,Z and related rates they all
appear as Zero (0). My understanding from datasheet is that UM7 provides this convenience of translating latitude and longitude to cartesian coordinates. Am I wrong? or Is there an interim step I missed to recover those data items? Could it be related to the SPI? (I do not think so since I get real values for the other stuff as I mentioned). To note this was tested in the open with valid GPS source conected to the UM7.
Another topic is that Course data item seems off with a very large number where it should be bounded to below 360º. Any idea?
Thank you. LC.
***********************************************************************
/*
* Class UM7_SPI
* Luis C.: November 2015 for Quadcopter project
*
*/

using System;
using Microsoft.SPOT.Hardware;
using System.Threading;
using GT = Gadgeteer;

namespace LG_Tools
{
public class UM7
{
private SPI.Configuration SPI_config;
private SPI MySPI;
public UM7(int socket)
{
//Open UM6 SPI Port & Set-up
SPI.SPI_module mod = GT.Socket.GetSocket(socket, true, null, null).SPIModule;
Cpu.Pin pin = GT.Socket.GetSocket(socket, true, null, null).CpuPins[6]; //Chip select signal is always pin 6
SPI_config = new SPI.Configuration(pin, false, 0, 0, false, true, 400, SPI.SPI_module.SPI1);
MySPI = new SPI(SPI_config);
if (Get_Firmware() != “U71D”) throw new System.InvalidOperationException(“IMU device not reachable!”);
}

#region UM7_Functions
public void Reset_Gyros() { MySPI.Write(tx_data_gyro_reset); }
public void Set_GPS_Home() { MySPI.Write(tx_data_gps_set); }
public string Get_Firmware() {
MySPI.WriteRead(tx_data_firmware, rx_data_firmware);
return ((char)rx_data_firmware[2]).ToString() + (char)rx_data_firmware[3] + (char)rx_data_firmware[4] + (char)rx_data_firmware[5];
}
public void Update_GPS_fix()
{
//Get bytes
MySPI.WriteRead(tx_data_gps_fix, rx_data_gps_fix);
//Get X
_GPS_x = Converter_Library.Bytes_to_Single(rx_data_gps_fix, 2);
//Get Y
_GPS_y = Converter_Library.Bytes_to_Single(rx_data_gps_fix, 6);
//Get Z
_GPS_z = Converter_Library.Bytes_to_Single(rx_data_gps_fix, 10);

}
public void Update_GPS_latlonh()
{
//Get bytes
MySPI.WriteRead(tx_data_gps_latlonh, rx_data_gps_latlonh);
//Get latitude
_GPS_Lat = Converter_Library.Bytes_to_Single(rx_data_gps_latlonh, 2);
//Get longitude
_GPS_Lon = Converter_Library.Bytes_to_Single(rx_data_gps_latlonh, 6);
//Get Altitude
_GPS_Alt = Converter_Library.Bytes_to_Single(rx_data_gps_latlonh, 10);

}
public void Update_GPS_rates()
{
//Get bytes
MySPI.WriteRead(tx_data_gps_vels, rx_data_gps_vels);
//Get u
_GPS_u = Converter_Library.Bytes_to_Single(rx_data_gps_vels, 2);
//Get v
_GPS_v = Converter_Library.Bytes_to_Single(rx_data_gps_vels, 6);
//Get w
_GPS_w = Converter_Library.Bytes_to_Single(rx_data_gps_vels, 10);
}
public void Update_GPS_Course_Vg()
{ //Get bytes
MySPI.WriteRead(tx_data_gps_Course_Vg, rx_data_gps_Course_Vg);
//Get Course
_GPS_Course = Converter_Library.Bytes_to_Single(rx_data_gps_Course_Vg, 2);
//Get Ground Speed
_GPS_Vg = Converter_Library.Bytes_to_Single(rx_data_gps_Course_Vg, 6);
}
public void Update_IMU_Eulers()
{
//Get bytes
MySPI.WriteRead(tx_data_angles, rx_data_angles);
// Get Fi
short_aux = Converter_Library.Bytes_to_Short(rx_data_angles,2);
_Fi= (float)short_aux / 91.02222f;
//Get Theta
short_aux = Converter_Library.Bytes_to_Short(rx_data_angles, 4);
_Theta = (float)short_aux / 91.02222f;
// Get Psi
short_aux = Converter_Library.Bytes_to_Short(rx_data_angles,6);
_Psi = (float)short_aux / 91.02222f;
}
public void Update_IMU_Accels()
{
//Get bytes
MySPI.WriteRead(tx_data_accels, rx_data_accels);

//_az = Converter_Library.Bytes_to_Single_BigEndian(rx_data_accel, 2);
_ax = Converter_Library.Bytes_to_Single(rx_data_accels, 2);
_ay = Converter_Library.Bytes_to_Single(rx_data_accels, 6);
_az = Converter_Library.Bytes_to_Single(rx_data_accels, 10);

}
#endregion

#region UM7_Params
public string fw {
get { return Get_Firmware(); } }
private float _Fi = 0;
public float Fi{ get { return _Fi; } }
private float _Psi = 0;
public float Psi{ get { return _Psi; } }
private float _Theta = 0;
public float Theta { get { return _Theta; } }

private float _GPS_x = 0;
public float GPS_x { get { return _GPS_x; } }
private float _GPS_y = 0;
public float GPS_y { get { return _GPS_y; } }
private float _GPS_z = 0;
public float GPS_z { get { return _GPS_z; } }

private float _GPS_u = 0;
public float GPS_u { get { return _GPS_u; } }
private float _GPS_v = 0;
public float GPS_v { get { return _GPS_v; } }
private float _GPS_w = 0;
public float GPS_w { get { return _GPS_w; } }

private bool _GPS = false;
public bool GPS { get { return _GPS; } }
private ushort _GPS_nsats = 0;
public ushort GPS_nsats { get { return _GPS_nsats; } }

private float _ax = 0;
public float ax { get { return _ax; } }
private float _ay= 0;
public float ay { get { return _ay; } }
private float _az = 0;
public float az { get { return _az; } }

private float _GPS_Lat = 0;
public float GPS_Lat { get { return _GPS_Lat; } }
private float _GPS_Lon = 0;
public float GPS_Lon { get { return _GPS_Lon; } }
private float _GPS_Alt = 0;
public float GPS_Alt { get { return _GPS_Alt; } }

private float _GPS_Vg = 0;
public float GPS_Vg { get { return _GPS_Vg; } }
private float _GPS_Course = 0;
public float GPS_Course { get { return _GPS_Course; } }

#endregion

#region UM7_auxParams
private short short_aux = 0;
#endregion

#region UM7_Arrays
//gyros reset AD?????
static readonly byte[] tx_data_gyro_reset = new byte[] { (byte)0x01, (byte)0xAD, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//set_magnetic_reference
static readonly byte[] tx_data_mag_set = new byte[] { (byte)0x01, (byte)0xB0, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//set_gps_home command
static readonly byte[] tx_data_gps_set = new byte[] { (byte)0x01, (byte)0xAE, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//get_firmware
static readonly byte[] tx_data_firmware = new byte[] { (byte)0x01, (byte)0xAA, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
private byte[] rx_data_firmware = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//get_angles
static readonly byte[] tx_data_angles = new byte[] { (byte)0x00, (byte)0x70, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x71, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
private byte[] rx_data_angles = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//get_accels
static readonly byte[] tx_data_accels = new byte[] { (byte)0x00, (byte)0x65, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x66, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x67, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
private byte[] rx_data_accels = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//get_gps_fix
static readonly byte[] tx_data_gps_fix = new byte[] { (byte)0x00, (byte)0x75, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x76, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x77, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
private byte[] rx_data_gps_fix = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//get_gps_vels
static readonly byte[] tx_data_gps_vels = new byte[] { (byte)0x00, (byte)0x79, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x7A, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x7B, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
private byte[] rx_data_gps_vels = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
//get_gps_latlonh
static readonly byte[] tx_data_gps_latlonh = new byte[] { (byte)0x00, (byte)0x7D, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x7E, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x7F, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00};
private byte[] rx_data_gps_latlonh = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00};
//get_GPS__Course_Vg
static readonly byte[] tx_data_gps_Course_Vg = new byte[] { (byte)0x00, (byte)0x80, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x81, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
private byte[] rx_data_gps_Course_Vg = new byte[] { (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00 };
#endregion
}
public static class Converter_Library
{
public static short Bytes_to_Short(byte byte1, byte byte2)
{
return (short)((int)(byte1 << 8) | (int)byte2);
}
public static short Bytes_to_Short(byte[] array, int startIndex)
{
return (short)((int)(array[startIndex] << 8) | array[startIndex + 1]);
}
public static unsafe float Bytes_to_Single(byte[] array, int startIndex)
{
uint value = (uint)((uint)(array[startIndex] << 24) | (uint)(array[startIndex + 1] << 16) | (uint)(array[startIndex + 2] << 8) | (uint)(array[startIndex + 3]));
return *((float*)&value);
}

}
}
***********************
Use example:

UM7 imu = new UM7(5); //5 is socket in Cerberus mainboard
imu.Reset_Gyros();
imu.Update_GPS_rates();
Debug.Print(“u=”+ imu.GPS_u.ToString(“F2″) + ” v=” + imu.GPS_v.ToString(“F2”));

[Author]

Posts