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;
}
