| Objectifs : |
Dans l'idée d'utiliser un PC relié à un GPS pour faire un pilote automatique performant de bateau je me suis heurté au problème de certains GPS qui n'affiche plus le cap alors que la position continue de changer. En effet sur un GPS G-Space le cap ne s'affiche plus en dessous de 2.7 noeuds alors que la position ne change plus sous 1.2 noeuds.
Mes recherches ce sont donc portées à calculer le cap suivant la position et à resortir mes vieux cahiers de math, de navigation et beaucoup de recherche sur internet pour construire cette formule :
C=ACOS(SIN(radN1)*SIN(radN2)+COS(radN1)*COS(radN2)*COS(radE1-radE2)))
COScap=(COS(a)-(COS(b)*COS(C)))/(SIN(b)*SIN(c))
Ac=ACOS(COScap)
cap=Ac*57.29577951
Cette formule est utilisée a l'envers: on considaire que l'on est à une position (N1,E1) et que l'on se rend à une autre position (N2,E2) alors quand fait on retient la position ou l'on était (N1,E1) et que l'on calcul quand on est rendu à l'autre position (N2,E2).
Pour tester cette formule j'ai fait un programme en VB6 (voir la source plus bas) et retranscrit la formule pour effectuer le calcule, converti la source en visual studio .net pour la re écrire en visual studio .net pour pocket pc afin de la tester avec un GPS et MIRACLE ..... CA MARCHE!!!!!!!! 
Dans les programme en VB6 et .NET il faut entrer les positions manuellement en dégrés décimaux, dans la version pocket PC les positions sont mise à jour via un GPS.
Je vous fait ici, profiter des calcules, de la source en VB6 et .net ainsi que la source et le programme pour pocket PC. |
 |
| Calcule de la distance et du cap à suivre entre 2 points GPS : |
| Introduction : |
 |
Le voyageur, muni d'une boussole, se trouve au centre du cercle, à la croisée des vecteurs rouge et noir, au point de Coordonnées (N1,E1), et il doit se rendre au point de coordonnées (N2,E2) , ou sa destination.
A cause de la sphéricité de la terre, le cap va changer tout au long de son chemin: pour le vérifier, on peut tendre un fil qui matérialise l'orthodromie entre deux points "éloignés" sur un globe: le cap varie bien en cours de route, il faut donc le recalculer au fur et à mesure de son avancement : cette variation est faible pour de courtes distances, elle augmente quand on traverse plusieurs fuseaux horaires. La raison est que le cap est une fonction des éléments N1,E1,N2 et E2, et si la destination (N2,E2) ne varie pas, l'endroit où l'on se situe (N1,E1), évolue sans cesse. Un G.P.S. calcule le cap en temps réel, il le réactualise, car il connaît notre position, qui peut varier, en raison de notre déplacement, guidé par l'indication (ou le calcul ) du cap. |
CONVENTION:
On compte POSITIVEMENT: les longitudes vers l' EST, les Latitudes vers le NORD.
les longitudes vers l' OUEST, les Latitudes vers le SUD.
1' = 1 minute d'arc ; 1" = 1 seconde.
CONVERSION: j'ai pris pour référence: 40 000 km = 21600 nautical miles; en effet, il y a 21600' d'arc pour 360° qui représentent la circonférence du globe terrestre; 1' représente 1 mile marin, soit : 1 mile marin = 40 000 / 21 600 = 1,85185 km ( 1" équivaut environ à 100 pieds )
RAPPEL: 360° = 360 x 60' = 21600' (circonférence de la terre = 40 000 km)
Le "nautical mile" est le mile marin 100 pieds = 30,48 m
L'équivalence entre: angle et distance ( 1'= 1 mile et 1"= environ 100 pieds ) n'est obtenue que si la mesure de l'angle est effectuée au niveau des points de départ et d'arrivée. On pourra vérifier que 1° de longitude au niveau de l'équateur correspond à 60 miles , alors que ce même écart de 1° vers le 48 ème parallèle, correspondra à une distance plus courte : environ 40 miles : les méridiens sont plus rapprochés vers les pôles, et un angle séparant deux points sur le globe, doit correspondre à un arc de grand cercle, ayant pour centre : le centre de la sphère terrestre, ET passant par ces deux points :
1 ) le point de départ, de coordonées (N1,E1)
2 ) le point d'arrivée, de coordonées (N2,E2)
|
| Données GPS : |
Pour pouvoir calculer la distance et le cap à suivre entre N1,E1/N2,E2 on va récupérer les positions via la trame GPRMC sous la forme : "$GPRMC,225446,A,474743.1,N,041637.1,W,000.5,054.7,191194,020.3,E*68". Pour le programme .net sur le pocket pc on va transformer la position pour l'avoir en degré,minute,seconde par ((tableau(5) - 100 * (Math.Floor(tableau(5) / 100))) / 100 / 60 * 100) + (Math.Floor(tableau(5) / 100)) (tableau 5 étant la latitude en position 5 dans la trame) ce qui va nous donner 47°47'43.1" pour la reconvertir en degrés décimaux par (47+(47/60)+(43.1/3600)) = 47.7953 puis la transformer en radians : 47.79.53/57,29577951= 0.834185 .
Dans le programme en visual basic 6 on utilisera que des dégrés décimaux (47.7953) que l'on convertira en radian. |
| La formule : |
|
On prend comme position de départ la position de mon bateau au port du Guilvinec : N1=47°47'43.1"N E1=04°16'37.1"O
On prend comme position d'arrivée la position de la sortie du port du Guilvinec : N2=47°47'28.2" N E04°17'05.6"O
On converti ces positions en dégrés décimaux :
Formulation générale : latitude/ longitude en degrés décimaux
= degrés + (minutes / 60) + (secondes / 3600)
Nos position en degrés décimaux : N1=47.7954N E1=-3.7230O et N2=47.7913N E2=-3.7151O
On converti nos positions en radians :
1 radian=180/pi°=57.29577951=R1
RadN1=47.7954/R1 = 0.834185
RadE1=-3.72300/R1 = -0.064979
RadN2=47.7913/R1 = 0.834113
RadE2=-3.7151/R1 = -0.064841
Calcule de la distance entre le point de départ N1/E1 et le point d'arrivée N2/E2:
DKM=R1 * (1000/9) * ArcCos ((sin(RadN1) * (sin(RadN2)) + (cos(RadN1) * cos(RadN2) * cos(RadE1 - RadE2))
DKM = 0.721 soit 721 mètres
Calcule du cap entre le point de départ N1/E1 et le point d'arrivée N2/E2:
Calcules intermédiaires:
N Pôles = PI/2= 1,570796327
a=N pôles - RadN2
b=Npôles -RadN1
C=ACOS(SIN(radN1)*SIN(radN2)+COS(radN1)*COS(radN2)*COS(radE1-radE2)))
COScap=(COS(a)-(COS(b)*COS(C)))/(SIN(b)*SIN(c))
Ac=ACOS(COScap)
cap=Ac*R1
cap = 230,866621611074°
Reste à savoir le sens nord/sud:
Si E1 < 0 et E2 < 0
Et que If E1 < E2 Alors newcap = 360 - captempo
Si E1 > 0 et E2 > 0
Et qu E1 > E2 Alors newcap = 360 - captempo
Si E1 > 0 et E2 < 0
Alors newcap = 360 - captempo
Si E1 = E2 et N2 > N1 = 0
Si E1 = E2 et N2 < N1 = 180
Si E1 = E2 et N2 = N1 = 0
Si E1 < E2 = 360 - cap
|
|
| Le programme en Visual Basic 6 (VB6) : |
Il faut entrer les positions manuellement en degrés décimaux.
La fonction ArcCos n'existant pas sous VB6 ,il faut créer une fonction pour (voir plus bas).
| Déclaration des variables : |
Dim N1 As String 'latitude départ
Dim E1 As String 'longitude départ
Dim N2 As String 'latitude arrivée
Dim E2 As String 'longitude arrivée
Dim radN1 As String 'radian latitude départ
Dim rade1 As String 'radian longitude départ
Dim radN2 As String 'radian latitude arrivée
Dim rade2 As String 'radian longitude arrivée
Dim r1 As String 'radian en degrés (constante :57.29577951)
Dim dkm As String 'distance en kilometre
Dim captempo As String 'calcule un cap temporaire sans correction N/S ou O/E
Dim newcap As String ' captempo avec corrections
Dim npole As String 'nord polaire (constante :1.570796327)
Dim a As String
Dim b As String
Dim c As String
Dim coscap As String ' nombres de radians
Dim Ac As String 'arccos de coscap |
| La fonction pour calculer la distance entre N1, E1/N2,E2 : appelle de la fonction : Text5.Text = distance(N1, E1, N2, E2) |
Public Function distance(ByVal N1 As String, ByVal E1 As String, ByVal N2 As String, ByVal E2 As String)
r1 = 57.29577951 'radian en degrés
radN1 = N1 / r1
rade1 = E1 / r1
radN2 = N2 / r1
rade2 = E2 / r1
dkm = r1 * (1000 / 9) * Arccos((Sin(radN1) * Sin(radN2)) + (Cos(radN1) * Cos(radN2) * Cos(rade1 - rade2)))
distance = dkm
End Function |
| La fonction pour calculer le cap entre N1, E1/N2,E2 : appelle de la fonction : captempo = cap(N1, E1, N2, E2) |
Public Function cap(ByVal N1 As String, ByVal E1 As String, ByVal N2 As String, ByVal E2 As String)
On Error Resume Next
r1 = 57.29577951 'radian en degrés
radN1 = N1 / r1
rade1 = E1 / r1
radN2 = N2 / r1
rade2 = E2 / r1
npole = 1.570796327
a = npole - radN2
b = npole - radN1
c = Arccos(Sin(radN1) * Sin(radN2) + (Cos(radN1) * Cos(radN2) * Cos(rade1 - rade2)))
coscap = (Cos(a) - (Cos(b) * Cos(c))) / (Sin(b) * Sin(c))
Ac = Arccos(coscap)
cap = Ac * r1
End Function |
| La fonction pour calculer ArcCos : appelle de la fonction : arccos(X) |
|
Function Arccos(X) As Double
If Round(X, 8) = 1# Then Arccos = 0#: Exit Function
If Round(X, 8) = -1# Then Arccos = PI: Exit Function
Arccos = Atn(-X / Sqr(-X * X + 1)) + 2 * Atn(1)
End Function
|
| Téléchargement de la source en VB6 : |
 |
|
| Le programme en Visual studio 2008 .net : |
Il faut entrer les positions manuellement en degrés décimaux.
La fonction ArcCos n'existant pas sous visual studio .net ,il faut créer une fonction pour (voir plus bas).
| Déclaration des variables : |
Inherits System.Windows.Forms.Form
Dim N1 As String 'latitude départ
Dim E1 As String 'longitude départ
Dim N2 As String 'latitude arrivée
Dim E2 As String 'longitude arrivée
Dim radN1 As String 'radian latitude départ
Dim rade1 As String 'radian longitude départ
Dim radN2 As String 'radian latitude arrivée
Dim rade2 As String 'radian longitude arrivée
Dim r1 As String 'radian en degrés (constante :57.29577951)
Dim dkm As String 'distance en kilometre
Dim captempo As String 'calcule un cap temporaire sans correction N/S ou O/E
Dim newcap As String ' captempo avec corrections
Dim npole As String 'nord polaire (constante :1.570796327)
Dim a As String
Dim b As String
Dim c As String
Dim coscap As String ' nombres de radians
Dim Ac As String 'arccos de coscap |
| La fonction pour calculer la distance entre N1, E1/N2,E2 : appelle de la fonction : Text5.Text = distance(N1, E1, N2, E2) |
Public Function distance(ByVal N1 As String, ByVal E1 As String, ByVal N2 As String, ByVal E2 As String) As Object
'On Error Resume Next
r1 = CStr(57.29577951) 'radian en degrés
radN1 = CStr(CDbl(N1) / CDbl(r1))
rade1 = CStr(CDbl(E1) / CDbl(r1))
radN2 = CStr(CDbl(N2) / CDbl(r1))
rade2 = CStr(CDbl(E2) / CDbl(r1))
dkm = CStr(CDbl(r1) * (1000 / 9) * Arccos((System.Math.Sin(CDbl(radN1)) * System.Math.Sin(CDbl(radN2))) + (System.Math.Cos(CDbl(radN1)) * System.Math.Cos(CDbl(radN2)) * System.Math.Cos(CDbl(rade1) - CDbl(rade2)))))
distance = dkm
End Function |
| La fonction pour calculer le cap entre N1, E1/N2,E2 : appelle de la fonction : captempo = cap(N1, E1, N2, E2) |
Public Function cap(ByVal N1 As String, ByVal E1 As String, ByVal N2 As String, ByVal E2 As String) As Object
Dim coscap As Object
On Error Resume Next
r1 = CStr(57.29577951) 'radian en degrés
radN1 = CStr(CDbl(N1) / CDbl(r1))
rade1 = CStr(CDbl(E1) / CDbl(r1))
radN2 = CStr(CDbl(N2) / CDbl(r1))
rade2 = CStr(CDbl(E2) / CDbl(r1))
npole = CStr(1.570796327)
a = CStr(CDbl(npole) - CDbl(radN2))
b = CStr(CDbl(npole) - CDbl(radN1))
c = CStr(Arccos(System.Math.Sin(CDbl(radN1)) * System.Math.Sin(CDbl(radN2)) + (System.Math.Cos(CDbl(radN1)) * System.Math.Cos(CDbl(radN2)) * System.Math.Cos(CDbl(rade1) - CDbl(rade2)))))
coscap = (System.Math.Cos(CDbl(a)) - (System.Math.Cos(CDbl(b)) * System.Math.Cos(CDbl(c)))) / (System.Math.Sin(CDbl(b)) * System.Math.Sin(CDbl(c)))
Ac = CStr(Arccos(coscap))
cap = CDbl(Ac) * CDbl(r1)
End Function |
| La fonction pour calculer ArcCos : appelle de la fonction : arccos(X) |
|
Function Arccos(ByRef X As Object) As Double
Dim PI As Object
If System.Math.Round(X, 8) = 1# Then Arccos = 0# : Exit Function
If System.Math.Round(X, 8) = -1# Then
Arccos = PI : Exit Function
End If
Arccos = System.Math.Atan(-X / System.Math.Sqrt(-X * X + 1)) + 2 * System.Math.Atan(1)
End Function
|
| Téléchargement de la source en Visual Studio 2008 .NET: |
|
|
| Le programme pour pocket PC sous visual studio 2008 Smart Pocket : |
Essais effectués sur un pocket PC Dell Axim X51 v:
Capture de gauche : capture du pocket Dell en fonctionnement
Capture de droite : capture de form1.vb Design de visual studio |
Dell Axim X51v
|
Visual studio Form1.vb
 |
Copier sur le pocket PC les fichiers AutoPilotPDA.exe et autop.xml.
Lancer AutPilotPDA.exe, cliquer sur "Connection" puis "Configuration" puis "GPS", sélectionner le port du GPS. Fermer la frame de configuration.
La connexion au GPS ce fait par le composant SerialPort1.
Cliquer sur "Connection" puis sur Connecter" pour connecter le port com, le programme démarre, les trames GPS doivent défiler dans la ListBox1.
- Label1 : cap venant de la trame RMC (GPS)
- Label2 : Vitesse venant de la trame RMC (GPS)
- ListBox1 : Déffilement des trames venant du GPS
- Label3 : distance en KMS venant de la formule
- Label4 : cap venant de la formule
La capture d'écran a été effectuée en marchant et il est difficile de garder le cap. La différence sur la capture d'écran entre le cap venant de la trame GPS (Label1) et le calcule (Label3) vient que le calcule n'a pas été effectué juste en même temps que la mise à jour de la trame GPS. Cette différence est nul si on ce trouve en voiture ou en vélo.
Le calcule est effectué via le timer1 toute les 1 seconde.
Avant de quitter le programme penser à libérer le port com en cliquant sur "Déconnecter". |
|
| Téléchargement de la source pour pocket PC et du programme : |
|
|
|
|
