int main() { pid_t PID; int status; register_sig_handler(); setup(); runSim(); shutdown(); }
int main() { register_sig_handler(); ulib::periodic thdmgr; thdmgr.schedule_repeated(ulib::us_from_now(0), 1000000, print_thread, (void *)1); thdmgr.schedule_repeated(ulib::us_from_now(0), 3000000, print_thread, (void *)3); thdmgr.schedule_repeated(ulib::us_from_now(0), 5000000, print_thread, (void *)5); assert(thdmgr.start() == 0); while (cnt_1s < 12) sleep(1); thdmgr.stop_and_join(); printf("passed\n"); return 0; }
int main(int argc, char *argv[]) { long ins = 1000000; long get = 50000000; uint64_t seed = time(NULL); if (argc > 1) ins = atol(argv[1]); if (argc > 2) get = atol(argv[2]); RAND_NR_INIT(u, v, w, seed); register_sig_handler(); constant_insert(ins, get); printf("passed\n"); return 0; }
int main(int argc, char **argv) { int opt; int i2c_bus = DEFAULT_I2C_BUS; int sample_rate = DEFAULT_SAMPLE_RATE_HZ; mag_mode = -1; memset(calFile, 0, sizeof(calFile)); while ((opt = getopt(argc, argv, "b:s:y:amh")) != -1) { switch (opt) { case 'b': i2c_bus = strtoul(optarg, NULL, 0); if (errno == EINVAL) usage(argv[0]); if (i2c_bus < 0 || i2c_bus > 5) usage(argv[0]); break; case 's': sample_rate = strtoul(optarg, NULL, 0); if (errno == EINVAL) usage(argv[0]); if (sample_rate < 2 || sample_rate > 50) usage(argv[0]); break; case 'y': if (strlen(optarg) >= sizeof(calFile)) { printf("Choose a shorter path for the cal file\n"); usage(argv[0]); } strcpy(calFile, optarg); break; case 'a': if (mag_mode != -1) usage(argv[0]); mag_mode = 0; break; case 'm': if (mag_mode != -1) usage(argv[0]); mag_mode = 1; break; case 'h': default: usage(argv[0]); break; } } if (mag_mode == -1) usage(argv[0]); register_sig_handler(); if (mpu9150_init(i2c_bus, sample_rate, 0)) exit(1); read_loop(sample_rate); if (strlen(calFile) == 0) { if (mag_mode) strcpy(calFile, "magcal.txt"); else strcpy(calFile, "accelcal.txt"); } write_cal(); mpu9150_exit(); return 0; }
int main(int argc, char **argv) { int opt, len; int i2c_bus = DEFAULT_I2C_BUS; int sample_rate = DEFAULT_SAMPLE_RATE_HZ; int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR; int verbose = 0; char *mag_cal_file = NULL; char *accel_cal_file = NULL; MQTT_init(); while ((opt = getopt(argc, argv, "b:s:y:a:m:vh")) != -1) { switch (opt) { case 'b': i2c_bus = strtoul(optarg, NULL, 0); if (errno == EINVAL) usage(argv[0]); if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) usage(argv[0]); break; case 's': sample_rate = strtoul(optarg, NULL, 0); if (errno == EINVAL) usage(argv[0]); if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) usage(argv[0]); break; case 'y': yaw_mix_factor = strtoul(optarg, NULL, 0); if (errno == EINVAL) usage(argv[0]); if (yaw_mix_factor < 0 || yaw_mix_factor > 100) usage(argv[0]); break; case 'a': len = 1 + strlen(optarg); accel_cal_file = (char *)malloc(len); if (!accel_cal_file) { perror("malloc"); exit(1); } strcpy(accel_cal_file, optarg); break; case 'm': len = 1 + strlen(optarg); mag_cal_file = (char *)malloc(len); if (!mag_cal_file) { perror("malloc"); exit(1); } strcpy(mag_cal_file, optarg); break; case 'v': verbose = 1; break; case 'h': default: usage(argv[0]); break; } } register_sig_handler(); mpu9150_set_debug(verbose); if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor)) exit(1); set_cal(0, accel_cal_file); set_cal(1, mag_cal_file); if (accel_cal_file) free(accel_cal_file); if (mag_cal_file) free(mag_cal_file); read_loop(sample_rate); mpu9150_exit(); MQTTAsync_destroy(&client); return 0; }
int main(void) { FILE * fp; char * line = NULL; size_t len = 0; ssize_t read; int line_counter=0; unsigned long qw,qx,qy,qz; quaternion_t q_t; vector3d_t v_t; register_sig_handler(); while(!done) { fp = fopen("/dev/imu", "r"); if (fp == NULL) exit(-1); line_counter=0; while ((read = getline(&line, &len, fp)) != -1) { switch (line_counter++) { case 1: sscanf(line,"%*[^0123456789]%lu",&qw); //printf("Q_W: %lu\n",qw); break; case 2: sscanf(line,"%*[^0123456789]%lu",&qx); //printf("Q_X: %lu\n",qx); ; break; case 3: sscanf(line,"%*[^0123456789]%lu",&qy); //printf("Q_Y: %lu\n",qy); break; case 4: sscanf(line,"%*[^0123456789]%lu",&qz); //printf("Q_Z: %lu\n",qz); break; case 6: //printf("Time\n"); break; case 9: //printf("Mag_X\n"); break; case 10: //printf("Mag_Y\n"); break; case 11: //printf("Mag_Z\n"); break; case 12: //printf("Mag Time\n"); break; default: //printf("%d ist irgendwas\n",line_counter); break; } } if (line) free(line); fclose(fp); //printf("\rW: %lu X: %lu Y: %lu Z: %lu ",qw,qx,qy,qz); //fflush(stdout); q_t[QUAT_W] = qw; q_t[QUAT_X] = qx; q_t[QUAT_Y] = qy; q_t[QUAT_Z] = qz; quaternionToEuler(q_t, v_t); printf("\rX: %f Y: %f Z: %f",v_t[VEC3_X] ,v_t[VEC3_Y] ,v_t[VEC3_Z]); fflush(stdout); usleep(100000); } exit(1); }
int main(int argc, char **argv) { ros::init(argc, argv, "mpu9150_node"); ros::NodeHandle n; //creates publisher of IMU message ros::Publisher pub = n.advertise<sensor_msgs::Imu>("imu/data_raw", 1000); //creates publisher of Magnetic FIeld message ros::Publisher pubM = n.advertise<sensor_msgs::MagneticField>("imu/mag", 1000); ros::Rate loop_rate(10); /* Init the sensor the values are hardcoded at the local_defaults.h file */ int opt, len; int i2c_bus = DEFAULT_I2C_BUS; int sample_rate = DEFAULT_SAMPLE_RATE_HZ; int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR; int verbose = 0; char *mag_cal_file = NULL; char *accel_cal_file = NULL; unsigned long loop_delay; //creates object of mpudata_t mpudata_t mpu; // Initialize the MPU-9150 register_sig_handler(); mpu9150_set_debug(verbose); if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor)) exit(1); set_cal(0, accel_cal_file); set_cal(1, mag_cal_file); if (accel_cal_file) free(accel_cal_file); if (mag_cal_file) free(mag_cal_file); if (sample_rate == 0) return -1; // ROS loop config loop_delay = (1000 / sample_rate) - 2; printf("\nEntering MPU read loop (ctrl-c to exit)\n\n"); linux_delay_ms(loop_delay); /** * A count of how many messages we have sent. This is used to create * a unique string for each message. */ int count = 0; while (ros::ok()) { //creates objects of each message type //IMU Message sensor_msgs::Imu msgIMU; std_msgs::String header; geometry_msgs::Quaternion orientation; geometry_msgs::Vector3 angular_velocity; geometry_msgs::Vector3 linear_acceleration; //magnetometer message sensor_msgs::MagneticField msgMAG; geometry_msgs::Vector3 magnetic_field; //modified to output in the format of IMU message if (mpu9150_read(&mpu) == 0) { //IMU Message //sets up header for IMU message msgIMU.header.seq = count; msgIMU.header.stamp.sec = ros::Time::now().toSec(); msgIMU.header.frame_id = "/base_link"; //adds data to the sensor message //orientation msgIMU.orientation.x = mpu.fusedQuat[QUAT_X]; msgIMU.orientation.y = mpu.fusedQuat[QUAT_Y]; msgIMU.orientation.z = mpu.fusedQuat[QUAT_Z]; msgIMU.orientation.w = mpu.fusedQuat[QUAT_W]; //msgIMU.orientation_covariance[0] = //angular velocity msgIMU.angular_velocity.x = mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE; msgIMU.angular_velocity.y = mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE; msgIMU.angular_velocity.z = mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE; //msgIMU.angular_velocity_covariance[] = //linear acceleration msgIMU.linear_acceleration.x = mpu.calibratedAccel[VEC3_X]; msgIMU.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y]; msgIMU.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z]; //msgIMU.linear_acceleration_covariance[] = //Magnetometer Message //sets up header msgMAG.header.seq = count; msgMAG.header.stamp.sec = ros::Time::now().toSec(); msgMAG.header.frame_id = "/base_link"; //adds data to magnetic field message msgMAG.magnetic_field.x = mpu.calibratedMag[VEC3_X]; msgMAG.magnetic_field.y = mpu.calibratedMag[VEC3_Y]; msgMAG.magnetic_field.z = mpu.calibratedMag[VEC3_Z]; //fills the list with zeros as per message spec when no covariance is known //msgMAG.magnetic_field_covariance[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; } //publish both messages pub.publish(msgIMU); pubM.publish(msgMAG); ros::spinOnce(); loop_rate.sleep(); ++count; } mpu9150_exit(); return 0; }
/** * new connecion comes, copy file descroptor of new connecton to child * process , parent process select * @return 0 */ int main(int argc, char **argv) { int res; res = register_sig_handler(); if (res < 0) err_sys("register_sig_handler is err"); my_getopt(argc, argv); int pipefd[2]; int socket_fd; res = srv_socket_init(&socket_fd, 50, PORT); if (res < 0) err_sys("srv_socket_init err"); fd_set myset; FD_ZERO(&myset); FD_SET(socket_fd, &myset); int max = socket_fd; arraychild = (child_t *) malloc(sizeof(child_t) * childnum); memset(arraychild, 0, sizeof(child_t) * childnum); for (int i = 0 ; i < childnum; i++) { res = socketpair(AF_LOCAL, SOCK_STREAM, 0, pipefd); if (res < 0) err_sys("socketpair is err"); arraychild[i].child_pipefd = pipefd[0]; FD_SET(pipefd[0], &myset); if (pipefd[0] > max) max = pipefd[0]; int pid = fork(); if (pid < 0) err_sys("fork err"); if (pid == 0) { srv_socket_destory(&socket_fd); int connfd; int childpid = getpid(); while(1) { char c; res = read_fd(pipefd[1], &c, 1, &connfd); if (res < 0) err_sys("read_fd err"); fprintf(stdout, "pid is %d, accept success.\n",childpid); child_process(connfd); write(pipefd[1], "", 1); srv_socket_close(&connfd); } //exit(0); } else { arraychild[i].child_pid = pid; } } struct sigaction myact; myact.sa_handler = sig_int_handler; if (sigaction(SIGINT, &myact, NULL) < 0) err_sys("sigaction err"); int navail = childnum; fd_set rset ; int newfd; int i = 0; while(1) { rset = myset; if (navail <=0 ) FD_CLR(socket_fd, &rset); select(max + 1, &rset, NULL, NULL, NULL); if (FD_ISSET(socket_fd, &rset)) { newfd = accept(socket_fd, NULL, NULL); for (i = 0; i < childnum; i++) { if (arraychild[i].child_status == 0) break; } res = write_fd(arraychild[i].child_pipefd, "", 1, newfd); if (res < 0) { continue; } srv_socket_close(&newfd); arraychild[i].child_status = 1; arraychild[i].child_count++; navail--; } for (int i = 0; i < childnum; i++) { char c; if (FD_ISSET(arraychild[i].child_pipefd, &rset)) { read(arraychild[i].child_pipefd, &c, 1); arraychild[i].child_status = 0; navail++; } } } return 0; }
int main(int argc, char **argv) { int opt, delay_ms, adc, i; struct timeval start, end; int adc_list[NUM_ADC]; register_sig_handler(); delay_ms = 50; while ((opt = getopt(argc, argv, "d:h")) != -1) { switch (opt) { case 'd': delay_ms = atoi(optarg); if (delay_ms < 1) { printf("Invalid delay %d. Must be greater then zero.\n", delay_ms); usage(argv[0]); } break; case 'h': default: usage(argv[0]); break; } } // now get the adc list if (optind == argc) { printf("A list of ADCs is required\n"); usage(argv[0]); } memset(adc_list, 0, sizeof(adc_list)); for (i = optind; i < argc; i++) { adc = atoi(argv[i]); if (adc < 2 || adc > 7) { printf("adc %d is out of range\n", adc); usage(argv[0]); } adc -= 2; if (adc_list[adc]) { printf("adc %d listed more then once\n", adc + 2); usage(argv[0]); } adc_list[adc] = 1; } if (gettimeofday(&start, NULL) < 0) { perror("gettimeofday: start"); return 1; } int count = loop(delay_ms, adc_list); if (gettimeofday(&end, NULL) < 0) perror("gettimeofday: end"); else show_elapsed(&start, &end, count); return 0; }