Attribute VB_Name = "General"
'-----------------------------------------------------------
'Sample Code for 3DM-G
'MicroStrain, Inc.
'www.microstrain.com
'May 2003
'Built with VB6.0 SP5
'Built at 800 X 600 TrueColor(32-bit)
'-----------------------------------------------------------
'This sample code is written for RS-485 implementations.
'-----------------------------------------------------------
'Please refer to the Data Communication Protocol section of the
'3DM-G User Manual for detailed coding instructions.
'-----------------------------------------------------------
'Comments, suggestions and corrections to the code are welcomed
'as well as spiritual, philosophical and factual discourse concerning
'our world and beyond.
'-----------------------------------------------------------
'MicroStrain makes no warranty as to the design, capability, capacity
'or suitability of this code.
'-----------------------------------------------------------
'Version 1.0.0
'Barry, Developer
'-----------------------
'Add revisions here....
'
'MsgBox title
Public Const MBT = "3DM-G Sample Code"
'byte array
Public arrayByte() As Byte
'pi
Public Const PI As Double = 3.14159265
'constant parameter to maximize gyroscope resolution
Public Const GyroGainScale As Double = 64
'common pubvars
Public MyHeader As Double
Public TimerTicks As Double
Public MyCheckSum As Double
Public TheirCheckSum As Double
'matrix pubvars
Public MS11 As Double
Public MS21 As Double
Public MS31 As Double
Public MS12 As Double
Public MS22 As Double
Public MS32 As Double
Public MS13 As Double
Public MS23 As Double
Public MS33 As Double
'angle pubvars
Public MyPitch As Double
Public MyYaw As Double
Public MyRoll As Double
'quaternion pubvars
Public Quat0 As Double
Public Quat1 As Double
Public Quat2 As Double
Public Quat3 As Double
'vector pubvars
Public MagX As Double
Public MagY As Double
Public MagZ As Double
Public AccX As Double
Public AccY As Double
Public AccZ As Double
Public AngRateX As Double
Public AngRateY As Double
Public AngRateZ As Double
'sensor bits pubvars
Public RawMag1 As Double
Public RawMag2 As Double
Public RawMag3 As Double
Public RawAcc1 As Double
Public RawAcc2 As Double
Public RawAcc3 As Double
Public RawAngRate1 As Double
Public RawAngRate2 As Double
Public RawAngRate3 As Double
'temperature
Public MyTemp As Double
'firmware
Public Firmware As Double
Public MyFirmware As String
'serial number
Public MySerial As Double
'--------------------------
'for sampling timer
Public SampleTime As Date
'--------------------------

Public Function OpenCommPort(Temp As Control, CallScreen As Form)
'normal syntax
'Call OpenCommPort(MSComm1, Me)

If Temp.PortOpen = True Then
    'exit
    Exit Function
ElseIf Temp.PortOpen = False Then
    'set serial port
    Temp.CommPort = 1
    'set input mode to binary
    Temp.InputMode = 1
    ' set baud, parity, data, and stop bit
    Temp.Settings = "38400,n,8,1"
    'set the MSComm control to read entire buffer when input is used
    Temp.InputLen = 0
    'open port
    Temp.PortOpen = True
End If

End Function

Public Function CloseCommPort(Temp As Control, CallScreen As Form)
'normal syntax
'Call CloseCommPort(MSComm1, Me)

'close port
If Temp.PortOpen = True Then Temp.PortOpen = False

End Function

Public Function ApplySignRead(IncomingValue As Double) As Double
'Logic required to handle signed integer in VB
If IncomingValue > 32767 Then
    ApplySignRead = IncomingValue - 65536
Else
    ApplySignRead = IncomingValue
End If
End Function

Public Function ATAN2(X As Double, Y As Double) As Double
On Error GoTo ErrorHandler

'test for zeros
If X = 0 And Y = 0 Then
    'set return
    ATAN2 = 0
    'exit
    Exit Function
End If

'dims
Dim MyAngle As Double
Dim PI As Double
PI = 3.14159265

'test X and Y
If Abs(X) < Abs(Y) Then
    'calc arctangent
    MyAngle = Atn(Abs(X) / Abs(Y))
Else
    'calc arctangent
    MyAngle = (PI / 2) - Atn(Abs(Y) / Abs(X))
End If

'dims
Dim varOffset As Double
Dim varSign As Double

'test
If Y < 0 Then
    'set var
    varOffset = PI
Else
    'set var
    varOffset = 0
End If

'test using Xor
If X < 0 Xor Y < 0 Then
    'set
    varSign = 1
Else
    'set
    varSign = -1
End If
    
'set function return
ATAN2 = (varOffset - MyAngle) * varSign

Exit Function

ErrorHandler:
    'set return
    ATAN2 = 0
    'exit
    Exit Function

End Function

Public Function ApplySignWrite(IncomingValue As Double) As Double
'Logic required to handle signed integer in VB
If IncomingValue < 0 Then
    ApplySignWrite = 65536 + IncomingValue
Else
    ApplySignWrite = IncomingValue
End If
End Function
