frmcockpit.frm

一个低成本的捷联惯性导航系统设计程序

                TextAnalog(6).Text = Format(Idirection * c_r2g, "000.000")
                TextAnalog(5).Text = Format(Ipitch * c_r2g, "000.000")
                TextAnalog(4).Text = Format(Ibank * c_r2g, "000.000")
                TextAnzahl.Text = CStr(RecordCounter)
            End If
            ' Pointer zu juengstem Datensatz updaten (Laenge)
            iLoopCount = iLoopCount + 1
        Else
            iRetval% = NIDAQErrorHandler(iStatus%, "DAQ_DB_HalfReady", 0)
            ' Jetzt wird abwechselnd entweder der Kompass oder Horizont neu gezeichnet
            If whichDispUpdate = True Then
                Call frmArtHorizon.CalculateBar(Ibank, Ipitch)
                whichDispUpdate = False
            Else
                Call frmVirtCompass.CalculateArrow(-Idirection)
                whichDispUpdate = True
            End If
        End If
        DoEvents
    Wend
        
    ' Abbrechen
    iStatus% = DAQ_Clear(iDevice%)
    iStatus% = DAQ_DB_Config(iDevice%, iDBmodeOFF%)

    ' Timeouts Abschalten
    iStatus% = Timeout_Config(iDevice%, -1)
    
    ' Buttons:
    ButtonExit.Enabled = True
    ButtonStart.Enabled = True
    ButtonStop.Enabled = False
End Sub

Private Sub calculateOrientation()
    Static MittelPitch As Double
    Static MittelBank As Double
    Static MittelDirection As Double
    Static Mittelx, Mittely, Mittelz As Double
    Static MittelCounter As Integer
    Dim iterate As Long
    ' Integriere die Winkelgeschwindigkeit des ENC03
    If chkNull.Value = 1 Then
        ' Die "Null" - Checkbox ist gesetzt, d.h. solange werden nun die Messwerte
        ' gemittelt. Die Box sollte dazu horizontal stillstehen.
        MittelPitch = MittelPitch + MessArr(RecordCounter, cWhichGyroPitch)
        MittelBank = MittelBank + MessArr(RecordCounter, cWhichGyroBank)
        MittelDirection = MittelDirection + MessArr(RecordCounter, cWhichGyroDirection)
        Mittelx = Mittelx + MessArr(RecordCounter, 0)
        Mittely = Mittely + MessArr(RecordCounter, 1)
        Mittelz = Mittelz + MessArr(RecordCounter, 2)
        MittelCounter = MittelCounter + 1
        If MittelCounter > 0 Then
            NullTicksH = MittelBank / MittelCounter
            NullTicksV = MittelPitch / MittelCounter
            NullTicksD = MittelDirection / MittelCounter
            NullTicksX = Mittelx / MittelCounter
            NullTicksY = Mittely / MittelCounter
            NullTicksZ = Mittelz / MittelCounter - c_NullOffsetZ
        End If
    Else
        ' Also nicht Nullen, sondern "Navigieren".
        ' Zuerst Nullsetzen der Variablen fuer die Nullung
        MittelPitch = 0
        MittelBank = 0
        MittelDirection = 0
        Mittelx = 0
        Mittely = 0
        Mittelz = 0
        MittelCounter = 0
        
        ' Transformiere den Drehgeschwindigkeitsvektor in das OriginalKoordSystem
        Dim myVector As VectorType
        myVector.arot = (MessArr(RecordCounter, cWhichGyroBank) - NullTicksH) * cGyroSensBank
        myVector.brot = (MessArr(RecordCounter, cWhichGyroPitch) - NullTicksV) * cGyroSensPitch
        myVector.crot = (MessArr(RecordCounter, cWhichGyroDirection) - NullTicksD) * cGyroSensDirection
        myVector = f_koord_transform(myVector, Ibank, Ipitch, Idirection)
        
        ' Regle die Winkelgeschwindigkeiten
        If chkFeedback.Value = 1 Then
            myVector = f_reg_omega(myVector)
        End If
        ' Integriere die transformierten und geregelten Winkelgeschwindigkeiten:
        myVector.arot = f_int_omega(myVector.arot, Ibank)
        myVector.brot = f_int_omega(myVector.brot, Ipitch)
        myVector.crot = f_int_omega(myVector.crot, Idirection)
        ' Regle die Orientierung
        If chkFeedback.Value = 1 Then
            myVector = f_reg_phi(myVector)
        End If
        
        Ibank = myVector.arot
        Ipitch = myVector.brot
        Idirection = myVector.crot
        ' Um Overflows zu vermeiden, groebste Schranken fuer die Winkel (in rad!)
        If Abs(Ipitch) > 1000 Then
            Ipitch = 0
        End If
        If Abs(Ibank) > 1000 Then
            Ibank = 0
        End If
        If Abs(Idirection) > 1000 Then
            Idirection = 0
        End If
    End If
    PosCounter = RecordCounter
End Sub

Private Sub ButtonStop_Click()
    GoOnAquire = False
End Sub

Private Sub chkDirection_Click()
    If chkDirection.Value = 1 Then
        ' Aha, der graphische Kompass ist erwuenscht
        frmVirtCompass.Show
    Else
        ' ... nicht erwuenscht
        frmVirtCompass.Hide
    End If
End Sub

Private Sub chkFeedback_Click()
    ' Aufgestaute Integralanteile von f_reg_omega() muessen auch zurueckgesetzt werden
    iao = 0
    ibo = 0
    ico = 0
End Sub

Private Sub chkHorizont_Click()
    If chkHorizont.Value = 1 Then
        ' Aha, der graphische kuenstliche Horizont ist erwuenscht
        frmArtHorizon.Show
    Else
        ' ... nicht erwuenscht
        frmArtHorizon.Hide
    End If
End Sub

Private Sub cmdInitialize_Click()
    ' Der User will die Werte also zuruecksetzen
    If RecordCounter > 0 Then
        ' Falls wir schon mitten in einer Messung drin sind, Ausrichten nach g-Vektor
        Ibank = Atn((MessArr(RecordCounter - 1, 0) - NullTicksX) / (MessArr(RecordCounter - 1, 2) - NullTicksZ))
        Ipitch = Atn((MessArr(RecordCounter - 1, 1) - NullTicksY) / (MessArr(RecordCounter - 1, 2) - NullTicksZ))
        Idirection = 0 ' Wir haben ja keinen Kompass, also Laengsrichtung = Nord
    Else
        Ibank = 0
        Ipitch = 0
        Idirection = 0
    End If
    ' aufgestaute Integralanteile von f_reg_omega() muessen auch zurueckgesetzt werden
    iao = 0
    ibo = 0
    ico = 0
End Sub

Private Sub Form_Load()
    ' Enable und disable die richtigen Buttons:
    ButtonExit.Enabled = True
    ButtonStart.Enabled = True
    ButtonStop.Enabled = False
    
    ' Initialisierung der default Samplingparameter:
    S_Freq = 300
    S_bufferSize = 2048
    S_NrCh = 8
    S_gain = 2
    frmCockpit.Top = 100
    frmCockpit.Left = 100
End Sub

Private Sub mnuSampling_Click()
    frmConfig.Show
End Sub

Private Function f_int_omega(W As Double, oldint As Double) As Double
    ' "Integrieren der Drehgeschwindigkeit zum alten Wert"
    ' Im Moment nur Addition
    f_int_omega = oldint + W / S_Freq
End Function

Private Function f_reg_omega(W As VectorType) As VectorType
    ' Diese Funktion wird nach dem Temperaturkompensieren und dem
    ' Transformieren ins Referenzkoordinatensystem aufgerufen.
    ' Sie korrigiert die Drehgeschwindigkeiten nach gewissen Kriterien, z.B.
    ' wird die Drehgeschwindigkeit normalerweise nicht ewig konstant
    ' zunehmen (einzig moegliche Korrektur um z-Achse ohne Kompass).
    ' Oder es waere ein Abgleich nach dem g-Vektor denkbar. (Aufrichtung)
    
    ' Aufrichtlogik nach dem g-Vektor (siehe f_reg_phi())
    f_reg_omega.xtrans = W.xtrans
    f_reg_omega.ytrans = W.ytrans
    f_reg_omega.ztrans = W.ztrans
    f_reg_omega.arot = W.arot + iao * c_RegOmegaA
    f_reg_omega.brot = W.brot + ibo * c_RegOmegaB
    f_reg_omega.crot = W.crot
    'f_reg_omega = w
End Function

Private Function f_reg_phi(p As VectorType) As VectorType
    ' Diese Funktion wird nach dem Integrieren der Drehgeschwindigkeit
    ' aufgerufen. Sie korrigiert die erhaltene raeumliche Orientierung
    ' nach gewissen Kriterien, z.B. Aufrichtung nach g-Vektor
    
    ' Aufrichtung nach dem g-Vektor
    Static expa, expb, expc As Double
    Static alpha, beta As Double
    Static expaold, expbold, expcold As Double
    Static tempa, tempb As Double
    ' Berechnen des Lotes: Addition von 0.0001 damit keine Div by Zero (geht, da MessArr Integer)
    alpha = Atn((MessArr(RecordCounter, 0) - NullTicksX) / (MessArr(RecordCounter, 2) - NullTicksZ + 0.0000001))
    beta = Atn((MessArr(RecordCounter, 1) - NullTicksY) / (MessArr(RecordCounter, 2) - NullTicksZ + 0.0000001))
    ' Translatorische Koordinaten des VectorTypes unveraendert zurueckgeben
    f_reg_phi.xtrans = p.xtrans
    f_reg_phi.ytrans = p.ytrans
    f_reg_phi.ztrans = p.ztrans
    ' Floating point modulo, so dass tempa, tempb innerhalb +- 2Pi
    tempa = p.arot
    tempb = p.brot
    While (tempa > c_pi): tempa = tempa - 2 * c_pi: Wend
    While (tempa < -c_pi): tempa = tempa + 2 * c_pi: Wend
    If (Abs(alpha) < c_pi / 3) And (Abs(beta) < c_pi / 3) And ((MessArr(RecordCounter, 2) - NullTicksZ) > 0) Then
        ' Keine zu extremen Winkel (Lage) --> Regeln sinnvoll
        ' Alte geregelte Werte speichern
        expaold = expa
        expbold = expb
        ' Neue geregelte Werte berechnen
        expa = p.arot + Sgn(alpha - tempa) * c_RegPhiA / S_Freq
        expb = p.brot + Sgn(beta - tempb) * c_RegPhiB / S_Freq
        ' Die Geschwindigkeit wird nur bei einer Rotationsgeschwindigkeit von unter
        ' ca. 3 Grad = 0.05 rad
        If (Abs(expaold - expa) < 0.1 / S_Freq) And (Abs(expbold - expb) < 0.1 / S_Freq) Then
            ' Korrekturterm iao wird durch das Differential der Position erweitert.
            iao = iao - (expaold - expa)
            ' Dasselbe fuer ibo
            ibo = ibo - (expbold - expb)
            ' Rueckgabewert
            f_reg_phi.arot = expa
            f_reg_phi.brot = expb
            f_reg_phi.crot = p.crot     ' Richtung kann ohne Kompass nicht geregelt werden
        Else
            ' Rueckgabewert ungeregelt
            f_reg_phi.arot = p.arot
            f_reg_phi.brot = p.brot
            f_reg_phi.crot = p.crot
        End If
    Else
        ' Nicht regeln, da Lage dazu unguenstig
        f_reg_phi.arot = p.arot
        f_reg_phi.brot = p.brot
        f_reg_phi.crot = p.crot
    End If
    'f_reg_phi = p
End Function

Private Function f_koord_transform(v As VectorType, a As Double, b As Double, c As Double) As VectorType
    ' Diese Funktion transformiert die translatorischen
    ' Koordinaten (position, geschwindigkeit, beschleunigung)
    ' in das Ursprungskoordinatg ensystem.
    ' Optimierungsmoeglichkeit: statische Variablen fuer Terme wie six*siy*coz-cox*siy
    Static cosa, cosb, cosc As Double     ' Cosinus von rot*
    Static sina, sinb, sinc As Double     ' Sinus von rot*
    Static Vector As VectorType        ' Resultatevektor
    ' Berechnen der Sinuesser und Cosinuesser
    cosa = Cos(a)
    cosb = Cos(b)
    cosc = Cos(c)
    sina = Sin(a)
    sinb = Sin(b)
    sinc = Sin(c)
    ' transformationen der Rotatorischen Komponenten
    ' Der Rotationsvektor um die Querachse (Winkel b) muss erst noch parallel zu x-y-Ebene transformiert werden
    ' (um den Winkel c um die y-Achse des Geraetes = Laengsachse)
    Vector.brot = (cosa) * v.brot + (sina) * v.crot
    ' Der Rotationsvektor um die Laengsachse bleibt sich gleich
    Vector.arot = v.arot + ((sina) * v.brot - cosa * v.crot) * Tan(b)
    ' Der Rotationsvektoranteil um die Hochachse (parallel zu z-Achse)
    Vector.crot = ((cosa) * v.crot - (sina) * v.brot) / cosb
    f_koord_transform = Vector
End Function