aboutsummaryrefslogtreecommitdiff
path: root/apps/gps/ubx.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/gps/ubx.c')
-rw-r--r--apps/gps/ubx.c327
1 files changed, 235 insertions, 92 deletions
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index e15ed4e00..50bf579a0 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -52,7 +52,7 @@
#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3
#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
-#define UBX_BUFFER_SIZE 1000
+#define UBX_BUFFER_SIZE 500
extern bool gps_mode_try_all;
extern bool gps_mode_success;
@@ -63,18 +63,10 @@ extern int current_gps_speed;
pthread_mutex_t *ubx_mutex;
gps_bin_ubx_state_t *ubx_state;
+enum UBX_CONFIG_STATE ubx_config_state;
static struct vehicle_gps_position_s *ubx_gps;
-//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};
-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};
void ubx_decode_init(void)
{
@@ -100,15 +92,15 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b)
int ubx_parse(uint8_t b, char *gps_rx_buffer)
{
-// printf("b=%x\n",b);
+ //printf("b=%x\n",b);
if (ubx_state->decode_state == UBX_DECODE_UNINIT) {
- if (b == 0xb5) {
+ if (b == UBX_SYNC_1) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC1;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) {
- if (b == 0x62) {
+ if (b == UBX_SYNC_2) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC2;
} else {
@@ -122,16 +114,25 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
//check for known class
switch (b) {
- case UBX_CLASS_NAV: //NAV
+ case UBX_CLASS_ACK:
+ ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
+ ubx_state->message_class = ACK;
+ break;
+
+ case UBX_CLASS_NAV:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = NAV;
break;
- case UBX_CLASS_RXM: //RXM
+ case UBX_CLASS_RXM:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
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;
@@ -193,9 +194,36 @@ 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;
+ case ACK:
+ switch (b) {
+ case UBX_MESSAGE_ACK_ACK:
+ ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
+ ubx_state->message_id = ACK_ACK;
+ break;
+ case UBX_MESSAGE_ACK_NAK:
+ ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
+ ubx_state->message_id = ACK_NAK;
+ break;
+ default: //unknown class: reset state machine, should not happen
+ ubx_decode_init();
+ break;
+ }
+ break;
default: //should not happen
ubx_decode_init();
break;
@@ -542,6 +570,75 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
break;
}
+ case ACK_ACK: {
+// 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
+ if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
+
+ switch (ubx_config_state) {
+ case UBX_CONFIG_STATE_PRT:
+ if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
+ ubx_config_state++;
+ break;
+ case UBX_CONFIG_STATE_NAV5:
+ if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
+ ubx_config_state++;
+ break;
+ case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
+ case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
+ case UBX_CONFIG_STATE_MSG_NAV_DOP:
+ case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
+ case UBX_CONFIG_STATE_MSG_NAV_SOL:
+ case UBX_CONFIG_STATE_MSG_NAV_VELNED:
+ case UBX_CONFIG_STATE_MSG_RXM_SVSI:
+ if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
+ ubx_config_state++;
+ break;
+ default:
+ break;
+ }
+
+
+ ret = 1;
+
+ } else {
+ if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n");
+
+ ret = 0;
+ }
+
+ // Reset state machine to decode next packet
+ ubx_decode_init();
+ return ret;
+
+ break;
+ }
+
+ case ACK_NAK: {
+// 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) {
+
+ if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n");
+ ret = 1;
+
+ } else {
+ if (gps_verbose) printf("[gps] ACK_NAK: 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();
@@ -574,53 +671,105 @@ 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)
{
- //fflush(((FILE *)fd));
-
- //TODO: write this in a loop once it is tested
- //UBX_CFG_PRT_PART:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd);
-
- usleep(100000);
-
- //NAV_POSLLH:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
- //NAV_TIMEUTC:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
- //NAV_DOP:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
- //NAV_SOL:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
-
- //NAV_SVINFO:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
- //NAV_VELNED:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
-
- //RXM_SVSI:
- write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
- usleep(100000);
-
- return 0;
+ // only needed once like this
+ const type_gps_bin_cfg_prt_packet_t cfg_prt_packet = {
+ .clsID = UBX_CLASS_CFG,
+ .msgID = UBX_MESSAGE_CFG_PRT,
+ .length = UBX_CFG_PRT_LENGTH,
+ .portID = UBX_CFG_PRT_PAYLOAD_PORTID,
+ .mode = UBX_CFG_PRT_PAYLOAD_MODE,
+ .baudRate = current_gps_speed,
+ .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK,
+ .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK,
+ .ck_a = 0,
+ .ck_b = 0
+ };
+
+ // only needed once like this
+ const type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet = {
+ .clsID = UBX_CLASS_CFG,
+ .msgID = UBX_MESSAGE_CFG_NAV5,
+ .length = UBX_CFG_NAV5_LENGTH,
+ .mask = UBX_CFG_NAV5_PAYLOAD_MASK,
+ .dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL,
+ .fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE,
+ .ck_a = 0,
+ .ck_b = 0
+ };
+
+ // this message is reusable for different configuration commands, so not const
+ type_gps_bin_cfg_msg_packet cfg_msg_packet = {
+ .clsID = UBX_CLASS_CFG,
+ .msgID = UBX_MESSAGE_CFG_MSG,
+ .length = UBX_CFG_MSG_LENGTH,
+ .rate = UBX_CFG_MSG_PAYLOAD_RATE
+ };
+
+ uint64_t time_before_config = hrt_absolute_time();
+
+ while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) {
+
+// 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;
}
@@ -637,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.
@@ -671,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;
}
@@ -688,28 +832,41 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
return ret;
}
-int write_config_message_ubx(uint8_t *message, size_t length, int fd)
+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;
unsigned int i;
- for (i = 2; i < length; i++) {
+ 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("[%x,%x]\n", ck_a, ck_b);
+ // write sync bytes first
+ buffer[0] = UBX_SYNC_1;
+ buffer[1] = UBX_SYNC_2;
+
+ // 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;
- unsigned int result_write = write(fd, message, length);
- result_write += write(fd, &ck_a, 1);
- result_write += write(fd, &ck_b, 1);
- fsync(fd);
+ // write the checksum
+ result_write += write(*fd, buffer, sizeof(buffer));
- return (result_write != length + 2); //return 0 as success
+ fsync(*fd);
+ if ((unsigned int)result_write != length + 2)
+ return ERROR;
+ return OK;
}
void *ubx_watchdog_loop(void *args)
@@ -723,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;
@@ -732,7 +892,7 @@ void *ubx_watchdog_loop(void *args)
bool once_ok = false;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
+ ubx_config_state = UBX_CONFIG_STATE_PRT;
//int err_skip_counter = 0;
while (!(*thread_should_exit)) {
@@ -833,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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-07 22:37:49 +0000