void *ubx_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps ubx read", getpid());
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* Initialize gps stuff */
char gps_rx_buffer[UBX_BUFFER_SIZE];
if (gps_verbose) printf("[gps] UBX protocol driver starting..\n");
//set parameters for ubx_state
//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;
/* set parameters for ubx */
struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
ubx_gps = &ubx_gps_d;
orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) {
/* publish new GPS position */
orb_publish(ORB_ID(vehicle_gps_position), gps_pub, ubx_gps);
} else {
/* de-advertise */
close(gps_pub);
break;
}
}
if(gps_verbose) printf("[gps] ubx read is going to terminate\n");
close(gps_pub);
return NULL;
}
int ubx_parse(uint8_t b, char *gps_rx_buffer)
{
//printf("b=%x\n",b);
if (ubx_state->decode_state == UBX_DECODE_UNINIT) {
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 == UBX_SYNC_2) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC2;
} else {
// Second start symbol was wrong, reset state machine
ubx_decode_init();
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC2) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
//check for known class
switch (b) {
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:
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;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_CLASS) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
//depending on class look for message id
switch (ubx_state->message_class) {
case NAV:
switch (b) {
case UBX_MESSAGE_NAV_POSLLH: //NAV-POSLLH: Geodetic Position Solution
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_POSLLH;
break;
case UBX_MESSAGE_NAV_SOL:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_SOL;
break;
case UBX_MESSAGE_NAV_TIMEUTC:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_TIMEUTC;
break;
case UBX_MESSAGE_NAV_DOP:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_DOP;
break;
case UBX_MESSAGE_NAV_SVINFO:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_SVINFO;
break;
case UBX_MESSAGE_NAV_VELNED:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = NAV_VELNED;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
case RXM:
switch (b) {
case UBX_MESSAGE_RXM_SVSI:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = RXM_SVSI;
break;
default: //unknown class: reset state machine, should not happen
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;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_MESSAGEID) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
ubx_state->payload_size = b;
ubx_state->decode_state = UBX_DECODE_GOT_LENGTH1;
} else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH1) {
// Add to checksum
ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
ubx_state->payload_size += b << 8;
ubx_state->decode_state = UBX_DECODE_GOT_LENGTH2;
} else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH2) {
uint8_t ret = 0;
// Add to checksum if not yet at checksum byte
if (ubx_state->rx_count < ubx_state->payload_size) ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b));
// Fill packet buffer
gps_rx_buffer[ubx_state->rx_count] = b;
//if whole payload + checksum is in buffer:
if (ubx_state->rx_count >= ubx_state->payload_size + 1) {
//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");
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
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->lat = packet->lat;
ubx_gps->lon = packet->lon;
ubx_gps->alt = packet->height_msl;
ubx_gps->counter_pos_valid++;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_POSLLH - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_POSLLH: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_SOL: {
// 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
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->fix_type = packet->gpsFix;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
ubx_gps->s_variance = packet->sAcc;
ubx_gps->p_variance = packet->pAcc;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_SOL: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_DOP: {
// 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
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->eph = packet->hDOP;
ubx_gps->epv = packet->vDOP;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_DOP - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_DOP: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_TIMEUTC: {
// 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
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
//convert to unix timestamp
struct tm timeinfo;
timeinfo.tm_year = packet->year - 1900;
timeinfo.tm_mon = packet->month - 1;
timeinfo.tm_mday = packet->day;
timeinfo.tm_hour = packet->hour;
timeinfo.tm_min = packet->min;
timeinfo.tm_sec = packet->sec;
time_t epoch = mktime(&timeinfo);
// printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, packet->time_nanoseconds);
ubx_gps->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_TIMEUTC - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("\t[gps] NAV_TIMEUTC: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_SVINFO: {
// 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;
char gps_rx_buffer_part1[length_part1];
memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1);
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) gps_rx_buffer_part1;
//read checksum
const int length_part3 = 2;
char gps_rx_buffer_part3[length_part3];
memcpy(gps_rx_buffer_part3, &(gps_rx_buffer[ubx_state->rx_count - 1]), length_part3);
gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) gps_rx_buffer_part3;
//Check if checksum is valid and then store the gps information
if (ubx_state->ck_a == packet_part3->ck_a && ubx_state->ck_b == packet_part3->ck_b) {
//definitions needed to read numCh elements from the buffer:
const int length_part2 = 12;
gps_bin_nav_svinfo_part2_packet_t *packet_part2;
char gps_rx_buffer_part2[length_part2]; //for temporal storage
int i;
for (i = 0; i < packet_part1->numCh; i++) { //for each channel
/* Get satellite information from the buffer */
memcpy(gps_rx_buffer_part2, &(gps_rx_buffer[length_part1 + i * length_part2]), length_part2);
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) gps_rx_buffer_part2;
/* Write satellite information in the global storage */
ubx_gps->satellite_prn[i] = packet_part2->svid;
//if satellite information is healthy store the data
uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
if (!unhealthy) {
if ((packet_part2->flags) & 1) { //flags is a bitfield
ubx_gps->satellite_used[i] = 1;
} else {
ubx_gps->satellite_used[i] = 0;
}
ubx_gps->satellite_snr[i] = packet_part2->cno;
ubx_gps->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
ubx_gps->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
} else {
ubx_gps->satellite_used[i] = 0;
ubx_gps->satellite_snr[i] = 0;
ubx_gps->satellite_elevation[i] = 0;
ubx_gps->satellite_azimuth[i] = 0;
}
}
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
/* Unused channels have to be set to zero for e.g. MAVLink */
ubx_gps->satellite_prn[i] = 0;
ubx_gps->satellite_used[i] = 0;
ubx_gps->satellite_snr[i] = 0;
ubx_gps->satellite_elevation[i] = 0;
ubx_gps->satellite_azimuth[i] = 0;
}
/* set flag if any sat info is available */
if (!packet_part1->numCh > 0) {
ubx_gps->satellite_info_available = 1;
} else {
ubx_gps->satellite_info_available = 0;
}
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_SVINFO - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("\t[gps] NAV_SVINFO: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case NAV_VELNED: {
// 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
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->vel = (uint16_t)packet->speed;
ubx_gps->vel_n = packet->velN / 100.0f;
ubx_gps->vel_e = packet->velE / 100.0f;
ubx_gps->vel_d = packet->velD / 100.0f;
ubx_gps->vel_ned_valid = true;
ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_VELNED - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] NAV_VELNED: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case RXM_SVSI: {
// 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);
gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) gps_rx_buffer_part1;
//Check if checksum is valid and the store the gps information
if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) {
ubx_gps->satellites_visible = packet->numVis;
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
//pthread_mutex_unlock(ubx_mutex);
ret = 1;
} else {
if (gps_verbose) printf("[gps] RXM_SVSI: 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");
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();
break;
}
}
(ubx_state->rx_count)++;
}
return 0; // no valid packet found
}
int configure_gps_ubx(int *fd)
{
// 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");
printf("\nGPS CONFIGURED\n");
thread_should_exit = true;
return OK;
break;
default:
break;
}
usleep(10000);
}
if (gps_verbose) printf("[gps] ubx configuration timeout\n");
return ERROR;
}
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
{
uint8_t ret = 0;
uint8_t c;
int rx_count = 0;
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = *fd;
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) {
ret = 1;
break;
}
if (poll(&fds, 1, 1000) > 0) {
if (read(*fd, &c, 1) > 0) {
// 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.
gpsRxOverflow++;
rx_count = 0;
ubx_decode_init();
if (gps_verbose) printf("[gps] Buffer full\n");
} else {
//gps_rx_buffer[rx_count] = c;
rx_count++;
}
int msg_read = ubx_parse(c, gps_rx_buffer);
if (msg_read > 0) {
//printf("Found sequence\n");
break;
}
} else {
break;
}
} else {
break;
}
}
return ret;
}
int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd)
{
uint8_t ck_a = 0;
uint8_t ck_b = 0;
unsigned int 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;
}
// 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;
// write the checksum
result_write += write(*fd, buffer, sizeof(buffer));
fsync(*fd);
if ((unsigned int)result_write != length + 2)
return ERROR;
return OK;
}
int gps_main(int argc, char *argv[])
{
// print text
printf("Hello, %s!\n",APPNAME);
usleep(100000);
/* initialize shared data structures */
global_data_init(&global_data_gps.access_conf);
global_data_init(&global_data_sys_status.access_conf);
/* default values */
char * commandline_usage = "\tusage: gps -d devicename -m mode\n";
char * device = "/dev/ttyS3";
char * mode = "nmea";
/* read arguments */
int i;
for (i = 1; i < argc; i++) //argv[0] is "mavlink"
{
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) //device set
{
if(argc > i+1)
{
device = argv[i+1];
}
else
{
printf(commandline_usage);
return 0;
}
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) //device set
{
if(argc > i+1)
{
mode = argv[i+1];
}
else
{
printf(commandline_usage);
return 0;
}
}
}
/* open port (baud rate is set in defconfig file) */
int fd = open_port(device);
if(fd != -1)
{
printf("\t%s: Port opened: %s\n", APPNAME, device);
}
else
{
printf("\t%s: Could not open port, exiting gps app!\n", APPNAME);
sleep(1);
return 0;
}
//TODO: add mode if here (reads from arguments and configuration, watchdog will look for changes in mode configuration while running (?))
/* Init mutex for datasharing between ubx gps reading thrad (ubx_thread) and ubx_watchdog thread*/
pthread_mutex_init(&ubx_mutex, NULL);
/* Initialize ubx state */
ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
ubx_decode_init();
/* create pthreads */
pthread_create (&ubx_thread, NULL, ubx_loop, (void *)&fd);
/* wait before starting watchdog */
sleep(5);
pthread_create (&ubx_watchdog_thread, NULL, ubx_watchdog_loop, (void *)&fd);
/* wait for threads to complete */
pthread_join(ubx_thread, NULL);
pthread_join(ubx_watchdog_thread, (void *)&fd);
return 0;
}
static void *ubx_loop(void * arg) //runs as a pthread and listens to uart1 ("/dev/ttyS0")
{
/* Initialize ubx state */
// ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
// ubx_decode_init();
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Initialize gps stuff */
int buffer_size = 1000;
// nmeaINFO * info = malloc(sizeof(nmeaINFO));
bool health_set = false;
char * gps_rx_buffer = malloc(buffer_size*sizeof(char));
/* gps parser (nmea) */
// nmeaPARSER parser;
// nmea_parser_init(&parser);
// nmea_zero_INFO(info);
// float lat_dec = 0;
// float lon_dec = 0;
/* custom (mediatek custom) */
// gps_bin_custom_state_t * mtk_state = malloc(sizeof(gps_bin_custom_state_t));
// mtk_decode_init(mtk_state);
// mtk_state->print_errors = false;
// if( !strcmp("custom",mode) )
// {
// printf("\t%s: custom mode\n",APPNAME);
// // configure_gps_custom(fd); // ?
//
//
// // while(1)
// //
// // if (configure_gps_ubx(fd, ubx_state) != 0)
// //
// // {
// // //TODO: execute custom read
// // }
//
// }
// else if( !strcmp("ubx",mode) )
// {
printf("\t%s: ubx mode\n",APPNAME);
//set parameters for ubx
//ubx state
gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
printf("%s: ubx_state created\n",APPNAME);
ubx_decode_init();
ubx_state->print_errors = false;
int config_not_finished = 1; //is set to 0 as soon as all configurations are completed
bool configured = false;
/* set parameters for ubx */
if (configure_gps_ubx(fd) != 0)
{
printf("Configuration of gps module to ubx failed\n");
/* Write shared variable sys_status */
global_data_lock(&global_data_sys_status.access_conf);
global_data_sys_status.onboard_control_sensors_present |= 1 << 5;//TODO: write wrapper for bitmask
global_data_sys_status.onboard_control_sensors_enabled &= ~(1 << 5);
global_data_sys_status.onboard_control_sensors_health &= ~(1 << 5);
global_data_sys_status.counter++;
global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_sys_status.access_conf);
}
else
{
printf("Configuration of gps module to ubx successful\n");
/* Write shared variable sys_status */
global_data_lock(&global_data_sys_status.access_conf);
global_data_sys_status.onboard_control_sensors_present |= 1 << 5;//TODO: write wrapper for bitmask
global_data_sys_status.onboard_control_sensors_enabled |= 1 << 5;
global_data_sys_status.counter++;
global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_sys_status.access_conf);
}
/* Inform the other processes that there is new gps data available */
global_data_broadcast(&global_data_sys_status.access_conf);
while(1)
{
read_gps_ubx(fd, gps_rx_buffer, buffer_size, &ubx_mutex);
//
// /* set health to true if config is finished after certain time (only executed once) */
// if(config_not_finished == 0 && counter >= GPS_COUNTER_LIMIT && false == health_set)
// {
// global_data_lock(&global_data_sys_status.access_conf);
// global_data_sys_status.onboard_control_sensors_health |= 1 << 5;
// global_data_sys_status.counter++;
// global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
// global_data_unlock(&global_data_sys_status.access_conf);
//
// health_set = true;
//
// printf("%s: gps configuration successful\n",APPNAME);
// }
// else if (config_not_finished != 0 && counter >= GPS_COUNTER_LIMIT)
// {
// //reset state machine
// ubx_decode_init(ubx_state);
// ubx_state->print_errors = false;
// ubx_state->last_config_message_sent = UBX_CONFIGURE_NOTHING;
// ubx_state->last_ack_message_received = UBX_CONFIGURE_NOTHING;
// ubx_state->last_config_failed = false;
// config_not_finished = 1; //is set to 0 as soon as all configurations are completed
// bool configured = false;
// counter = 0;
//
//
// printf("%s: gps configuration probably failed, exiting now\n",APPNAME);
//// sleep(1);
//// return 0;
// }
/* Inform the other processes that there is new gps data available */
global_data_broadcast(&global_data_gps.access_conf);
}
// }
// else if( !strcmp("nmea",mode) )
// {
// printf("\t%s: nmea mode\n",APPNAME);
// }
// while(1)
// {
// if( !strcmp("nmea",mode) ) //TODO: implement use of global_data-gps also in nmea mode (currently only in ubx mode)
// {
// printf("\t%s: nmea mode\n");
// //get gps data into info
// read_gps_nmea(fd, gps_rx_buffer, buffer_size, info, &parser);
// //convert latitude longitude
// lat_dec = ndeg2degree(info->lat);
// lon_dec = ndeg2degree(info->lon);
//
// //Test output
//// printf("Lat:%d, Lon:%d,Elev:%d, Sig:%d, Fix:%d, Inview:%d\n", (int)(lat_dec*1e6), (int)(lon_dec*1e6), (int)(info->elv*1e6), info->sig, info->fix, info->satinfo.inview);
// }
//// else if ( !strcmp("ubx",mode) )
//// {
////
//// //get gps data into info
//// read_gps_ubx(fd, gps_rx_buffer, buffer_size, ubx_state); //TODO: atm using the info struct from the nmea library, once the gps/mavlink structures are clear--> use own struct
////// lat_dec = info->lat;
////// lon_dec = info->lon;
////// printf("Lat:%d, Lon:%d,Elev:%d, Sig:%d, Fix:%d, Inuse:%d, PDOP:%d\n", (int)(lat_dec*1e6), (int)(lon_dec*1e6), (int)(info->elv*1e6), info->sig, info->fix, info->satinfo.inuse, (int)(info->PDOP*1e4));
//// }
// else if ( !strcmp("custom",mode) ) //TODO: implement use of global_data-gps also in custom mode (currently only in ubx mode)
// {
// //info is used as storage of the gps information. lat lon are already in fractional degree format * 10e6
// //see custom.h/mtk_parse for more information on which variables are stored in info
// nmea_zero_INFO(info);
//
// //get gps data into info
// read_gps_custom(fd, gps_rx_buffer, buffer_size, info, mtk_state);
//
// //Test output
//// printf("Lat:%d, Lon:%d,Elev:%d, Sig:%d, Fix:%d, Inview:%d\n", (int)(info->lat), (int)info->lon, (int)info->elv, info->sig, info->fix, info->satinfo.inview);
//
// }
//
//
//
//
//
// }
free(gps_rx_buffer);
// free(info);
//close port
close_port(fd);
//destroy gps parser
// nmea_parser_destroy(&parser);
return 0;
}
void *ubx_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps ubx read", getpid());
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* Initialize gps stuff */
char gps_rx_buffer[UBX_BUFFER_SIZE];
if (gps_verbose) printf("[gps] UBX protocol driver starting..\r\n");
//set parameters for ubx_state
//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;
/* set parameters for ubx */
if (configure_gps_ubx(fd) != 0) {
printf("[gps] Configuration of gps module to ubx failed\r\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\r\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;
orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) {
/* publish new GPS position */
orb_publish(ORB_ID(vehicle_gps_position), gps_pub, ubx_gps);
} else {
/* de-advertise */
close(gps_pub);
break;
}
}
if(gps_verbose) printf("[gps] ubx read is going to terminate\n");
close(gps_pub);
return NULL;
}
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
{
uint8_t ret = 0;
uint8_t c;
int rx_count = 0;
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = *fd;
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);
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.
gpsRxOverflow++;
rx_count = 0;
ubx_decode_init();
if (gps_verbose) printf("[gps] Buffer full\n");
} else {
//gps_rx_buffer[rx_count] = c;
rx_count++;
}
int msg_read = ubx_parse(c, gps_rx_buffer);
if (msg_read > 0) {
// printf("Found sequence\n");
break;
}
} else {
break;
}
} else {
break;
}
}
return ret;
}