int main(int argc, char ** argv) { i2c_bus_open(); motor_ctrl_init(); int pos_left = 0, pos_right = 0; if (argc == 2) { pos_left = atoi(argv[1]) * PULSES_PR_DEG; } else if (argc == 3) { pos_left = atoi(argv[1]) * PULSES_PR_DEG; pos_right = atoi(argv[2]) * PULSES_PR_DEG; } printf("Left: %d, right: %d\n", pos_left, pos_right); motor_ctrl_set_state(STATE_POSITION); motor_ctrl_set_dir(DIR_LEFT_FORWARD | DIR_RIGHT_FORWARD); motor_ctrl_goto_position(pos_left, pos_right); motor_ctrl_wait(200); printf("Done.\n"); return 0; }
int omap_i2c_bus_init(void) { i2c_bus_interface_setup(&i2c_bus_interface, omap_i2c_bus_interface_setup); i2c_bus_context_setup(&i2c_bus_context, "omap_i2c", "/dev/i2c-3"); i2c_bus_setup(&i2c_bus, &i2c_bus_interface, &i2c_bus_context); return i2c_bus_open(&i2c_bus); }
void init(char *_i2c_bus) { assert(_i2c_bus); if (initialized) return; i2c_bus_open(&i2c_bus, _i2c_bus); i2c_dev_init(&i2c_dev, &i2c_bus, 0x3d); ssd1306_init(&ssd, &i2c_dev); initialized = true; }
int main(void) { i2c_bus_t bus; int ret = i2c_bus_open(&bus, "/dev/i2c-0"); if (ret < 0) { printf("could not open i2c bus", ret); return EXIT_FAILURE; } mpu6050_dev_t mpu; mpu6050_init(&mpu, &bus, MPU6050_DLPF_CFG_94_98Hz, MPU6050_FS_SEL_500, MPU6050_AFS_SEL_4G); while (1) { mpu6050_read(&mpu); } return 0; }
int main_i2c(void) { THROW_BEGIN(); void *gps_socket = scl_get_socket("gps"); THROW_IF(gps_socket == NULL, -EIO); int64_t hwm = 1; zmq_setsockopt(gps_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm)); void *sats_socket = scl_get_socket("sats"); THROW_IF(sats_socket == NULL, -EIO); i2c_bus_t bus; i2c_dev_t device; uint8_t data_w[128]; uint8_t data_r[128]; if (i2c_bus_open(&bus, "/dev/i2c-1")) { syslog(LOG_CRIT, "could not open i2c device"); exit(EXIT_FAILURE); } i2c_dev_init(&device, &bus, I2C_GPS_ADDRESS); msgpack_sbuffer *msgpack_buf = msgpack_sbuffer_new(); THROW_IF(msgpack_buf == NULL, -ENOMEM); msgpack_packer *pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write); THROW_IF(pk == NULL, -ENOMEM); while (1) { msleep(200); data_w[0] = I2C_GPS_STATUS; i2c_xfer(&device, 1, data_w, 1, data_r); msgpack_sbuffer_clear(msgpack_buf); int status = data_r[0]; int fix = 0; if (status & I2C_GPS_STATUS_2DFIX) fix = 2; if (status & I2C_GPS_STATUS_3DFIX) fix = 3; if (fix == 2) msgpack_pack_array(pk, 7); /* 2d fix */ else if (fix == 3) msgpack_pack_array(pk, 9); /* 3d fix */ else msgpack_pack_array(pk, 1); /* no fix */ data_w[0] = I2C_GPS_TIME; i2c_xfer(&device, 1, data_w, 4, data_r); long time = (((long) data_r[3]) << 24) | (((long) data_r[2]) << 16) | (((long) data_r[1]) << 8) | (data_r[0]); time /= 1000; char *time_str = ctime(&time); size_t len = strlen(time_str); msgpack_pack_raw(pk, len); msgpack_pack_raw_body(pk, time_str, len); /* gps array index 0 */ if (fix == 2 || fix == 3) { data_w[0] = I2C_GPS_LOCATION; i2c_xfer(&device, 1, data_w, 8, data_r); PACKD(( (((long) data_r[3]) << 24) | (((long) data_r[2]) << 16) | (((long) data_r[1]) << 8) | (data_r[0])) / 10000000.0); /* latitude, gps array index 1 */ PACKD(( (((long) data_r[7]) << 24) | (((long) data_r[6]) << 16) | (((long) data_r[5]) << 8) | (data_r[4])) / 10000000.0); /* logitude, gps array index 2 */ PACKI((status & I2C_GPS_STATUS_NUMSATS) >> 4); /* gps array index 3 */ data_w[0] = I2C_GPS_GROUND_SPEED; i2c_xfer(&device, 1, data_w, 2, data_r); PACKF(((float)((data_r[1] << 8) | data_r[0])) / 100.0 * 1.94384); /* gps array index 4 */ data_w[0] = I2C_GPS_GROUND_SPEED; i2c_xfer(&device, 1, data_w, 2, data_r); PACKF((data_r[1] << 8) | data_r[0]); /* gps array index 5 */ PACKF(0 /* HDOP */); /* gps array index 6 */ } if (fix == 3) { data_w[0] = I2C_GPS_ALTITUDE; i2c_xfer(&device, 1, data_w, 2, data_r); PACKF(((data_r[1]) << 8) | data_r[0]); /* gps array index 7 */ PACKF(0 /* VDOP */); /* gps array index 8 */ } scl_copy_send_dynamic(gps_socket, msgpack_buf->data, msgpack_buf->size); }
int main(void) { i2c_bus_t bus; int ret = i2c_bus_open(&bus, "/dev/i2c-3"); if (ret < 0) { fatal("could not open i2c bus", ret); return EXIT_FAILURE; } /* ITG: */ itg3200_dev_t itg; itg_again: ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ); if (ret < 0) { fatal("could not inizialize ITG3200", ret); if (ret == -EAGAIN) { goto itg_again; } return EXIT_FAILURE; } /* BMA: */ bma180_dev_t bma; bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ); /* HMC: */ hmc5883_dev_t hmc; hmc5883_init(&hmc, &bus); /* MS: */ ms5611_dev_t ms; ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096); if (ret < 0) { fatal("could not inizialize MS5611", ret); return EXIT_FAILURE; } pthread_t thread; pthread_create(&thread, NULL, ms5611_reader, &ms); /* initialize AHRS filter: */ madgwick_ahrs_t madgwick_ahrs; madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA); interval_t interval; interval_init(&interval); float init = START_BETA; udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0); /* kalman filter: */ kalman_t kalman1, kalman2, kalman3; kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0); kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0); kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0); vec3_t global_acc; /* x = N, y = E, z = D */ int init_done = 0; int converged = 0; sliding_avg_t *avg[3]; avg[0] = sliding_avg_create(1000, 0.0); avg[1] = sliding_avg_create(1000, 0.0); avg[2] = sliding_avg_create(1000, -9.81); float alt_rel_last = 0.0; int udp_cnt = 0; while (1) { int i; float dt = interval_measure(&interval); init -= BETA_STEP; if (init < FINAL_BETA) { init = FINAL_BETA; init_done = 1; } madgwick_ahrs.beta = init; /* sensor data acquisition: */ itg3200_read_gyro(&itg); bma180_read_acc(&bma); hmc5883_read(&hmc); /* state estimates and output: */ euler_t euler; madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt); quat_t q_body_to_world; quat_copy(&q_body_to_world, &madgwick_ahrs.quat); quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world); for (i = 0; i < 3; i++) { global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]); } if (init_done) { kalman_in_t kalman_in; kalman_in.dt = dt; kalman_in.pos = 0; kalman_out_t kalman_out; kalman_in.acc = global_acc.x; kalman_run(&kalman_out, &kalman1, &kalman_in); kalman_in.acc = global_acc.y; kalman_run(&kalman_out, &kalman2, &kalman_in); kalman_in.acc = -global_acc.z; pthread_mutex_lock(&mutex); kalman_in.pos = alt_rel; pthread_mutex_unlock(&mutex); kalman_run(&kalman_out, &kalman3, &kalman_in); if (!converged) { if (fabs(kalman_out.pos - alt_rel) < 0.1) { converged = 1; fprintf(stderr, "init done\n"); } } if (converged) // && udp_cnt++ > 10) { if (udp_cnt++ == 10) { char buffer[1024]; udp_cnt = 0; int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3, global_acc.x, global_acc.y, global_acc.z); udp_socket_send(socket, buffer, len); } printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos); fflush(stdout); } } } return 0; }