729 lines
34 KiB
C
729 lines
34 KiB
C
//**********************************************************************************************************************************************************************************
|
|
// Fichier G_COMMANDES.C
|
|
//
|
|
// Contient toutes les fonctions liées aux commandes envoyées et reçues sur G_BUS
|
|
//**********************************************************************************************************************************************************************************/
|
|
|
|
|
|
//#define __DEBUG_G_COMMANDES_GENERALES //Décommenter cette ligne pour avoir les messages de debug concernant les commandes générales du GNSS
|
|
//#define __DEBUG_G_COMMANDE_NAV
|
|
|
|
|
|
// Includes :
|
|
#include "main.h"
|
|
|
|
// Prototypes :
|
|
int8_t g_bus_down_commande_libre(uint8_t classe, uint8_t id, uint8_t * ptr_cde, uint16_t lg_cde, uint8_t mode_protocole, uint16_t time_out, int8_t max_repetitions);
|
|
int8_t g_bus_down_analyse_reponse_commande(void);
|
|
|
|
|
|
// Variables globales :
|
|
|
|
|
|
|
|
|
|
|
|
/*****************************************************************************************************************************************************************
|
|
Fonction : Commande libre formattée et envoyée vers le gestionnairer d'émission du protocole
|
|
Vérifie que le buffer de commandes à émettre sur G-Bus a un emplacement de libre.
|
|
Si oui, génère la commande correspondante dans le buffer de commandes et met à jour la longueur de la commande ainsi que
|
|
sa priorité.
|
|
Reçoit : La classe et l'ID de la commande, un pointeur sur le début de la commande (la chaine n'a PAS à se terminer par NULL), la longueur de la commande,
|
|
le mode protocole, la durée du time-out s'il doit être particulier (en x100ms) et le nombre maxi de répétitions (-1 si valeur par défaut).
|
|
Renvoit : 0 si aucune erreur, -1 si la commande n'a pas pu être générée et stockée dans le buffer de commandes.
|
|
******************************************************************************************************************************************************************/
|
|
int8_t g_bus_down_commande_libre(uint8_t classe, uint8_t id, uint8_t * ptr_cde, uint16_t lg_cde, uint8_t mode_protocole, uint16_t time_out, int8_t max_repetitions)
|
|
{
|
|
uint16_t i;
|
|
uint8_t priorite;
|
|
struct Struct_commande_g_bus *buffer_cde;
|
|
char tmp[MAX_LONGUEUR_BUFFER_TEMPORAIRE_DEBUG];
|
|
|
|
|
|
// Vérification des paramètres reçus :
|
|
if (mode_protocole != G_BUS_PROTOCOLE_NORMAL) return(-1);
|
|
|
|
if (lg_cde >= G_BUS_MAX_TAILLE_CDE) return(-1);
|
|
|
|
// Préparation de la commande :
|
|
priorite = PRIORITE_CDE_G_BUS_FAIBLE;
|
|
|
|
if (g_bus_next_write_buffer_cdes(priorite) != -1) // On recherche le prochain slot de commande libre
|
|
{
|
|
g_bus.nb_refus_buffer = 0; // RAZ du compteur à chaque réussite
|
|
|
|
buffer_cde = &g_bus.down.buffer_cdes[g_bus.down.ptr_fin_buffer_cdes]; // L'utilisation du pointeur de struct simplifie l'écriture ensuite...
|
|
buffer_cde->lg_cde = 0;
|
|
|
|
// Chargement de la classe et de l'ID :
|
|
buffer_cde->cde[buffer_cde->lg_cde ++] = classe;
|
|
buffer_cde->cde[buffer_cde->lg_cde ++] = id;
|
|
|
|
// Chargement de la longueur :
|
|
buffer_cde->cde[buffer_cde->lg_cde ++] = (uint8_t)(lg_cde & 0x00FF); // LSB
|
|
buffer_cde->cde[buffer_cde->lg_cde ++] = (uint8_t)(lg_cde >> 8); // MSB
|
|
|
|
// Chargement de la commande proprement-dite :
|
|
for (i = 0; i < lg_cde; i ++)
|
|
{
|
|
buffer_cde->cde[buffer_cde->lg_cde ++] = *(ptr_cde ++);
|
|
}
|
|
|
|
// Code de la commande :
|
|
buffer_cde->code = (classe << 8) | id; // On obtient un code unique sur 16 bits en concatenant la classe et l'ID (simplifie la recherche directe ensuite)
|
|
|
|
// Priorité de la commande :
|
|
buffer_cde->priorite = priorite;
|
|
|
|
// Type de réponse attendue :
|
|
buffer_cde->mode_protocole = mode_protocole;
|
|
|
|
// Time-out à utiliser pour la commande :
|
|
if (time_out > 0) buffer_cde->time_out = time_out;
|
|
else buffer_cde->time_out = G_BUS_TIME_OUT_REPONSE_TX;
|
|
|
|
// Nombre max de répétitions :
|
|
if (max_repetitions >= 0) buffer_cde->max_repetitions = max_repetitions;
|
|
else buffer_cde->max_repetitions = G_BUS_PROTOCOLE_TX_MAX_REPETITIONS;
|
|
|
|
return(0);
|
|
}
|
|
else
|
|
{
|
|
if (++ g_bus.nb_refus_buffer > G_BUS_MAX_REFUS_BUFFER_CDES_PLEIN) // Si les refus de nouvelle commandes sur M-Bus augmentent, on va réactiver le modem car il y a un sérieux problème...
|
|
{
|
|
sprintf(tmp, "Le nombre de refus de nouvelle commande dans le buffer de G-Bus est trop important (%i). Le récepteur GNSS va être réactivé", g_bus.nb_refus_buffer);
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_ERREUR);
|
|
|
|
// Enregistrement dans l'historique :
|
|
// enregistrement_historique(TYPE_SOURCE_HISTORIQUE_SYSTEME, SYSTEME_MODEM_ERREUR_G_BUS_PLEIN, 0, g_bus.nb_refus_buffer, NULL);
|
|
|
|
// modem.sequenceur.etat = ETAT_SEQUENCEUR_MODEM_DEBUT_IMPULSION_RESET; // Déclenche une impulsion >12s de reset sur POWER_KEY
|
|
}
|
|
return (-1); // Si aucun slot n'est disponible, on laisse tomber la commande...
|
|
}
|
|
}
|
|
|
|
|
|
/*****************************************************************************************************************************************************************
|
|
Fonction : Analyse la réponse à la commande et effectue, si besoin, le traitement nécessaire.
|
|
Le principe est de d'abord de détecter la présence d'un éventuel code d'erreur dans la réponse et de stocker son code.
|
|
Ensuite, on saute à la fonction (en pratique, un case d'un grand switch) correspondant à la commande qui avait été émise et donc qui déclenche
|
|
cette réponse. Là, on peut avoir les grandes tâches suivantes :
|
|
1- cas où on vient de recevoir un code d'erreur (y compris OK qui est considéré comme l'un d'eux) : dans ce cas, on décide ce qu'on doit faire
|
|
de ce code dans ce contexte. Souvent, il s'agit de ne pas tenir compte d'un OK intempestif (poru cela, on écrase l'erreur par G_BUS_ERREUR_AUCUNE).
|
|
2- On effectue le traitement de la trame reçue, selon la fonction émise et le contexte.
|
|
3- si on considère que le traitement est terminé, on écrase le code d'erreur pour le mettre à G_BUS_ERREUR_OK. Cette phase 3 est inutile pour pas
|
|
mal de commandes simples car leur trame de retour est suivie d'une trame OK qui libère le protocole tout seul.
|
|
En toute fin de fonction, on traite le code d'erreur subsistant. S'il est G_BUS_ERREUR_AUCUNE, on ne fait rien. S'il est G_BUS_ERREUR_OK on remet
|
|
le protocole au repos.
|
|
Reçoit : Rien (traite la réponse qui est stockée dans la structure g_bus.down.reponse.xxxx
|
|
Renvoit : 0 si aucune erreur, -1 si l'analyse de la réponse a rencontré une erreur.
|
|
******************************************************************************************************************************************************************/
|
|
int8_t g_bus_down_analyse_reponse_commande(void)
|
|
{
|
|
uint8_t erreur;
|
|
uint16_t i, j;
|
|
char tmp[MAX_LONGUEUR_BUFFER_TEMPORAIRE_DEBUG];
|
|
|
|
|
|
erreur = G_COMMANDE_ERREUR_AUCUNE;
|
|
|
|
switch(g_bus.down.reponse.classe)
|
|
{
|
|
//===============================================
|
|
case G_COMMANDE_CLASSE_ACK :
|
|
{
|
|
switch(g_bus.down.reponse.id)
|
|
{
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_ACK_NACK :
|
|
{
|
|
erreur = G_COMMANDE_ERREUR_NACK;
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_ACK_ACK :
|
|
{
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
default :
|
|
{
|
|
erreur = G_COMMANDE_ERREUR_ID;
|
|
break;
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//===============================================
|
|
case G_COMMANDE_CLASSE_CFG :
|
|
{
|
|
switch(g_bus.down.reponse.id)
|
|
{
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_CFG_PRT :
|
|
{
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_CFG_PM2 :
|
|
{
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_CFG_PMS :
|
|
{
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
default :
|
|
{
|
|
erreur = G_COMMANDE_ERREUR_ID;
|
|
break;
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//===============================================
|
|
case G_COMMANDE_CLASSE_MON :
|
|
{
|
|
switch(g_bus.down.reponse.id)
|
|
{
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_MON_VER :
|
|
{
|
|
if (g_bus.down.reponse.lg_reponse < 40) // Il y a normalement au minimum 40 octets en retour...
|
|
{
|
|
sprintf(tmp, "Retour de commande UBX_MON_VER en erreur : la longueur de la réponse [%i octets] est inférieure au minimum (40 octets)", g_bus.down.reponse.lg_reponse);
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_WARNING);
|
|
}
|
|
else
|
|
{
|
|
i = 0;
|
|
sprintf(tmp, "Retour de commande UBX_MON_VER :");
|
|
strcat(tmp, "\n\t"); // SwVersion (30 caractères)
|
|
strcat(tmp, (const char * restrict)(g_bus.down.reponse.reponse + i));
|
|
i += 30;
|
|
strcat(tmp, "\n\t"); // HwVersion (10 caractères)
|
|
strcat(tmp, (const char * restrict)(g_bus.down.reponse.reponse + i));
|
|
i += 10;
|
|
|
|
while (i < g_bus.down.reponse.lg_reponse)
|
|
{
|
|
strcat(tmp, "\n\t"); // Autre groupe (toujours 30 caractères)
|
|
strcat(tmp, (const char * restrict)(g_bus.down.reponse.reponse + i));
|
|
i += 30;
|
|
}
|
|
// affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_PAR_DEFAUT);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
default :
|
|
{
|
|
erreur = G_COMMANDE_ERREUR_ID;
|
|
break;
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//===============================================
|
|
case G_COMMANDE_CLASSE_NAV :
|
|
{
|
|
switch(g_bus.down.reponse.id)
|
|
{
|
|
//-----------------------------
|
|
case G_COMMANDE_UBX_NAV_PVT :
|
|
//-----------------------------
|
|
{
|
|
if (g_bus.down.reponse.lg_reponse != 92) // Longueur fixe...
|
|
{
|
|
sprintf(tmp, "Retour de commande G_COMMANDE_UBX_NAV_PVT en erreur : la longueur de la réponse [%i octets] est différente de la longueur attendue(92 octets)", g_bus.down.reponse.lg_reponse);
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_WARNING);
|
|
}
|
|
else
|
|
{
|
|
i = 0;
|
|
|
|
//----------------------------------------
|
|
// GPS time of week of the navigation epoch. :
|
|
i +=4; // Saut de iTOW car deprecated
|
|
|
|
|
|
struct Struct_date date;
|
|
struct Struct_heure heure;
|
|
//----------------------------------------
|
|
// Année :
|
|
date.annee = g_bus.down.reponse.reponse[i] + ((uint16_t)(g_bus.down.reponse.reponse[i+1]) << 8);
|
|
i += 2;
|
|
|
|
//----------------------------------------
|
|
// Mois :
|
|
date.mois = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Jour :
|
|
date.jour = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Heure :
|
|
heure.heures = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Minutes :
|
|
heure.minutes = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Secondes :
|
|
heure.secondes = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Validity flags :
|
|
uint8_t validity;
|
|
validity = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
bool flag_validity_valid_date = ((validity & 0x01) != 0) ? true : false; // 1 = valid UTC Date (see Time Validity section for details)
|
|
bool flag_validity_valid_time = ((validity & 0x02) != 0) ? true : false; // 1 = valid UTC time of day (see Time Validity section for details)
|
|
bool flag_validity_fully_resolved = ((validity & 0x04) != 0) ? true : false; // 1 = UTC time of day has been fully resolved (no seconds uncertainty). Cannot be used to check if time is completely solved.
|
|
bool flag_validity_valid_magnetic_declination = ((validity & 0x08) != 0) ? true : false; // 1 = valid magnetic declination
|
|
|
|
//----------------------------------------
|
|
// Time accuracy (ns) :
|
|
uint32_t time_accuracy;
|
|
time_accuracy = g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24);
|
|
i += 4;
|
|
|
|
//----------------------------------------
|
|
// Fraction of second (ns) :
|
|
int32_t nano;
|
|
nano = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
|
|
//----------------------------------------
|
|
// Fix type :
|
|
uint8_t fix_type;
|
|
fix_type = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Fix type flags :
|
|
uint8_t flags;
|
|
flags = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
bool flag_flags_gnss_fix_ok = ((flags & 0x01) != 0) ? true : false; // 1 = valid fix (i.e within DOP & accuracy masks)
|
|
bool flag_flags_differential_corrections_applied = ((flags & 0x02) != 0) ? true : false; // 1 = differential corrections were applied
|
|
bool flag_flags_heading_of_vehicle_valid = ((flags & 0x20) != 0) ? true : false; // 1 = heading of vehicle is valid, only set if the receiver is in sensor fusion mode
|
|
uint8_t flags_psm_state = (flags >> 2) & 0x07; // Pas de desciptif dans la doc...
|
|
uint8_t flags_carrier_phase_range_solution = (flags >> 6) & 0x03; // Carrier phase range solution status: 0: no carrier phase range solution, 1: carrier phase range solution with floating ambiguities, 2: carrier phase range solution with fixed ambiguities
|
|
|
|
//----------------------------------------
|
|
// Fix type flags 2 :
|
|
uint8_t flags2;
|
|
flags2 = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
bool flag_flags2_confirmed_available = ((flags2 & 0x20) != 0) ? true : false; // 1 = information about UTC Date and Time of Day validity confirmation is available (see Time Validity section for details)
|
|
bool flag_flags2_confirmed_date = ((flags2 & 0x40) != 0) ? true : false; // 1 = UTC Date validity could be confirmed (see Time Validity section for details)
|
|
bool flag_flags2_confirmed_time = ((flags2 & 0x80) != 0) ? true : false; // 1 = UTC Time of Day could be confirmed (see Time Validity section for details)
|
|
|
|
//----------------------------------------
|
|
// Number of satellites :
|
|
uint8_t siv;
|
|
siv = g_bus.down.reponse.reponse[i];
|
|
i += 1;
|
|
|
|
//----------------------------------------
|
|
// Longitude (deg) :
|
|
int32_t tmp_longitude;
|
|
float longitude;
|
|
tmp_longitude = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
longitude = ((float)tmp_longitude) / 10000000;
|
|
|
|
//----------------------------------------
|
|
// Latitude (deg) :
|
|
int32_t tmp_latitude;
|
|
float latitude;
|
|
tmp_latitude = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
latitude = ((float)tmp_latitude) / 10000000;
|
|
|
|
//----------------------------------------
|
|
// Height above ellipsoid (mm) :
|
|
int32_t tmp_height_ellipsoid;
|
|
float height_ellipsoid;
|
|
tmp_height_ellipsoid = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
height_ellipsoid = ((float)tmp_height_ellipsoid) / 1000;
|
|
|
|
//----------------------------------------
|
|
// Height above mean sea level (mm) :
|
|
int32_t tmp_height_sea;
|
|
float height_sea;
|
|
tmp_height_sea = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
height_sea = ((float)tmp_height_sea) / 1000;
|
|
|
|
//----------------------------------------
|
|
// Horizontal accuracy (mm) :
|
|
uint32_t tmp_horizontal_accuracy;
|
|
float horizontal_accuracy;
|
|
tmp_horizontal_accuracy = g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24);
|
|
i += 4;
|
|
horizontal_accuracy = ((float)tmp_horizontal_accuracy) / 1000;
|
|
|
|
//----------------------------------------
|
|
// Vertical accuracy (mm) :
|
|
uint32_t tmp_vertical_accuracy;
|
|
float vertical_accuracy;
|
|
tmp_vertical_accuracy = g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24);
|
|
i += 4;
|
|
vertical_accuracy = ((float)tmp_vertical_accuracy) / 1000;
|
|
|
|
//----------------------------------------
|
|
// NED north velocity (mm/s) :
|
|
int32_t tmp_ned_north_velocity;
|
|
float ned_north_velocity;
|
|
tmp_ned_north_velocity = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
// ned_north_velocity = (((float)tmp_ned_north_velocity) * 3.600) / 1000; // Conversion de 'mm/s' en 'km/h'
|
|
ned_north_velocity = (((float)tmp_ned_north_velocity) * 3.600) / 1852; // Conversion de 'mm/s' en 'Knts = noeuds'
|
|
|
|
//----------------------------------------
|
|
// NED east velocity (mm/s) :
|
|
int32_t tmp_ned_east_velocity;
|
|
float ned_east_velocity;
|
|
tmp_ned_east_velocity = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
// ned_east_velocity = (((float)tmp_ned_east_velocity) * 3.600) / 1000; // Conversion de 'mm/s' en 'km/h'
|
|
ned_east_velocity = (((float)tmp_ned_east_velocity) * 3.600) / 1852; // Conversion de 'mm/s' en 'Knts = noeuds'
|
|
|
|
|
|
//----------------------------------------
|
|
// NED down velocity (mm/s) :
|
|
int32_t tmp_ned_down_velocity;
|
|
float ned_down_velocity;
|
|
tmp_ned_down_velocity = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
// ned_down_velocity = (((float)tmp_ned_down_velocity) * 3.600) / 1000; // Conversion de 'mm/s' en 'km/h'
|
|
ned_down_velocity = (((float)tmp_ned_down_velocity) * 3.600) / 1852; // Conversion de 'mm/s' en 'Knts = noeuds'
|
|
|
|
//----------------------------------------
|
|
// Ground speed (mm/s) :
|
|
int32_t tmp_ground_speed;
|
|
float ground_speed;
|
|
tmp_ground_speed = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
// ground_speed = (((float)tmp_ground_speed) * 3.600) / 1000; // Conversion de 'mm/s' en 'km/h'
|
|
ground_speed = (((float)tmp_ground_speed) * 3.600) / 1852; // Conversion de 'mm/s' en 'Knts = noeuds'
|
|
|
|
//----------------------------------------
|
|
// Heading of motion (deg) :
|
|
int32_t tmp_heading_motion;
|
|
float heading_motion;
|
|
tmp_heading_motion = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
heading_motion = ((float)tmp_heading_motion) / 100000;
|
|
|
|
//----------------------------------------
|
|
// Speed accuracy estimate (mm/s):
|
|
uint32_t tmp_speed_accuracy;
|
|
float speed_accuracy;
|
|
tmp_speed_accuracy = g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24);
|
|
i += 4;
|
|
// speed_accuracy = (((float)tmp_speed_accuracy) * 3.600) / 1000; // Conversion de 'mm/s' en 'km/h'
|
|
speed_accuracy = (((float)tmp_speed_accuracy) * 3.600) / 1852; // Conversion de 'mm/s' en 'Knts = noeuds'
|
|
|
|
//----------------------------------------
|
|
// Heading accuracy estimate (deg):
|
|
uint32_t tmp_heading_accuracy;
|
|
float heading_accuracy;
|
|
tmp_heading_accuracy = g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24);
|
|
i += 4;
|
|
heading_accuracy = ((float)tmp_heading_accuracy) / 100000;
|
|
|
|
//----------------------------------------
|
|
// Position DOP :
|
|
uint16_t tmp_position_dop;
|
|
float position_dop;
|
|
tmp_position_dop = g_bus.down.reponse.reponse[i] + ((uint16_t)(g_bus.down.reponse.reponse[i+1]) << 8);
|
|
i += 2;
|
|
position_dop = ((float)tmp_position_dop) / 100;
|
|
|
|
//----------------------------------------
|
|
// Fix type flags 3 :
|
|
uint16_t flags3;
|
|
flags3 = g_bus.down.reponse.reponse[i] + ((uint16_t)(g_bus.down.reponse.reponse[i+1]) << 8);
|
|
i += 1;
|
|
bool flag_flags3_invalid_llh = ((flags3 & 0x01) != 0) ? true : false; // 1 = Invalid lon, lat, height and hMSL
|
|
uint8_t flags3_last_correction_age = (flags3 >> 1) & 0x0F; // Age of the most recently received differential correction: 0: Not available, 1: Age between 0 and 1 second, 2: Age between 1 (inclusive) and 2 seconds, 3: Age between 2 (inclusive) and 5 seconds, 4: Age between 5 (inclusive) and 10 seconds, 5: Age between 10 (inclusive) and 15 seconds, 6: Age between 15 (inclusive) and 20 seconds, 7: Age between 20 (inclusive) and 30 seconds, 8: Age between 30 (inclusive) and 45 seconds, 9: Age between 45 (inclusive) and 60 seconds, 10: Age between 60 (inclusive) and 90 seconds, 11: Age between 90 (inclusive) and 120 seconds, >=12: Age greater or equal than 120 seconds
|
|
|
|
//----------------------------------------
|
|
// Reserved (U1 x 4) :
|
|
i += 4;
|
|
|
|
//----------------------------------------
|
|
// Heading of vehicule (deg) :
|
|
int32_t tmp_heading_vehicule;
|
|
float heading_vehicule;
|
|
tmp_heading_vehicule = (int32_t) (g_bus.down.reponse.reponse[i] + ((uint32_t)(g_bus.down.reponse.reponse[i+1]) << 8) + ((uint32_t)(g_bus.down.reponse.reponse[i+2]) << 16) + ((uint32_t)(g_bus.down.reponse.reponse[i+3]) << 24));
|
|
i += 4;
|
|
heading_vehicule = ((float)tmp_heading_vehicule) / 100000;
|
|
|
|
//----------------------------------------
|
|
// Magnetic declination (deg) :
|
|
int16_t tmp_magnetic_declination;
|
|
float magnetic_declination;
|
|
tmp_magnetic_declination = (int16_t) (g_bus.down.reponse.reponse[i] + ((uint16_t)(g_bus.down.reponse.reponse[i+1]) << 8));
|
|
i += 2;
|
|
magnetic_declination = ((float)tmp_magnetic_declination) / 100;
|
|
|
|
//----------------------------------------
|
|
// Magnetic declination accuracy :
|
|
uint16_t tmp_magnetic_declination_accuracy;
|
|
float magnetic_declination_accuracy;
|
|
tmp_magnetic_declination_accuracy = g_bus.down.reponse.reponse[i] + ((uint16_t)(g_bus.down.reponse.reponse[i+1]) << 8);
|
|
i += 1;
|
|
magnetic_declination_accuracy = ((float)tmp_magnetic_declination_accuracy) / 100;
|
|
|
|
|
|
//-----------------------------------------
|
|
// TRAITEMENT :
|
|
//-----------------------------------------
|
|
|
|
if (flag_validity_valid_date && flag_validity_valid_time) // Ces deux flags indiquent que l'heure et la date sont valides.
|
|
{
|
|
if (gnss.sequenceur.compteur_time_out == 0) // On n'utiulise la date et heure valide que périodiquement car inutils sinon.
|
|
{
|
|
gnss.sequenceur.compteur_time_out = GNSS_PERIODE_SET_DATE_HEURE; // Rechargement de la tempo pour éviter les mises à jour chaque seconde...
|
|
set_date_heure(date, heure); // Mise à jour de la date/heure système à partir de l'heure reçue du GNSS (toujours en UTC !)
|
|
c_bus_down_commande_config_date_heure(); // Mise à jour de la date/heure du Compagnon dans la foulée (lui aussi, toujours en UTC)
|
|
#ifdef __DEBUG_G_COMMANDES_GENERALES
|
|
affichage_debug(__FILE__, __LINE__, "Une commande de mise à l'heure a été envoyée au Compagnon", TYPE_MESSAGE_DEBUG_PAR_DEFAUT);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
if (flag_flags_gnss_fix_ok) // C'est ce flag qui indique su la position est déterminée ou pas.
|
|
{
|
|
#ifdef __DEBUG_G_COMMANDES_GENERALES
|
|
affichage_debug(__FILE__, __LINE__, "L'aquisition de la position par le GNSS vient d'être faite avec succès", TYPE_MESSAGE_DEBUG_PAR_DEFAUT);
|
|
#endif
|
|
gnss.etat = ON;
|
|
|
|
// L'utilisation d'une boucle sur toutes les entrées Prédéfinies est plus efficace que plusieurs appels à 'no_entree_predefinie(0, ENTREE_PREDEFINIE_LAT)' puis test de validité de l'entrée puis lecture de la valeur...
|
|
for (j = 0; j < MAX_NOMBRE_ENTREES_PREDEFINIES; j ++)
|
|
{
|
|
if (!entrees.predefinies.entrees[j].flag_validite) continue;
|
|
if (entrees.predefinies.entrees[j].module != 0) continue; // On ne cherche que les entrées de ShipBase
|
|
|
|
switch(entrees.predefinies.entrees[j].no)
|
|
{
|
|
case ENTREE_PREDEFINIE_LAT: { entrees.predefinies.entrees[j].valeur = latitude; break; }
|
|
case ENTREE_PREDEFINIE_LON: { entrees.predefinies.entrees[j].valeur = longitude; break; }
|
|
case ENTREE_PREDEFINIE_ALT: { entrees.predefinies.entrees[j].valeur = height_sea; break; }
|
|
case ENTREE_PREDEFINIE_SOG: { entrees.predefinies.entrees[j].valeur = ground_speed; break; }
|
|
case ENTREE_PREDEFINIE_COG: { entrees.predefinies.entrees[j].valeur = heading_motion; break; }
|
|
case ENTREE_PREDEFINIE_SIV: { entrees.predefinies.entrees[j].valeur = siv; break; }
|
|
case ENTREE_PREDEFINIE_HPA: { entrees.predefinies.entrees[j].valeur = horizontal_accuracy; break; }
|
|
case ENTREE_PREDEFINIE_VPA: { entrees.predefinies.entrees[j].valeur = vertical_accuracy; break; }
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
#ifdef __DEBUG_G_COMMANDES_GENERALES
|
|
affichage_debug(__FILE__, __LINE__, "L'aquisition de la position par le GNSS n'est pas considérée comme valide (flag_flags_gnss_fix_ok = FALSE)", TYPE_MESSAGE_DEBUG_ERREUR);
|
|
#endif
|
|
gnss.etat = OFF;
|
|
|
|
// SDIR # 149 : Début
|
|
// L'utilisation d'une boucle sur toutes les entrées Prédéfinies est plus efficace que plusieurs appels à 'no_entree_predefinie(0, ENTREE_PREDEFINIE_LAT)' puis test de validité de l'entrée puis lecture de la valeur...
|
|
for (j = 0; j < MAX_NOMBRE_ENTREES_PREDEFINIES; j ++)
|
|
{
|
|
if (!entrees.predefinies.entrees[j].flag_validite) continue;
|
|
if (entrees.predefinies.entrees[j].module != 0) continue; // On ne cherche que les entrées de ShipBase
|
|
|
|
switch(entrees.predefinies.entrees[j].no)
|
|
{
|
|
// case ENTREE_PREDEFINIE_LAT: { entrees.predefinies.entrees[j].valeur = latitude; break; }
|
|
// case ENTREE_PREDEFINIE_LON: { entrees.predefinies.entrees[j].valeur = longitude; break; }
|
|
// case ENTREE_PREDEFINIE_ALT: { entrees.predefinies.entrees[j].valeur = height_sea; break; }
|
|
case ENTREE_PREDEFINIE_SOG: { entrees.predefinies.entrees[j].valeur = 0; break; }
|
|
case ENTREE_PREDEFINIE_COG: { entrees.predefinies.entrees[j].valeur = 0; break; }
|
|
case ENTREE_PREDEFINIE_SIV: { entrees.predefinies.entrees[j].valeur = 0; break; }
|
|
case ENTREE_PREDEFINIE_HPA: { entrees.predefinies.entrees[j].valeur = 0; break; }
|
|
case ENTREE_PREDEFINIE_VPA: { entrees.predefinies.entrees[j].valeur = 0; break; }
|
|
}
|
|
}
|
|
// SDIR # 149 : Fin
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// NB : les tabulations ci-dessous correspondent à un affichage oiptimal. Il est conseillé de ne pas y toucher...
|
|
sprintf(tmp, "Réponse GNSS :\n \
|
|
Date = %02i/%02i/%i %02i:%02i:%02i \n \
|
|
Validity flags : Valid_date = %s, Valid_time = %s, Fully_resolved = %s, valid_magnetic_declination = %s \n \
|
|
Time accuracy = %i ns \n \
|
|
Fraction of seconds = %i ns \n \
|
|
Fix type = %s \n \
|
|
Fix Status Flags : Gnss_fix_ok = %s, Differential_corrections_applied = %s, Heading_of_vehicle_valid = %s, Psm_state = %i, Carrier_phase_range_solution = %i \n \
|
|
Flags2 : Confirmed_available = %s, Confirmed_date = %s, Confirmed_time = %s \n \
|
|
Nb de satellites en vue = %i \n \
|
|
Longitude = %.8f° \n \
|
|
Latitude = %.8f° \n \
|
|
Height_ellipsoid = %.3f m \n \
|
|
Height_sea = %.3f m \n \
|
|
Horizontal_accuracy = %.3f m \n \
|
|
Vertical_accuracy = %.3f m \n \
|
|
Ned_north_velocity = %.3f knts \n \
|
|
Ned_east_velocity = %.3f knts \n \
|
|
Ned_down_velocity = %.3f knts \n \
|
|
Ground_speed = %.3f knts \n \
|
|
Heading_motion = %.5f° \n \
|
|
Speed_accuracy = %.3f knts \n \
|
|
Heading_accuracy = %.5f° \n \
|
|
Position_DOP = %.2f \n \
|
|
Flags3 : Invalid_llh = %s, flags3_last_correction_age = %i \n \
|
|
Heading_vehicule = %.5f \n \
|
|
Magnetic_declination = %.2f° \n \
|
|
Magnetic_declination_accur. = %.2f ",
|
|
date.jour, date.mois, date.annee, heure.heures, heure.minutes, heure.secondes,
|
|
flag_validity_valid_date ? "true" : "false", flag_validity_valid_time ? "true" : "false", flag_validity_fully_resolved ? "true" : "false", flag_validity_valid_magnetic_declination ? "true" : "false",
|
|
time_accuracy,
|
|
nano,
|
|
(fix_type == 0) ? "No fix" : ((fix_type == 1) ? "Dead Reckoning only" : ((fix_type == 2) ? "2D-Fix" : ((fix_type == 3) ? "3D-Fix" : ((fix_type == 4) ? "GNSS + Dead Reckoning combined" : "Time only fix")))),
|
|
flag_flags_gnss_fix_ok ? "true" : "false", flag_flags_differential_corrections_applied ? "true" : "false", flag_flags_heading_of_vehicle_valid ? "true" : "false", flags_psm_state, flags_carrier_phase_range_solution,
|
|
flag_flags2_confirmed_available ? "true" : "false", flag_flags2_confirmed_date ? "true" : "false", flag_flags2_confirmed_time ? "true" : "false",
|
|
siv,
|
|
longitude,
|
|
latitude,
|
|
height_ellipsoid,
|
|
height_sea,
|
|
horizontal_accuracy,
|
|
vertical_accuracy,
|
|
ned_north_velocity,
|
|
ned_east_velocity,
|
|
ned_down_velocity,
|
|
ground_speed,
|
|
heading_motion,
|
|
speed_accuracy,
|
|
heading_accuracy,
|
|
position_dop,
|
|
flag_flags3_invalid_llh ? "true" : "false", flags3_last_correction_age,
|
|
heading_vehicule,
|
|
magnetic_declination,
|
|
magnetic_declination_accuracy
|
|
);
|
|
#ifdef __DEBUG_G_COMMANDE_NAV // NB : on laisse faire le sprintf avant dans tous les cas pour éviter les warnings à la compilation...
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_PAR_DEFAUT);
|
|
#endif
|
|
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//-----------------------------
|
|
default :
|
|
{
|
|
erreur = G_COMMANDE_ERREUR_ID;
|
|
break;
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//===============================================
|
|
// Autres classes :
|
|
case G_COMMANDE_CLASSE_RXM :
|
|
case G_COMMANDE_CLASSE_INF :
|
|
case G_COMMANDE_CLASSE_UPD :
|
|
case G_COMMANDE_CLASSE_AID :
|
|
case G_COMMANDE_CLASSE_TIM :
|
|
case G_COMMANDE_CLASSE_ESF :
|
|
case G_COMMANDE_CLASSE_MGA :
|
|
case G_COMMANDE_CLASSE_LOG :
|
|
case G_COMMANDE_CLASSE_SEC :
|
|
case G_COMMANDE_CLASSE_HNR :
|
|
default:
|
|
{
|
|
erreur = G_COMMANDE_ERREUR_CLASSE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
switch(erreur)
|
|
{
|
|
//---------------------------------
|
|
case G_COMMANDE_ERREUR_AUCUNE :
|
|
{
|
|
g_bus.down.flag_attente_reponse = FALSE; // Libération du séquenceur d'émission pour passer à la commande suivante
|
|
g_bus.protocole_tx.repetitions = 0; // RAZ du nombre de répétitions pour usage futur
|
|
g_bus.protocole_tx.sequenceur.compteur_time_out = 0; // RAZ du time-out pour éviter tout déclenchement a postériori.
|
|
break;
|
|
}
|
|
|
|
//---------------------------------
|
|
case G_COMMANDE_ERREUR_CLASSE :
|
|
{
|
|
sprintf(tmp, "La réponse reçue sur G-Bus ne correspond à aucune classe connue [%02Xh]", g_bus.down.reponse.classe);
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_WARNING);
|
|
break;
|
|
}
|
|
|
|
//---------------------------------
|
|
case G_COMMANDE_ERREUR_ID :
|
|
{
|
|
sprintf(tmp, "La réponse reçue sur G-Bus dans la classe %02Xh ne correspond à aucun ID connu [%02Xh]", g_bus.down.reponse.classe, g_bus.down.reponse.id);
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_WARNING);
|
|
break;
|
|
}
|
|
|
|
//---------------------------------
|
|
case G_COMMANDE_ERREUR_NACK :
|
|
{
|
|
sprintf(tmp, "La réponse reçue sur G-Bus dans la classe [%02Xh] avec ID [%02Xh] est en erreur NACK", g_bus.down.reponse.classe, g_bus.down.reponse.id);
|
|
affichage_debug(__FILE__, __LINE__, tmp, TYPE_MESSAGE_DEBUG_WARNING);
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
g_bus.down.reponse.classe = 0; // Cette valeur semble n'être utilisée par aucune classe...
|
|
g_bus.down.reponse.lg_reponse = 0; // Repositionne le pointeur au début pour le traitement de la trame suivante. A faire dans tous les cas !!
|
|
|
|
|
|
return(0);
|
|
}
|
|
|
|
|
|
|