コード例 #1
0
ファイル: ubx.c プロジェクト: devbharat/pandapilot
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;

}
コード例 #2
0
ファイル: gps.c プロジェクト: romanpatscheider/px4_nxbuilder
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;

}
コード例 #3
0
ファイル: ubx.c プロジェクト: aqakulov/Firmware
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;

}