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