//init dan start itg3200
void ITG3200_Init_Start(int i2c_bus_handle, unsigned char sensor_addr, unsigned char *statusPeripheralOutput)
{
if(i2c_bus_handle>=0)
{
//init itg3200
if(!itg3200_init(i2c_bus_handle, sensor_addr))
{
printf("init itg3200 fail\n");
//return errno;
*statusPeripheralOutput &= ~(1<<BIT_ITG3200_READY);
}
else
{
*statusPeripheralOutput |= (1<<BIT_ITG3200_READY);
}
#if VERBOSE==1
printf("init itg3200 success and started\r\n");
#endif
}
else
{
#if VERBOSE==1
printf("i2c bus not ready\r\n");
#endif
}
}
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// ITG3200
itg3200_init(&imu_navgo.itg, &(IMU_NAVGO_I2C_DEV), ITG3200_ADDR_ALT);
// change the default configuration
imu_navgo.itg.config.smplrt_div = NAVGO_GYRO_SMPLRT_DIV; // 500Hz sample rate since internal is 1kHz
imu_navgo.itg.config.dlpf_cfg = NAVGO_GYRO_LOWPASS;
/////////////////////////////////////////////////////////////////////
// ADXL345
adxl345_i2c_init(&imu_navgo.adxl, &(IMU_NAVGO_I2C_DEV), ADXL345_ADDR_ALT);
// change the default data rate
imu_navgo.adxl.config.rate = NAVGO_ACCEL_RATE;
/////////////////////////////////////////////////////////////////////
// HMC58XX
hmc58xx_init(&imu_navgo.hmc, &(IMU_NAVGO_I2C_DEV), HMC58XX_ADDR);
#if NAVGO_USE_MEDIAN_FILTER
// Init median filters
InitMedianFilterRatesInt(median_gyro);
InitMedianFilterVect3Int(median_accel);
InitMedianFilterVect3Int(median_mag);
#endif
imu_navgo.gyr_valid = FALSE;
imu_navgo.acc_valid = FALSE;
imu_navgo.mag_valid = FALSE;
}
void imu_impl_init(void)
{
imu_aspirin.accel_valid = FALSE;
imu_aspirin.gyro_valid = FALSE;
imu_aspirin.mag_valid = FALSE;
/* Set accel configuration */
adxl345_i2c_init(&imu_aspirin.acc_adxl, &(ASPIRIN_I2C_DEV), ADXL345_ADDR);
// set the data rate
imu_aspirin.acc_adxl.config.rate = ASPIRIN_ACCEL_RATE;
/// @todo drdy int handling for adxl345
//imu_aspirin.acc_adxl.config.drdy_int_enable = TRUE;
// With CS tied high to VDD I/O, the ADXL345 is in I2C mode
GPIO_ARCH_SET_SPI_CS_HIGH();
/* Gyro configuration and initalization */
itg3200_init(&imu_aspirin.gyro_itg, &(ASPIRIN_I2C_DEV), ITG3200_ADDR);
/* change the default config */
// Aspirin sample rate divider
imu_aspirin.gyro_itg.config.smplrt_div = ASPIRIN_GYRO_SMPLRT_DIV;
// digital low pass filter
imu_aspirin.gyro_itg.config.dlpf_cfg = ASPIRIN_GYRO_LOWPASS;
/// @todo eoc interrupt for itg3200, polling for now (including status reg)
/* interrupt on data ready, idle high, latch until read any register */
//itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
/* initialize mag and set default options */
hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR);
#ifdef IMU_ASPIRIN_VERSION_1_0
imu_aspirin.mag_hmc.type = HMC_TYPE_5843;
#endif
}
int drotek_marg_init(drotek_marg_t *marg, i2c_bus_t *bus)
{
THROW_BEGIN();
THROW_ON_ERR(itg3200_init(&marg->itg, bus, ITG3200_DLPF_98HZ));
THROW_ON_ERR(bma180_init(&marg->bma, bus, BMA180_RANGE_4G, BMA180_BW_40HZ));
THROW_ON_ERR(hmc5883_init(&marg->hmc, bus));
THROW_END();
}
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// ITG3200
itg3200_init();
/////////////////////////////////////////////////////////////////////
// ADXL345
adxl345_init();
}
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// ITG3200
itg3200_init();
/////////////////////////////////////////////////////////////////////
// ADXL345
adxl345_init();
/////////////////////////////////////////////////////////////////////
// HMC58XX
hmc58xx_init();
}
void init_peripherals()
{
// LED
DDRC |= (1<<2);
// accel pins
lis3l_init();
// UART
uartInit();
uartSetBaudRate(19200);
uartSetFrameFormat(8, 0, 1);
rprintfInit(uartAddToTxBuffer);
cbi(DDRB, 1); // XBee CTS on PB1
// I2C
itg3200_i2cInit(200);
sbi(PORTC, 0); // i2c SCL on ATmega163,323,16,32,etc
sbi(PORTC, 1); // i2c SDA on ATmega163,323,16,32,etc
cbi(TWCR, TWIE); // disable interrupt
// itg3200_i2cSetBitrate(200); // todo, check if if w ecan do 200
// SPI
mySpiInit();
// a2d
a2dInit();
a2dSetReference(ADC_REFERENCE_256V);
a2dSetChannel(7);
cbi(PORTA, 7);
cbi(DDRA, 7);
a2dStartConvert();
_delay_ms(50);
// accel
BOOL accelOkay = lis3l_Reset();
itg3200_init();
// switch xbee to higher baudrate
// rprintfStr("+++");
// _delay_ms(55);
// rprintfStr("ATBD6,CN\r");
}
void imu_impl_init(void)
{
/////////////////////////////////////////////////////////////////////
// ITG3200
itg3200_init(&imu_umarim.itg, &(IMU_UMARIM_I2C_DEV), ITG3200_ADDR_ALT);
// change the default configuration
imu_umarim.itg.config.smplrt_div = UMARIM_GYRO_SMPLRT_DIV;
imu_umarim.itg.config.dlpf_cfg = UMARIM_GYRO_LOWPASS;
/////////////////////////////////////////////////////////////////////
// ADXL345
adxl345_i2c_init(&imu_umarim.adxl, &(IMU_UMARIM_I2C_DEV), ADXL345_ADDR_ALT);
// change the default data rate
imu_umarim.adxl.config.rate = UMARIM_ACCEL_RATE;
imu_umarim.adxl.config.range = UMARIM_ACCEL_RANGE;
}
void imu_impl_init(void)
{
/* Set accel configuration */
adxl345_i2c_init(&imu_ppzuav.acc_adxl, &(IMU_PPZUAV_I2C_DEV), ADXL345_ADDR);
/* set the data rate */
imu_ppzuav.acc_adxl.config.rate = IMU_PPZUAV_ACCEL_RATE;
/* Gyro configuration and initalization */
itg3200_init(&imu_ppzuav.gyro_itg, &(IMU_PPZUAV_I2C_DEV), ITG3200_ADDR);
/* change the default config */
imu_ppzuav.gyro_itg.config.smplrt_div = IMU_PPZUAV_GYRO_SMPLRT_DIV;
imu_ppzuav.gyro_itg.config.dlpf_cfg = IMU_PPZUAV_GYRO_LOWPASS;
/* initialize mag and set default options */
hmc58xx_init(&imu_ppzuav.mag_hmc, &(IMU_PPZUAV_I2C_DEV), HMC58XX_ADDR);
/* set the type to the old HMC5843 */
imu_ppzuav.mag_hmc.type = HMC_TYPE_5843;
}
void imu_impl_init(void)
{
imu_aspirin.accel_valid = FALSE;
imu_aspirin.gyro_valid = FALSE;
imu_aspirin.mag_valid = FALSE;
/* Set accel configuration */
adxl345_spi_init(&imu_aspirin.acc_adxl, &(ASPIRIN_SPI_DEV), ASPIRIN_SPI_SLAVE_IDX);
// set the data rate
imu_aspirin.acc_adxl.config.rate = ASPIRIN_ACCEL_RATE;
/// @todo drdy int handling for adxl345
//imu_aspirin.acc_adxl.config.drdy_int_enable = TRUE;
/* Gyro configuration and initalization */
itg3200_init(&imu_aspirin.gyro_itg, &(ASPIRIN_I2C_DEV), ITG3200_ADDR);
/* change the default config */
// Aspirin sample rate divider defaults to 533Hz
imu_aspirin.gyro_itg.config.smplrt_div = ASPIRIN_GYRO_SMPLRT_DIV;
// aspirin defaults to 8kHz internal with 256Hz low pass
imu_aspirin.gyro_itg.config.dlpf_cfg = ASPIRIN_GYRO_LOWPASS;
/// @todo eoc interrupt for itg3200, polling for now (including status reg)
/* interrupt on data ready, idle high, latch until read any register */
//itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
/* initialize mag and set default options */
hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR);
#ifdef IMU_ASPIRIN_VERSION_1_0
imu_aspirin.mag_hmc.type = HMC_TYPE_5843;
#endif
#if ASPIRIN_ARCH_INDEP
TODO("Arch dependent functions (accel and gyro eoc interrupt) not used for aspirin!")
#else
imu_aspirin_arch_init();
#endif
}
int main(int argc, char ** argv)
{
if(argc < 2)
{
fprintf(stderr, "Usage: %s <device>\n", argv[0]);
return -1;
}
//int fd = init_rs232(argv[1]);
int fd = init_i2c(argv[1], 0x69);
const bool init_success = itg3200_init(fd);
if(!init_success)
return -1;
uint8_t register_data[8];
while(true)
{
struct timespec time_before, time_after;
clock_gettime(CLOCK_MONOTONIC, &time_before);
itg3200_read_gyro_data(fd, register_data);
clock_gettime(CLOCK_MONOTONIC, &time_after);
int16_t rawtemp = itg3200_align_data(®ister_data[0]);
int16_t gyro_x = itg3200_align_data(®ister_data[2]);
int16_t gyro_y = itg3200_align_data(®ister_data[4]);
int16_t gyro_z = itg3200_align_data(®ister_data[6]);
fprintf(stdout, "%09d,%09d,%09d,%09d", time_before.tv_sec, time_before.tv_nsec, time_after.tv_sec, time_after.tv_nsec);
fprintf(stdout, ",%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x", register_data[0], register_data[1], register_data[2], register_data[3], register_data[4], register_data[5], register_data[6], register_data[7]);
fprintf(stdout, ",%d,%d,%d,%d", rawtemp, gyro_x, gyro_y, gyro_z);
fprintf(stdout, "\n");
fflush(stdout);
}
return 0;
};
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;
}
