fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 20:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

The CFG-NAV5 dynamic model is now checked as well

Browse Source
This commit is contained in:
Julian Oes
2012-12-16 20:26:29 -08:00
parent 80eb66c7a3
commit dce9b2d045
2 changed files with 93 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/gps/ubx.c
+60 -2
View File
@@ -54,6 +54,9 @@
#define UBX_BUFFER_SIZE 1000
// Set dynamic model to 7: Airborne with <2g Acceleration
#define DYN_MODEL_NO 0x07
extern bool gps_mode_try_all;
extern bool gps_mode_success;
extern bool terminate_gps_thread;
@@ -65,6 +68,7 @@ pthread_mutex_t *ubx_mutex;
gps_bin_ubx_state_t *ubx_state;
static struct vehicle_gps_position_s *ubx_gps;
bool gps_nav5_conf_success = false;
//Definitions for ubx, last two bytes are checksum which is calculated below
uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0xB5 , 0x62 , 0x06 , 0x00 , 0x14 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0xD0 , 0x08 , 0x00 , 0x00 , 0x80 , 0x25 , 0x00 , 0x00 , 0x07 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00};
@@ -76,8 +80,8 @@ uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
// Set dynamic model to 7: Airborne with <2g Acceleration
uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, DYN_MODEL_NO, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL[] = {0xB5, 0x62, 0x06, 0x00, 0x00};
@@ -137,6 +141,10 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
ubx_state->message_class = RXM;
break;
case UBX_CLASS_CFG:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = CFG;
break;
default: //unknown class: reset state machine
ubx_decode_init();
break;
@@ -198,7 +206,19 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
ubx_decode_init();
break;
}
break;
case CFG:
switch (b) {
case UBX_MESSAGE_CFG_NAV5:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = CFG_NAV5;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
default: //should not happen
@@ -546,6 +566,36 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
break;
}
case CFG_NAV5: {
printf("GOT CFG_NAV5 MESSAGE\n");
type_gps_bin_cfg_nav5_packet_t *packet = (type_gps_bin_cfg_nav5_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) {
// check if dynamic model number is correct
if (packet->dynModel == DYN_MODEL_NO) {
printf("[gps] ubx dynamic model set successful\n");
gps_nav5_conf_success = true;
}
else {
printf("[gps] ubx dynamic model set failed\n");
gps_nav5_conf_success = false;
}
ret = 1;
} else {
if (gps_verbose) printf("[gps] CFG_NAV5: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
default: //something went wrong
ubx_decode_init();
@@ -626,6 +676,10 @@ int configure_gps_ubx(int *fd)
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
usleep(100000);
//send CFG_NAV5_POLL to check whether previous CFG_NAV5 has been successful
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL, sizeof(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL) / sizeof(uint8_t) , *fd);
usleep(100000);
return 0;
}
@@ -757,6 +811,10 @@ void *ubx_watchdog_loop(void *args)
}
}
// check if CFG-NAV5 is correct
if (!gps_nav5_conf_success)
all_okay = false;
pthread_mutex_unlock(ubx_mutex);
if (!all_okay) {
apps/gps/ubx.h
+33 -2
View File
@@ -72,6 +72,8 @@
#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_CFG_NAV5 0x24
// ************
@@ -234,6 +236,33 @@ typedef struct {
typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgId;
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint32_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_nav5_packet;
typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t;
// END the structures of the binary packets
// ************
@@ -242,7 +271,8 @@ enum UBX_MESSAGE_CLASSES {
CLASS_UNKNOWN = 0,
NAV = 1,
RXM = 2,
ACK = 3
ACK = 3,
CFG = 4
};
enum UBX_MESSAGE_IDS {
@@ -254,7 +284,8 @@ enum UBX_MESSAGE_IDS {
NAV_DOP = 4,
NAV_SVINFO = 5,
NAV_VELNED = 6,
RXM_SVSI = 7
RXM_SVSI = 7,
CFG_NAV5 = 8
};
enum UBX_DECODE_STATES {