Using BasicX to Derive Acceleration From GARMIN GPS Text Data

Transcription

1 Basic Express Application Note Using BasicX to Derive Acceleration From GARMIN GPS Text Data Introduction Global Positioning System (GPS) receivers are typically able to measure position, velocity and time. With additional processing, a GPS receiver can also be used as an accelerometer by differentiating velocity with respect to time. GARMIN GPS receivers, such as etrex TM, can be configured to transmit data in a format called Simple Text Output protocol. A BasicX computer is used to read and decode the text data, and derive acceleration from GPS readings. Hardware interface Figure 1 (below) illustrates the hardware interface between a GPS receiver and BasicX system: Figure 1 The BasicX serial input pin number on depends on the system. For BX-01 systems, port Com2 is used on pin 12. For BX-24 and BX-35 systems, port Com3 is used on pin 16. If you use a GARMIN etrex or similar receiver, it should be set to text out mode at 1200 baud.

2 Software Interface The GARMIN GPS data format is documented here: In this format, GPS data consists of a simple 7 bit ASCII text string with a constant length of 57 bytes. On etrex receivers, the string is transmitted about once per second. A BasicX microcontroller is used to extract GPS time and velocity data embedded in the text strings. Single precision floating point variables are used to store the data. In the BasicX demonstration program, GPS data is decoded and written to an array called NewFix, which is a 4 dimensional floating point array. Each array element is defined as follows: Array Element Definition NewFix(1) NewFix(2) NewFix(3) NewFix(4) Velocity East (m/s) Velocity North (m/s) Velocity up (m/s) Time of day (s) A similar array called OldFix stores data from the previous GPS fix. The acceleration vector A is derived by taking the velocity vector V and differentiating it with respect to time. A is approximated by taking the differences between each velocity component at times t 2 and t 1 and dividing by t = (t 2 - t 1 ). The velocity and acceleration vectors are oriented assuming a right hand coordinate system. The magnitude of the acceleration vector A is also calculated using the following formula: A = A e 2 + A n 2 + A u 2 Whenever the BasicX processor decodes a GPS fix, the velocity and acceleration data are converted to ASCII strings and transmitted out the BasicX Com1 serial port configured to baud, 8 data bits, 1 stop bit, no parity. Example format: ================== Ve: m/s Vn: m/s Vu: m/s Ae: g An: g Au: g A: g ================== 2

3 Sometimes the GPS receiver may not pick up signals from enough satellites for complete position and velocity fixes. In these cases, data may not be available, or may be only partially available. When this happens, the text string Invalid data is transmitted in place of velocity and acceleration data. Raw text display. Instead of decoded data, you can choose to display untranslated text as received from the GPS receiver. In the main program, the Boolean constant RawText controls this selection. A typical line of raw text looks like Program listing. The program GPSAccelerometer consists of the following 5 modules, which are included as separate files along with this application note: GPSAccelerometer.bas GPSVelocityDecoder.bas GPSSerialPort.bas GPSPinAssignments.bas Format3x3.bas Includes the main program and other high level code Does the low level decoding of text data Handles the GPS serial port -- Defines the serial I/O pin number and port number -- Converts floating point number to formatted string Each file is listed on the following pages. The program is written for a BX-24 system. If you want to port the program to other BasicX systems, most code changes should be localized to module GPSPinAssignments, and are flagged by comments. 3

4 Module GPSAccelerometer Attribute VB_Name = "GPSAccelerometer" Option Explicit Public Sub Main() ' This program derives acceleration by differentiating GPS velocity data. ' The GPS data is in Garmin Simple Text Output format, and is received over ' a serial port. Dim OldFixIsValid As Boolean Dim OldFix(1 To 4) As Single Dim NewFixIsValid As Boolean Dim NewFix(1 To 4) As Single Dim N As Byte Dim Acceleration(1 To 3) As Single ' Set this flag to true to display raw text from the GPS receiver. Const RawText As Boolean = False OldFixIsValid = False NewFixIsValid = False Call InitializeGPS Debug.Print Debug.Print "GPS Accelerometer" Delay 0.5 Debug.Print Debug.Print "Waiting for GPS data..." Debug.Print If (RawText) Then Call DisplayRawText Do Call GetFixGPS(NewFixIsValid, NewFix) If (NewFixIsValid) And (OldFixIsValid) Then If (SanityCheckOK(OldFix, NewFix)) Then Call DisplayVelocity(NewFix) Call ToAcceleration(OldFix, NewFix, Acceleration) Call DisplayAcceleration(Acceleration) Call LargeDivider Else Call DisplayError 4

5 Else Call DisplayError If (NewFixIsValid) Then ' NewFix becomes OldFix for the next cycle. For N = 1 To 4 OldFix(N) = NewFix(N) Next OldFixIsValid = True Loop Private Sub DisplayRawText() ' This procedure displays raw text received from the GPS receiver. Do Debug.Print Chr(GetByte); Loop Private Function SanityCheckOK( _ ByRef OldFix() As Single, _ ByRef NewFix() As Single) As Boolean ' Check that the time difference between successive GPS velocity fixes is ' within a reasonable range. Dim DeltaT As Single ' Legal range is about 1 s to 3 s. Const MinDeltaT As Single = 0.9 Const MaxDeltaT As Single = 3.1 DeltaT = NewFix(4) - OldFix(4) If (DeltaT >= MinDeltaT) And (DeltaT <= MaxDeltaT) Then SanityCheckOK = True Else SanityCheckOK = False End Function 5

6 Private Sub ToAcceleration( _ ByRef OldFix() As Single, _ ByRef NewFix() As Single, _ ByRef Acceleration() As Single) ' Approximate acceleration by taking Delta_V over Delta_T. Dim DeltaT As Single DeltaT = NewFix(4) - OldFix(4) Acceleration(1) = (NewFix(1) - OldFix(1)) / DeltaT Acceleration(2) = (NewFix(2) - OldFix(2)) / DeltaT Acceleration(3) = (NewFix(3) - OldFix(3)) / DeltaT Private Sub DisplayVelocity( _ ByRef FixGPS() As Single) Debug.Print " Debug.Print " Debug.Print " Ve: "; ToStr(FixGPS(1)); " m/s" Vn: "; ToStr(FixGPS(2)); " m/s" Vu: "; ToStr(FixGPS(3)); " m/s" Call SmallDivider Private Sub DisplayAcceleration( _ ByRef Acceleration() As Single) ' This converts m/s^2 to G. Const UnitConvert As Single = 9.81 Dim A As Single Debug.Print " Ae: "; ToStr(Acceleration(1) / UnitConvert); " g" Debug.Print " An: "; ToStr(Acceleration(2) / UnitConvert); " g" Debug.Print " Au: "; ToStr(Acceleration(3) / UnitConvert); " g" A = Sqr( Acceleration(1) * Acceleration(1) _ + Acceleration(2) * Acceleration(2) _ + Acceleration(3) * Acceleration(3) ) Debug.Print Debug.Print " A: "; ToStr(A / UnitConvert); " g" 6

7 Private Function ToStr( _ ByVal X As Single) As String ' Converts float to a 7 character string, right justified, blank filled. The ' number is displayed to 3 decimal places. The number should be in range ' to Dim LeadingBlanks As New BoundedString_2 ' Space for minus sign. If (X >= 0.0) Then LeadingBlanks = " " Else LeadingBlanks = "" ' Space for digits. If (Abs(X) < 10.0) Then LeadingBlanks = LeadingBlanks & " " ToStr = LeadingBlanks & Fmt(X, 3) End Function Private Sub DisplayError() Debug.Print "Invalid data" Debug.Print Private Sub SmallDivider() Debug.Print " " Private Sub LargeDivider() Debug.Print "==================" 7

8 Module GPSVelocityDecoder Attribute VB_Name = "GPSVelocityDecoder" Option Explicit Private Const ChrEast As Byte = 69 ' E Private Const ChrNorth As Byte = 78 ' N Private Const ChrUp As Byte = 85 ' U Private Const MaxDigits As Byte = 5 Private PowTenLookup(1 To MaxDigits) As Integer Private DataIsValid As Boolean Public Sub InitializeGPS() PowTenLookup(1) = 1 PowTenLookup(2) = 10 PowTenLookup(3) = 100 PowTenLookup(4) = 1000 PowTenLookup(5) = Call OpenSerialPortGPS Public Sub GetFixGPS( _ ByRef FixIsValid As Boolean, _ ByRef FixGPS() As Single) Dim TimeGPS(1 To 3) As Byte Dim Velocity(1 To 3) As Integer Const SentenceStart As Byte = 64 ' "@" Dim N As Byte Dim Temp As Byte Dim Sign As Integer Dim T As Single DataIsValid = True ' Character ' Position ' Start sentence. Do Until (GetByte = SentenceStart) ' 1 ' Null Loop ' Ignore year/month/day. For N = 2 To 7 ' Temp = GetByte Next ' Decode hour/minute/second. For N = 1 To 3 TimeGPS(N) = CByte(GetNumber(2)) ' Next 8

9 ' Ignore all position data. For N = 14 To 40 ' Temp = GetByte Next ' Velocity east/west. Sign = GetSign(ChrEast) ' 41 Velocity(1) = GetNumber(4) * Sign ' ' Velocity north/south. Sign = GetSign(ChrNorth) ' 46 Velocity(2) = GetNumber(4) * Sign ' ' Velocity up/down. Sign = GetSign(ChrUp) ' 51 Velocity(3) = GetNumber(4) * Sign ' ' End sentence Temp = GetByte ' 56 Temp = GetByte ' 57 ' Velocity unit conversions. FixGPS(1) = CSng(Velocity(1)) / 10.0 FixGPS(2) = CSng(Velocity(2)) / 10.0 FixGPS(3) = CSng(Velocity(3)) / ' Time unit conversions. T = CSng(TimeGPS(3)) ' Seconds. T = T + ( CSng(TimeGPS(2)) * 60.0 ) ' Minutes. T = T + ( CSng(TimeGPS(1)) * ) ' Hours. FixGPS(4) = T FixIsValid = DataIsValid 9

10 Public Function GetNumber( _ ByVal DigitCount As Byte) As Integer ' This function converts a decimal string to an integer. The string is of ' length DigitCount. ' ' Each character must be a decimal digit -- otherwise the character is ' illegal. If any illegal characters are found, they're treated as ' equivalent to "0" and the error flag DataIsValid is set to False. Dim PowTen As Integer Dim N As Byte Dim InByte As Byte PowTen = PowTenLookup(DigitCount) GetNumber = 0 For N = 1 To DigitCount InByte = GetByte ' Check for legal decimal digit. If (InByte >= 48) And (InByte <= 57) Then GetNumber = GetNumber + (CInt(InByte - 48) * PowTen) Else DataIsValid = False PowTen = PowTen \ 10 Next End Function Public Function GetSign( _ ByVal Value As Byte) As Integer ' This function reads a byte and returns a sign value that depends on the ' specified number Value. If the new byte matches, the sign is positive. ' Otherwise the sign is negative. If (GetByte = Value) Then GetSign = 1 Else GetSign = -1 End Function 10

11 Public Function GetByte() As Byte ' This function reads the next byte from the input device. If the byte matches ' InvalidTag, the flag DataIsValid is set to False. Const InvalidTag As Byte = 95 ' "_" GetByte = GetByteGPS If (GetByte = InvalidTag) Then DataIsValid = False End Function 11

12 Module GPSSerialPort Attribute VB_Name = "GPSSerialPort" Option Explicit Private Const InputQueueSize As Integer = 10 ' 1 byte buffer. Private Const OutputQueueSize As Integer = 10 ' 1-byte buffer. Private InputQueue(1 To InputQueueSize) As Byte Private OutputQueue(1 To OutputQueueSize) As Byte Private Const Baud As Long = 1200 Private Const NullOutputPin As Byte = 0 Public Sub OpenSerialPortGPS() Call OpenQueue(InputQueue, InputQueueSize) Call OpenQueue(OutputQueue, OutputQueueSize) If (PortNumber = 3) Then ' Inverted logic, no parity, 8 data bits. Call DefineCom3(SerialInputPin, NullOutputPin, bx1000_1000) Call OpenCom(PortNumber, Baud, InputQueue, OutputQueue) Public Function GetByteGPS() As Byte Dim Value As Byte Call GetQueue(InputQueue, Value, 1) GetByteGPS = Value End Function 12

13 Module GPSPinAssignments Attribute VB_Name = "GPSPinAssignments" Option Explicit ' This module defines port and I/O pin numbers. ' BX-01 assignments. '>>Public Const PortNumber As Byte = 2 '>>Public Const SerialInputPin As Byte = 12 ' BX-24 assignments. Public Const PortNumber As Byte = 3 Public Const SerialInputPin As Byte = 16 ' BX-35 assignments. '>>Public Const PortNumber As Byte = 3 '>>Public Const SerialInputPin As Byte = 16 13

14 Module Format3x3 Attribute VB_Name = "Format3x3" Option Explicit Public Function Fmt( _ ByVal Value As Single, _ ByVal Aft As Byte) As String ' Converts a float to a string with up to 3 places on each side of the ' decimal point. Legal range is Dim Sign As New BoundedString_1 Dim Temp As Single Dim itemp As Long Dim N As Byte Dim D(1 To 3) As Byte Const MinValue As Single = Const MaxValue As Single = ' Check for errors. If (Value < MinValue) Or (Value > MaxValue) Then GoTo Failure If (Aft < 0) Or (Aft > 3) Then GoTo Failure End if Temp = Abs(Value) ' Discard sign for now. ' Adjust scale. Select Case Aft Case 0 Temp = Temp Case 1 Temp = Temp * 10.0 Case 2 Temp = Temp * Case 3 Temp = Temp * End Select ' Round and convert to integer. itemp = FixL(Temp + 0.5) ' Find each digit, in reverse order. For N = 1 To Aft D(4 - N) = CByte(iTemp Mod 10) itemp = itemp \ 10 Next If (Value < 0.0) Then Sign = "-" Else Sign = "" 14

15 Select Case Aft Case 0 Fmt = Sign & CStr(iTemp) Exit Function Case 1 Fmt = Sign & CStr(iTemp) & "." _ & Chr(D(3) + 48) Exit Function Case 2 Fmt = Sign & CStr(iTemp) & "." _ & Chr(D(2) + 48) & Chr(D(3) + 48) Exit Function Case 3 Fmt = Sign & CStr(iTemp) & "." _ & Chr(D(1) + 48) & Chr(D(2) + 48) & Chr(D(3) + 48) Exit Function End Select Exit Function Failure: Fmt = "*" Exit Function End Function 2002 by NetMedia, Inc. All rights reserved. Basic Express, BasicX, BX-01, BX-24 and BX-35 are trademarks of NetMedia, Inc. GARMIN is a registered trademark and etrex is a trademark of GARMIN Corporation. All other trademarks are the property of their respective owners. 2.01A 15