diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-01-17 16:53:32 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-01-17 16:54:32 -0800 |
commit | 9ca472bbc7727294c039dbced0dc7bea6925789e (patch) | |
tree | 11c43286273d1e9d1936a561d65165a492e472e0 /apps/gps | |
parent | ebaa38ad1b5c1d625467fc867480e1969edd60aa (diff) | |
download | px4-firmware-9ca472bbc7727294c039dbced0dc7bea6925789e.tar.gz px4-firmware-9ca472bbc7727294c039dbced0dc7bea6925789e.tar.bz2 px4-firmware-9ca472bbc7727294c039dbced0dc7bea6925789e.zip |
Ubx configuration working again, gps app is still complicated and big but should be wrking better now
Diffstat (limited to 'apps/gps')
-rw-r--r-- | apps/gps/gps.c | 7 | ||||
-rw-r--r-- | apps/gps/ubx.c | 256 | ||||
-rw-r--r-- | apps/gps/ubx.h | 7 |
3 files changed, 93 insertions, 177 deletions
diff --git a/apps/gps/gps.c b/apps/gps/gps.c index cb9a77c51..8a9512054 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -107,8 +107,8 @@ enum GPS_MODES { #define AUTO_DETECTION_COUNT 8 -const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400}; -const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea +const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600}; +const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea /**************************************************************************** * Private functions @@ -189,7 +189,7 @@ int gps_thread_main(int argc, char *argv[]) { gps_mode_success = true; terminate_gps_thread = false; bool retry = false; - gps_verbose = true; + gps_verbose = false; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -368,7 +368,6 @@ int gps_thread_main(int argc, char *argv[]) { args.thread_should_exit_ptr = &thread_should_exit; pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args); - sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver pthread_attr_t ubx_wd_attr; pthread_attr_init(&ubx_wd_attr); diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 33f44287b..50bf579a0 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -52,10 +52,7 @@ #define UBX_HEALTH_FAIL_COUNTER_LIMIT 3 #define UBX_HEALTH_PROBE_COUNTER_LIMIT 4 -#define UBX_BUFFER_SIZE 1000 - -// Set dynamic model to 7: Airborne with <2g Acceleration -#define DYN_MODEL_NO 0x07 +#define UBX_BUFFER_SIZE 500 extern bool gps_mode_try_all; extern bool gps_mode_success; @@ -69,24 +66,6 @@ gps_bin_ubx_state_t *ubx_state; enum UBX_CONFIG_STATE ubx_config_state; static struct vehicle_gps_position_s *ubx_gps; -bool gps_nav5_conf_success = false; - -unsigned int got_ack = 0; -unsigned int got_nak = 0; - -//Definitions for ubx, last two bytes are checksum which is calculated below -uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x96, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x21, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_DOP[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x00, 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}; - -//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}; - void ubx_decode_init(void) @@ -117,13 +96,11 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) if (ubx_state->decode_state == UBX_DECODE_UNINIT) { if (b == UBX_SYNC_1) { -// printf("got sync1"); ubx_state->decode_state = UBX_DECODE_GOT_SYNC1; } } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) { if (b == UBX_SYNC_2) { -// printf("got sync2"); ubx_state->decode_state = UBX_DECODE_GOT_SYNC2; } else { @@ -138,7 +115,6 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //check for known class switch (b) { case UBX_CLASS_ACK: - printf("class ack"); ubx_state->decode_state = UBX_DECODE_GOT_CLASS; ubx_state->message_class = ACK; break; @@ -281,7 +257,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //convert to correct struct switch (ubx_state->message_id) { //this enum is unique for all ids --> no need to check the class case NAV_POSLLH: { - printf("GOT NAV_POSLLH MESSAGE\n"); +// printf("GOT NAV_POSLLH MESSAGE\n"); gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -314,7 +290,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_SOL: { - printf("GOT NAV_SOL MESSAGE\n"); +// printf("GOT NAV_SOL MESSAGE\n"); gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -346,7 +322,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_DOP: { - printf("GOT NAV_DOP MESSAGE\n"); +// printf("GOT NAV_DOP MESSAGE\n"); gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -378,7 +354,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_TIMEUTC: { - printf("GOT NAV_TIMEUTC MESSAGE\n"); +// printf("GOT NAV_TIMEUTC MESSAGE\n"); gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -424,7 +400,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_SVINFO: { - printf("GOT NAV_SVINFO MESSAGE\n"); +// printf("GOT NAV_SVINFO MESSAGE\n"); //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message const int length_part1 = 8; @@ -525,7 +501,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_VELNED: { - printf("GOT NAV_VELNED MESSAGE\n"); +// printf("GOT NAV_VELNED MESSAGE\n"); gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -561,7 +537,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case RXM_SVSI: { - printf("GOT RXM_SVSI MESSAGE\n"); +// printf("GOT RXM_SVSI MESSAGE\n"); const int length_part1 = 7; char gps_rx_buffer_part1[length_part1]; memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); @@ -593,39 +569,9 @@ 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; -// } case ACK_ACK: { - printf("GOT ACK_ACK\n"); +// printf("GOT ACK_ACK\n"); gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer; //Check if checksum is valid @@ -633,7 +579,6 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) switch (ubx_config_state) { case UBX_CONFIG_STATE_PRT: - printf("clsID: %d, msgID: %d\n", packet->clsID, packet->msgID); if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) ubx_config_state++; break; @@ -672,13 +617,13 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case ACK_NAK: { - printf("GOT ACK_NAK\n"); +// printf("GOT ACK_NAK\n"); gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer; //Check if checksum is valid if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - printf("[gps] the ubx gps returned: not acknowledged\n"); + if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n"); ret = 1; } else { @@ -726,10 +671,6 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length) message[length - 2] = ck_a; message[length - 1] = ck_b; - -// printf("[%x,%x]", ck_a, ck_b); - -// printf("[%x,%x]\n", message[length-2], message[length-1]); } int configure_gps_ubx(int *fd) @@ -741,7 +682,7 @@ int configure_gps_ubx(int *fd) .length = UBX_CFG_PRT_LENGTH, .portID = UBX_CFG_PRT_PAYLOAD_PORTID, .mode = UBX_CFG_PRT_PAYLOAD_MODE, - .baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE, + .baudRate = current_gps_speed, .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK, .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK, .ck_a = 0, @@ -768,59 +709,67 @@ int configure_gps_ubx(int *fd) .rate = UBX_CFG_MSG_PAYLOAD_RATE }; - if (gps_verbose) printf("Config state: %d\n", ubx_config_state); + uint64_t time_before_config = hrt_absolute_time(); - switch (ubx_config_state) { - case UBX_CONFIG_STATE_PRT: - write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); - break; - case UBX_CONFIG_STATE_NAV5: - write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_RXM_SVSI: - cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_CONFIGURED: - if (gps_verbose) printf("[gps] ubx configuration finished\n"); - return OK; - break; - default: - break; - } + while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) { - return OK; +// if (gps_verbose) printf("[gps] ubx config state: %d\n", ubx_config_state); + + switch (ubx_config_state) { + case UBX_CONFIG_STATE_PRT: +// if (gps_verbose) printf("[gps] Configuring ubx with baudrate: %d\n", cfg_prt_packet.baudRate); + + write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); + break; + case UBX_CONFIG_STATE_NAV5: + write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_RXM_SVSI: + cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_CONFIGURED: + if (gps_verbose) printf("[gps] ubx configuration finished\n"); + return OK; + break; + default: + break; + } + usleep(10000); + } + if (gps_verbose) printf("[gps] ubx configuration timeout\n"); + return ERROR; } @@ -837,22 +786,17 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) fds.events = POLLIN; // UBX GPS mode - // This blocks the task until there is something on the buffer while (1) { //check if the thread should terminate if (terminate_gps_thread == true) { -// printf("terminate_gps_thread=%u ", terminate_gps_thread); -// printf("exiting mtk thread\n"); -// fflush(stdout); ret = 1; break; } - if (poll(&fds, 1, 1000) > 0) { if (read(*fd, &c, 1) > 0) { - // printf("Read %x\n",c); +// printf("Read %x\n",c); if (rx_count >= buffer_size) { // The buffer is already full and we haven't found a valid ubx sentence. // Flush the buffer and note the overflow event. @@ -871,7 +815,7 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) int msg_read = ubx_parse(c, gps_rx_buffer); if (msg_read > 0) { - // printf("Found sequence\n"); + //printf("Found sequence\n"); break; } @@ -890,7 +834,6 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd) { - //calculate and write checksum to the end uint8_t ck_a = 0; uint8_t ck_b = 0; @@ -899,36 +842,31 @@ int write_config_message_ubx(const uint8_t *message, const size_t length, const uint8_t buffer[2]; ssize_t result_write = 0; + //calculate and write checksum to the end for (i = 0; i < length-2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } -// printf("chk: [%x,%x]\n", ck_a, ck_b); - + // write sync bytes first buffer[0] = UBX_SYNC_1; buffer[1] = UBX_SYNC_2; - result_write = write(*fd, buffer, sizeof(buffer)); - result_write += write(*fd, message, length-2); - -// for(i=0; i<length-2; i++) { -// printf("%00#x", message[i]); -// } + // write config message without the checksum + result_write = write(*fd, buffer, sizeof(buffer)); + result_write += write(*fd, message, length-2); buffer[0] = ck_a; buffer[1] = ck_b; - result_write += write(*fd, buffer, sizeof(buffer)); - -//// printf("%00#x", ck_a); - result_write += write(*fd, &ck_b, sizeof(ck_b)); -// printf("%00#x", ck_b); + // write the checksum + result_write += write(*fd, buffer, sizeof(buffer)); fsync(*fd); - if (result_write != length + 2) + if ((unsigned int)result_write != length + 2) return ERROR; + return OK; } void *ubx_watchdog_loop(void *args) @@ -942,6 +880,9 @@ void *ubx_watchdog_loop(void *args) int *fd = arguments->fd_ptr; bool *thread_should_exit = arguments->thread_should_exit_ptr; + /* first try to configure the GPS anyway */ + configure_gps_ubx(fd); + /* GPS watchdog error message skip counter */ bool ubx_healthy = false; @@ -970,10 +911,6 @@ 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) { @@ -1026,7 +963,6 @@ void *ubx_watchdog_loop(void *args) if(gps_verbose) printf("[gps] ubx loop is going to terminate\n"); close(mavlink_fd); - return NULL; } @@ -1050,7 +986,6 @@ void *ubx_loop(void *args) //ubx state ubx_state = malloc(sizeof(gps_bin_ubx_state_t)); - //printf("gps: ubx_state created\n"); ubx_decode_init(); ubx_state->print_errors = false; @@ -1058,23 +993,6 @@ void *ubx_loop(void *args) /* set parameters for ubx */ - if (configure_gps_ubx(fd) != 0) { - printf("[gps] Configuration of gps module to ubx failed\n"); - - /* Write shared variable sys_status */ - // TODO enable this again - //global_data_send_subsystem_info(&ubx_present); - - } else { - if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n"); - - // XXX Shouldn't the system status only change if the module is known to work ok? - - /* Write shared variable sys_status */ - // TODO enable this again - //global_data_send_subsystem_info(&ubx_present_enabled); - } - struct vehicle_gps_position_s ubx_gps_d = {.counter = 0}; ubx_gps = &ubx_gps_d; @@ -1094,8 +1012,8 @@ void *ubx_loop(void *args) } } -// if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); -// close(gps_pub); + if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); + close(gps_pub); return NULL; } diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index eb19d17f2..e700fe388 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -49,12 +49,11 @@ //internal definitions (not depending on the ubx protocol -#define CONFIGURE_UBX_FINISHED 0 -#define CONFIGURE_UBX_MESSAGE_ACKNOWLEDGED 1 -#define CONFIGURE_UBX_MESSAGE_NOT_ACKNOWLEDGED 2 #define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */ -#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 500000 /**< Check for current state every 0.4 seconds */ +#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 1000000 /**< Check for current state every second */ + +#define UBX_CONFIG_TIMEOUT 500000 #define APPNAME "gps: ubx" |