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;
}
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;
}
