void *ubx_watchdog_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps ubx watchdog", 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;
ubx_config_state = UBX_CONFIG_STATE_PRT;
/* first try to configure the GPS anyway */
configure_gps_ubx(fd);
/* GPS watchdog error message skip counter */
bool ubx_healthy = false;
uint8_t ubx_fail_count = 0;
uint8_t ubx_success_count = 0;
bool once_ok = false;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
//int err_skip_counter = 0;
while (!(*thread_should_exit)) {
/* if some values are to old reconfigure gps */
int i;
pthread_mutex_lock(ubx_mutex);
bool all_okay = true;
uint64_t timestamp_now = hrt_absolute_time();
for (i = 0; i < UBX_NO_OF_MESSAGES; i++) {
// printf("timestamp_now=%llu\n", timestamp_now);
// printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]);
if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) {
//printf("Warning: GPS ubx message %d not received for a long time\n", i);
all_okay = false;
}
}
pthread_mutex_unlock(ubx_mutex);
if (!all_okay) {
/* gps error */
ubx_fail_count++;
// if (err_skip_counter == 0)
// {
// printf("GPS Watchdog detected gps not running or having problems\n");
// err_skip_counter = 20;
// }
// err_skip_counter--;
//printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
/* If we have too many failures and another mode or baud should be tried, exit... */
if ((gps_mode_try_all == true || gps_baud_try_all == true) && (ubx_fail_count >= UBX_HEALTH_PROBE_COUNTER_LIMIT) && (ubx_healthy == false) && once_ok == false) {
if (gps_verbose) printf("[gps] Connection attempt failed, no UBX module found\n");
gps_mode_success = false;
break;
}
if (ubx_healthy && ubx_fail_count == UBX_HEALTH_FAIL_COUNTER_LIMIT) {
printf("[gps] ERROR: UBX GPS module stopped responding\n");
// global_data_send_subsystem_info(&ubx_present_enabled);
mavlink_log_critical(mavlink_fd, "[gps] UBX module stopped responding\n");
ubx_healthy = false;
ubx_success_count = 0;
}
/* trying to reconfigure the gps configuration */
ubx_config_state = UBX_CONFIG_STATE_PRT;
configure_gps_ubx(fd);
fflush(stdout);
sleep(1);
} else {
/* gps healthy */
ubx_success_count++;
ubx_fail_count = 0;
once_ok = true; // XXX Should this be true on a single success, or on same criteria as ubx_healthy?
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
ubx_healthy = true;
}
}
usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
if(gps_verbose) printf("[gps] ubx loop is going to terminate\n");
close(mavlink_fd);
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;
}
static void *ubx_watchdog_loop(void * arg) //runs as a pthread and listens to uart1 ("/dev/ttyS0")
{
/* Retrieve file descriptor */
int fd = *((int *)arg);
while(1)
{
/* if some values are to old reconfigure gps */
int i;
pthread_mutex_lock(&ubx_mutex);
bool all_okay = true;
uint64_t timestamp_now = global_data_get_timestamp_milliseconds();
for(i = 0; i < UBX_NO_OF_MESSAGES; i++)
{
if(timestamp_now - ubx_state->last_message_timestamps[i] > GPS_WATCHDOG_CRITICAL_TIME_MILLISECONDS)
{
// printf("Warning: GPS ubx message %d not received for a long time\n", i);
all_okay = false;
}
}
pthread_mutex_unlock(&ubx_mutex);
if(!all_okay)
{
/* gps error */
printf("GPS Watchdog detected gps not running or having problems\n");
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);
/* trying to reconfigure the gps configuration */
configure_gps_ubx(fd);
fflush(stdout);
sleep(1);
// int killres = pthread_kill(&ubx_thread, SIGKILL);
// printf("killres=%d",killres);
// sleep(1);
// pthread_create (&ubx_thread, NULL, ubx_loop, (void *)&fd);
}
else
{
/* gps healthy */
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);
}
usleep(GPS_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
}
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;
}
