GPS data for X, Y, Z and related speeds

Home Forums UM7 Product Support GPS data for X, Y, Z and related speeds

This topic contains 0 replies, has 1 voice, and was last updated by  Luis 2 years, 11 months ago.

  • Author
    Posts
  • #999

    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”));

You must be logged in to reply to this topic.