示例#1
0
int main(int argc, char **argv)
{
	int fd;
        char *port = "/dev/can1";	// use "1" to "4" for steinhoff
        int opt;
	int status;
	db_id_t *pdb_id_list = NULL;
	db_clt_typ *pclt;              /* Database client pointer */
	char hostname[MAXHOSTNAMELEN+1];
	char *domain = DEFAULT_SERVICE; 
	int xport = COMM_OS_XPORT;	
	int count = 0;
	
        while ((opt = getopt(argc, argv, "p:")) != -1) {
                switch (opt) {
                case 'p':
                        port = strdup(optarg);
                        break;
                default:
                        printf("Usage: %s -p <port>\n", argv[0]);
                        exit(1);
                }
        }

	fd = can_open(port, O_RDONLY);

	if (fd == -1)
		exit(EXIT_FAILURE);	// error message printed by can_open 

	printf("program %s, device name %s, fd: %d\n", argv[0], port, fd);
	fflush(stdout);

	pdb_id_list = get_db_id_list();
	get_local_name(hostname, MAXHOSTNAMELEN);

	// Log in to the data server and create variables for all VAA messages
	if ((pclt = db_list_init( argv[0], hostname, domain, xport, 
			pdb_id_list, num_vaa_msg_ids, NULL, 0)) == NULL) {
		printf("Database initialization error in %s.\n", argv[0]);
		exit(EXIT_FAILURE);
	}
	
        if(setjmp(exit_env) != 0) {
		printf("Successfully wrote %d messages to data server\n",
				count);
		if (fd != -1)
			status = can_close(&fd);
		if (status != -1)			
			exit(EXIT_SUCCESS);
		else
			exit(EXIT_FAILURE);	// can_close prints error info
        } else
		sig_ign(sig_list, sig_hand);

	for(;;) {
		int retval;
		retval = rcv_g(pclt, fd, (void *) &vaa_msg[0]);
		count += retval;
	}
}
示例#2
0
void aversive_close(aversive_dev_t* dev)
{
#if CONFIG_USE_I2C
  i2c_close(&dev->i2c_dev);
#elif CONFIG_USE_CAN
  can_close(dev->can_dev);
#endif
}
示例#3
0
/////////////////////////////////////////////////////////////////////////////////////////
// Close CAN data channel
void CloseCAN()
{
	int ret;

	StopCANListenThread();

	for (int ch = 0; ch < CAN_Ch_COUNT; ch++) {
		if (!CAN_Ch_Enabled[ch]) continue;
		printf(">CAN(%d): close\n", CAN_Ch[ch]);
		ret = can_close(CAN_Ch[ch]);
		if(ret < 0) printf("ERROR command_can_close !!! \n");
	}
}
示例#4
0
文件: print.c 项目: kralf/era-libs
int main(int argc, char **argv) {
  if ((argc < 2) || (argc > 3)) {
    fprintf(stderr, "Usage: %s DEV [HZ]\n", argv[0]);
    return -1;
  }

  double frequency = 0.0;
  if (argc == 3) sscanf(argv[2], "%lf", &frequency);

  can_init(argv[1]);

  int result = era_sensors_start(print, frequency);

  if (!result) era_thread_wait_exit(&era_sensors_thread);

  can_close();
  return 0;
}
示例#5
0
int main(int argc, char **argv)
{
	int size;
	int fd;
	unsigned long id;  
	unsigned char data[8];
        char *port = "/dev/can1";	// use "1" to "4" for steinhoff
        int opt;
	int status;
	int read_err = 0;
	int verbose = 0;
	int msg_count = 0;

        while ((opt = getopt(argc, argv, "p:v")) != -1) {
                switch (opt) {
                  case 'p':
                        port = strdup(optarg);
                        break;
                  case 'v':
                        verbose = 1; 
                        break;

                  default:
                        printf("Usage: %s -p <port>\n", argv[0]);
                        exit(1);
                }
        }

	printf("can_rx: trying to open %s\n", port); 
	fflush(stdout);

	fd = can_open(port, O_RDONLY);

	if (fd == -1)
		exit(EXIT_FAILURE);	// error message printed by can_open 

	printf("program %s, device name %s, fd: %d\n", argv[0], port, fd);
	fflush(stdout);

        if(setjmp(exit_env) != 0) {
		printf("%d messages, %d read errors\n", msg_count, read_err);
		if (fd != -1)
			status = can_close(&fd);
		if (status != -1)			
			exit(EXIT_SUCCESS);
		else
			exit(EXIT_FAILURE);	// can_close prints error info
        } else
		sig_ign(sig_list, sig_hand);

	for(;;) {
		int i;
		id = 0;		
		size = can_read(fd,&id,(char *)NULL,data,8);
		if (size < 0) 
			read_err++;	
		else {
			msg_count++;
			if (verbose) {
				printf(" %5lu ", id);		
				printf(" %u ", size);		
				for (i = 0; i < size; i++)
					printf("%03hhu ",data[i]);
				printf("\n");
				fflush(stdout);
			}
		}
	}
}
int main(int argc, char** argv) {
	MotionInfoPtr cmd;
	int saveStuff = -1;

	mcdc_load(); //load mcdc specific structures
	settings_init(); //init settings struct
	gonz_init(); //init main controller
    logging_init();

	for (int i=0; i<argc; i++) {
		if (strcmp(argv[i],"--test")==0) {
			printf("Test Mode\n");
			gonz_set_mode(GONZ_MODE_TEST);
		}
		if (strcmp(argv[i],"--save")==0) {
			printf("Saveing Stuff...\n");
			saveStuff = 1;
		}
	}





	enableCanEvent(&processCanMsg); //set the can calback

	int caninited = can_init(); //init can communication

	printf("Can Init Status:%d\n",caninited);
	can_startListener(); //start can listener thread
	int initialised = 0;

	if (gonz_get_mode() == GONZ_MODE_NORMAL) {
		printf("Spica Init:\n");
	    RosHelper::initialize();
	}
    //RosHelper::sendInfo("Motion Initialiasing...");
	do {
        initialised = mcdc_init_controllers(); //init the controllers
		if (!initialised) {
                can_close();
                //RosHelper::sendWarning("Cannot Init Controllers");
                usleep(500000);
                can_init();
        }
	} while(!initialised);

	if(saveStuff > 0) {
		printf("Saving...\n");
		mcdc_save_all();
		sleep(5);
		exit(1);
	}
    //RosHelper::sendInfo("Motion Initialiased.");
	//unsigned char buffer[5] = {CAN_CMD_SET_VELOCITY,0,0,0,0};
	//INT2BYTEPOS(12*60,buffer,1);
        gonz_state.currentMotionGoal.x = 0;
        gonz_state.currentMotionGoal.y = 0;
        gonz_state.currentMotionGoal.rotation = 0;

	while(1) {
		//char charbuf[256];
	    //writeCanMsg(CAN_ID_PDO2_CMD,1,buffer,5);

        /*
        gonz_state.currentMotionGoal.x = 100;
        gonz_state.currentMotionGoal.y = 0;
        gonz_state.currentMotionGoal.rotation = 0;
        */
		switch(gonz_get_mode()) {
			case GONZ_MODE_NORMAL:
				cmd = RosHelper::getMotion();
				if (cmd) {
//				    printf("GOT COMMAND\n");
					gonz_state.currentMotionGoal.rotation=(cmd->getRotation()*1024);
					gonz_state.currentMotionGoal.x=(cmd->getTranslation()*cos(cmd->getAngle()));
					gonz_state.currentMotionGoal.y=(cmd->getTranslation()*sin(cmd->getAngle()));
				}
				printf("MM CURCMD %f\t%f\t%f\n",gonz_state.currentMotionGoal.x,gonz_state.currentMotionGoal.y,gonz_state.currentMotionGoal.rotation);
		        gonz_main();
				break;
			case GONZ_MODE_TEST:
                gonz_test_loop();
				break;
			default:
				printf("Unknown Mode, doing nothing\n");

		}
		cout<<"PCanGonzales::main logging data"<<endl;
		logData();
        mcdc_query_infos();

 		usleep(current_settings.controllerLoopTime*1000);
	}
	can_close();
	return 0;
}
示例#7
0
RudderMotor::~RudderMotor() 
{
	emergency_stop();
    can_close();
}
示例#8
0
int main(int argc, char **argv)
{
	int size;
	int fd;
	unsigned long id;  
	unsigned char data[8];
        char *port = "/dev/can1";	/// use "1" to "4" for steinhoff
        int opt;
	int status;
	int read_err = 0;
	int msg_count = 0;
	timestamp_t start_ts, end_ts, diff_ts; /// report start and end times
	can_err_count_t errs;

        while ((opt = getopt(argc, argv, "p:")) != -1) {
                switch (opt) {
                  case 'p':
                        port = strdup(optarg);
                        break;
                  default:
                        printf("Usage: %s -p <port>\n", argv[0]);
                        exit(1);
                }
        }

	fd = can_open(port, O_RDONLY);

	if (fd == -1)
		exit(EXIT_FAILURE);	// error message printed by can_open 

	// Save starting time
	get_current_timestamp(&start_ts);

        if(setjmp(exit_env) != 0) {
		unsigned int i;
		get_current_timestamp(&end_ts);
		print_timestamp(stdout, &start_ts);
		printf(" ");
		print_timestamp(stdout, &end_ts);
		printf(" ");
		decrement_timestamp(&end_ts, &start_ts, &diff_ts);
		print_timestamp(stdout, &diff_ts); 	
		printf("\n");

		errs = can_get_errs(fd);

		if (read_err > 0)
			printf(" %d client read errors\n", read_err);

		printf("Error counts  at finish:\n");
		printf("intr_in_handler_count %u\n", errs.intr_in_handler_count);
		printf("rx_interrupt_count %u\n", errs.rx_interrupt_count);
		printf("rx_message_lost_count %u\n", errs.rx_message_lost_count);
		printf("tx_interrupt_count %u\n", errs.tx_interrupt_count);
		printf("shadow_buffer_count %u\n", errs.shadow_buffer_count);

		if (fd != -1)
			status = can_close(&fd);

		for (i = 0; i < msg_count; i++) { 
			save_data_t *p = &saved_data[i];
			printf("%u ", i);
			printf("%hhu ", p->can_id); 
			printf("%hd ", p->longitudinal);
			printf("%hd ", p->vertical);
			printf("%hd ", p->lateral);
			printf("%hhu ", p->thread_counter); 
			printf("%hhu ",	p->sensor_group_counter); 
			printf("\n");
		}

		if (status != -1)			
			exit(EXIT_SUCCESS);
		else
			exit(EXIT_FAILURE);	// can_close prints error info
        } else
		sig_ign(sig_list, sig_hand);

	/// Clear error counts in driver
	errs =  can_clear_errs(fd);

	printf("Error counts at start:\n");
	printf("intr_in_handler_count %u\n", errs.intr_in_handler_count);
	printf("rx_interrupt_count %u\n", errs.rx_interrupt_count);
	printf("rx_message_lost_count %u\n", errs.rx_message_lost_count);
	printf("tx_interrupt_count %u\n", errs.tx_interrupt_count);
	printf("shadow_buffer_count %u\n", errs.shadow_buffer_count);

	for(;;) {
		id = 0;		
		size = can_read(fd,&id,(char *)NULL,data,8);

		if (size < 0) 
			read_err++;	
		else {
			saved_data[msg_count].can_id = id;
			saved_data[msg_count].longitudinal = 
				(*(short *) &data[2]);
			saved_data[msg_count].vertical = 
				(*(short *) &data[0]);
			saved_data[msg_count].lateral = 
				(*(short *) &data[6]);
			saved_data[msg_count].sensor_group_counter = 
				 data[4];
			saved_data[msg_count].thread_counter = 
				 data[5];
		}
		msg_count++;
		if (msg_count >= max_count)
			longjmp(exit_env, 1);
	}
}