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;
}
}
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
}
/////////////////////////////////////////////////////////////////////////////////////////
// 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");
}
}
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;
}
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;
}
RudderMotor::~RudderMotor()
{
emergency_stop();
can_close();
}
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);
}
}
