diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-12-16 20:26:29 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-01-17 16:54:32 -0800 |
commit | dce9b2d0459b8dea5aa9e072a6914f0768f1867f (patch) | |
tree | 38c30788eb1c773668ab636a74ee030c1eeb7a49 | |
parent | 80eb66c7a3f70ccaa46b3d955808a10a222241df (diff) | |
download | px4-firmware-dce9b2d0459b8dea5aa9e072a6914f0768f1867f.tar.gz px4-firmware-dce9b2d0459b8dea5aa9e072a6914f0768f1867f.tar.bz2 px4-firmware-dce9b2d0459b8dea5aa9e072a6914f0768f1867f.zip |
The CFG-NAV5 dynamic model is now checked as well
-rw-r--r-- | apps/gps/ubx.c | 62 | ||||
-rw-r--r-- | apps/gps/ubx.h | 35 |
2 files changed, 93 insertions, 4 deletions
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index a17260329..85cdcafbf 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -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) { diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index f4f01df9a..c3f203184 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -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 { |