예제 #1
0
 /// <summary>
 /// Class containing the latitude and longitude sent through the protocol.
 /// </summary>
 /// <param name="Lat"></param>
 /// <param name="Long"></param>
 public LocationPacketReceivedEventArgs(Latitude Lat, Longitude Long)
 {
     this.Lat = Lat;
     this.Long = Long;
 }
예제 #2
0
        /// <summary>
        /// Send the helicopter to a gps cooridinate, and specify the action on arrival
        /// </summary>
        /// <param name="Lat">A filled Latitude struct</param>
        /// <param name="Long">a filled longitude struct</param>
        /// <param name="action">0x48 to hover on arrival and 0x53 to circle at arrival</param>
        /// <param name="Altitude">Altitude to perform action at (metres)</param>
        public void Goto(Latitude Lat, Longitude Long, byte action, UInt16 Altitude)
        {
            /*
              1 0xA5    header
            0x5A

            0x0F    number of bytes in data
            0x46	Command type “Flight Ops”
            0x47	Command “Go To”

              6 0xDD	Degrees (0-180) Latitude
            0xMM	Minutes (0-59) Latitude
            0xSSSS	Seconds(0-59) Latitude. Use upper six bits for integer portion of seconds, use remaining 10 bits for decimal portion of seconds. There are approximately 31 meters in one second of latitude (and slightly less in 1 minute of longitude at our distance from the equator). 31m / 2^10 = 3 cm accuracy, which is way more accuracy than the GPS will determine.
            0xNS	0x4E = North
                    0x53 = South

             11 0xDD	Degrees (0-180) Longitude
            0xMM	Minutes (0-59) Longitude
            0xSSSS	Seconds(0-59) Longitude. See above for more details.
            0xEW	0x45 = East
                    0x57 = West

             16 0xAA	Action on Arrival:
                    IF 0x48 Hover at given altitude ELSE IF 0x53 Circle at given altitude
            0xAAAA	Given altitude

             19 0xXX checksum High
            0xXX checksum Low
            0xCC
             22 0x33 footer
            */
            if (ExpectedResponse.ResponseExpected == false)
            {
                OutGoingPacket = new byte[22];
                OutGoingPacket[0] = 0xA5;
                OutGoingPacket[1] = 0x5A;
                OutGoingPacket[2] = 0x0F;//number of bytes
                OutGoingPacket[3] = 0x46;//Command type “Flight Ops”
                OutGoingPacket[4] = 0x47;
                OutGoingPacket[5] = Lat.Degrees;
                OutGoingPacket[6] = Lat.Minutes;
                //highest 2 bits of the first seconds packet were used for bits 9 and 10 of the secondsL value
                OutGoingPacket[7] = (byte)((Lat.FractionalMinutes & 0xFF00) >> 8);
                OutGoingPacket[8] = (byte)(Lat.FractionalMinutes & 0xFF );
                if (Lat.North)
                {
                    OutGoingPacket[9] = 0x4E;
                }
                else
                {
                    OutGoingPacket[9] = 0x53;
                }
                OutGoingPacket[10] = Long.Degrees;
                OutGoingPacket[11] = Long.Minutes;
                //highest 2 bits of the first seconds packet were used for bits 9 and 10 of the secondsL value
                OutGoingPacket[12] = (byte)((Long.FractionalMinutes & 0xFF00)>>8);
                OutGoingPacket[13] = (byte)(Long.FractionalMinutes & 0xFF);
                if (Long.East)
                {
                    OutGoingPacket[14] = 0x45;
                }
                else
                {
                    OutGoingPacket[14] = 0x57;
                }
                if (action == 0x48)
                {
                    OutGoingPacket[15] = 0x48;
                }
                else if (action == 0x53)
                {
                    OutGoingPacket[15] = 0x53;
                }
                else
                {
                    throw new Exception("CommProtocol.Goto(): invalid action specified.  Must be either 0x48 or 0x53");
                }
                OutGoingPacket[16] = (byte)((Altitude & 0xFF00) >> 8);//altitude high byte
                OutGoingPacket[17] = (byte)(Altitude & 0x00FF);//altitude low byte
                //calculate checksum
                short checksum = 0;
                for (int i = 2; i < 18; i++)//calculate checksum
                {
                    checksum += OutGoingPacket[i];
                }
                OutGoingPacket[18] = (byte)((checksum & 0xFF00) >> 8);//checksum high
                OutGoingPacket[19] = (byte)(checksum & 0x00FF);//checksum low
                OutGoingPacket[20] = 0xCC;
                OutGoingPacket[21] = 0x33;//footer

                SendPacket("Goto", ExpectedResponses.type.FullPacketResponse);

            }
        }
예제 #3
0
        private void ParseLocationPacket()
        {
            /*
            Location Packet:
              1 0xA5    SOT1
                0x5A    SOT2

              3 0x0B    bytes

                0x74	Report type “Telemetry”
              5 0x4C	Report “Location”

                0xDD	Degrees (0-180) Latitude
                0xMM	Minutes (0-59) Latitude
                0xSSSS	Seconds (0-59) Latitude. Use first six bits for integer portion of seconds, use remaining 10 bits for decimal portion of seconds. There are approximately 31 meters in one second of latitude (and slightly less in 1 minute of longitude at our distance from the equator). 31m / 2^10 = 3 cm accuracy, which is way more accuracy than the GPS will determine.
             10 0xNS	0x4E = North or 0x53 = South

                0xDD	Degrees (0-180) Longitude
                0xMM	Minutes (0-59) Longitude
                0xSSSS	Seconds (0-59) Longitude. See above for more details.
             15 0xEW	0x45 = East or 0x57 = West

                0xXX    Checksum 1
                0xXX    Checksum 2

                0xCC    EOT1
             19 0x33    EOT2
            */
            if (IncomingDataBuffer.Length == 19 && (int)IncomingDataBuffer[17] == 0xCC && (int)IncomingDataBuffer[18] == 0x33)
            {
                //calculate checksum
                UInt16 sum = 0;
                for (int i = 2; i < 15; i++)
                {
                    sum += (ushort)IncomingDataBuffer[i];
                }
                byte chk1 = (byte)((sum & 0xFF00) >> 8);
                byte chk2 = (byte)(sum & 0x00FF);
                if (chk1 != (int)IncomingDataBuffer[15] || chk2 != (int)IncomingDataBuffer[16])
                {
                    Invoke(BadPacketReceived, new object[] {
                        this,
                        new BadPacketReceivedEventArgs(IncomingDataBuffer,
                        string.Format("Invalid Location packet: invalid checksum received: {0:x4}, expected: {1:x4}",
                        (Convert.ToUInt16((int)IncomingDataBuffer[15]) << 8) + (int)IncomingDataBuffer[16], sum))
                    }
                    );
                    ClearBuffer();
                }
                else
                {
                    //checksum ok
                    Latitude Lat = new Latitude();
                    Lat.Degrees = (byte)IncomingDataBuffer[5];
                    Lat.Minutes = (byte)IncomingDataBuffer[6];
                    Lat.FractionalMinutes = (ushort)(((ushort)IncomingDataBuffer[7] << 8) + (ushort)IncomingDataBuffer[8]);
                    if ((byte)IncomingDataBuffer[9] == 0x4E)
                    {
                        Lat.North = true;
                    }
                    else if ((byte)IncomingDataBuffer[9] == 0x4E)
                    {
                        Lat.North = false;
                    }
                    else
                    {
                        Invoke(BadPacketReceived, new object[] { this, new BadPacketReceivedEventArgs(IncomingDataBuffer, "Invalid packet: The north south byte of the Latitude in a received location packet is invalid.") });
                        ClearBuffer();
                        return;
                    }

                    Longitude Long = new Longitude();
                    Long.Degrees = (byte)IncomingDataBuffer[10];
                    Long.Minutes = (byte)IncomingDataBuffer[11];
                    Long.FractionalMinutes = (ushort)(((ushort)IncomingDataBuffer[12] << 8) + (ushort)IncomingDataBuffer[13]);
                    if ((byte)IncomingDataBuffer[14] == 0x45)
                    {
                        Long.East = true;
                    }
                    else if ((byte)IncomingDataBuffer[14] == 0x57)
                    {
                        Long.East = false;
                    }
                    else
                    {
                        Invoke(BadPacketReceived, new object[] {this,
                            new BadPacketReceivedEventArgs(IncomingDataBuffer,
                            "Invalid packet: The east west byte of the Longitude in a received location packet is invalid.")
                        });
                        ClearBuffer();
                        return;
                    }
                    //invoke the event
                    ClearBuffer();
                    Invoke(LocationPacketReceived, new object[] { this, new LocationPacketReceivedEventArgs(Lat, Long) });

                }
            }
            else if (IncomingDataBuffer.Length >= 19)
            {
                Invoke(BadPacketReceived, new object[] {
                    this, new BadPacketReceivedEventArgs(IncomingDataBuffer, "Bad location packet received")
                });
                ClearBuffer();
            }
        }
예제 #4
0
        void GPSP_GPSStringReceived(object sender, GPSParser.GPSStringReceivedEventArgs e)
        {
            if (e.data.Long.Degrees != 0 && e.data.Lat.Degrees != 0)
            {
                Lon = e.data.Long;
                Lat = e.data.Lat;

                if (GoogleMapCtrl.BaseStationSet)
                {
                    RequestInfoTimer.Enabled = false;

                    double Longitude = -(Lon.Degrees + ((Lon.Minutes + (Lon.FractionalMinutes / 10000.0)) / 60.0));
                    double Latitude = Lat.Degrees + ((Lat.Minutes + (Lat.FractionalMinutes / 10000.0)) / 60.0);
                    GoogleMapCtrl.SetMarkerAtLoc(Latitude, Longitude);
                    double LatCorrection = 0;//GoogleMapCtrl.BaseStation.latitude - Latitude;
                    double LongCorrection = 0;//GoogleMapCtrl.BaseStation.longitude - Longitude;

                    GPSCorrection gpsc = new GPSCorrection();
                    if (AltitudeSamples < 10)
                    {
                        gpsc.Altitude = 0;//zero correction until we have enough base station samples
                        BaseStationAltitude += e.data.Altitude;
                        AltitudeSamples++;

                    }
                    if (AltitudeSamples == 10)
                    {
                        gpsc.Altitude = 0;//zero correction until we have enough base station samples
                        AltitudeSamples = 11;
                        BaseStationAltitude = BaseStationAltitude / 10.0;
                    }

                    if (AltitudeSamples == 11)
                    {
                        gpsc.Altitude = Convert.ToInt16((e.data.Altitude * 10) - (BaseStationAltitude * 10));
                        txtAltitudeCorrection.Text = gpsc.Altitude.ToString();
                    }
                    gpsc.Time = new DateTime();
                    gpsc.Time.AddSeconds(e.data.GPSDateTime.Second);

                    gpsc.LatMinutes = (short)(LatCorrection * 60.0 * 10000.0);
                    gpsc.LongMinutes = (short)-(LongCorrection * 60.0 * 10000.0);

                    txtLatCorrection.Text = gpsc.LatMinutes.ToString();
                    txtLongCorrection.Text = gpsc.LongMinutes.ToString();

                    if (ComProt != null)
                    {
                        int timeout = 0;
                        while (ComProt.ExpectedResponse.ResponseExpected)
                        {
                            if (timeout > 1000)
                            {
                                break;
                            }
                            timeout++;
                        }
                        ComProt.SendGPSCorrectionFactor(gpsc);

                        RequestInfoTimer.Enabled = true;
                    }
                }
            }
        }
예제 #5
0
        void ComProt_LocationPacketReceived(object sender, CommProtocol.LocationPacketReceivedEventArgs e)
        {
            CommPacketRXLight.On = true;
            PacketReceived = true;
            if (e.Long.Degrees != 0 && e.Lat.Degrees != 0)
            {
                Lon = e.Long;
                Lat = e.Lat;

                double Longitude = -(Lon.Degrees + ((Lon.Minutes + (Lon.FractionalMinutes / 10000.0)) / 60.0));
                double Latitude = Lat.Degrees + ((Lat.Minutes + (Lat.FractionalMinutes / 10000.0)) / 60.0);
                GoogleMapCtrl.GotoLoc(Latitude, Longitude);
            }
        }
예제 #6
0
        private void cp_LocationPacketReceived(object sender, CommProtocol.LocationPacketReceivedEventArgs e)
        {
            ConsecutiveBadPackets = 0;

            Lat = e.Lat;
            Lon = e.Long;

            Double Longitude = -(Lon.Degrees + Lon.Minutes / 60.0 + (double)(Lon.FractionalMinutes) / 60000.0);
            Double Latitude = Lat.Degrees + Lat.Minutes / 60.0 + (double)(Lat.FractionalMinutes) / 60000.0;

            textBox1.AppendText("Location packet received: Location" +
                                " Lat degrees: " + Lat.Degrees +
                                " Lat minutes: " + Lat.Minutes +
                                " Lat minutes remainder: " + Lat.FractionalMinutes +
                                " North: " + Lat.North +
                                " floating Latitude: " + Latitude +
                                " Long degrees: " + Lon.Degrees +
                                " Long minutes: " + Lon.Minutes +
                                " Long minutes remainder: " + Lon.FractionalMinutes +
                                " East: " + Lon.East +
                                " floating Longitude: " + Longitude + "\r\n");

            if (!ManualMode)
            {
                InsertRowToReceived_packetsTable("Location",
                                    " Lat degrees: " + Lat.Degrees +
                                    " Lat minutes: " + Lat.Minutes +
                                    " Lat minutes remainder: " + Lat.FractionalMinutes +
                                    " North: " + Lat.North +
                                    " Long degrees: " + Lon.Degrees +
                                    " Long minutes: " + Lon.Minutes +
                                    " Long minutes remainder: " + Lon.FractionalMinutes +
                                    " East: " + Lon.East);
            }
        }
예제 #7
0
        private void btnGoto_Click(object sender, EventArgs e)
        {
            Latitude lat = new Latitude();
            lat.Degrees= 0;
            lat.Minutes = 0;
            lat.North = true;
            lat.FractionalMinutes = 0;

            Longitude lon = new Longitude();
            lon.Degrees = 0;
            lon.Minutes = 0;
            lon.East = true;
            lon.FractionalMinutes = 0;

            cp.Goto(lat,lon,0x48,50);
        }