void IMU_Init(void) {
// Check which IMU is installed by trying to read ID registers
unsigned char id_adxl = I2C_ReadRegister(ADXL_ADDRESS, 0x00);
unsigned char id_itg = I2C_ReadRegister(ITG_ADDRESS, 0x00);
unsigned char id_hmc[3];
I2C_ReadMultipleRegisters(HMC_ADDRESS, 0x0A, &id_hmc[0], 3);
if ((id_adxl == 0xe5) &&
((id_itg & 0x7e) == ITG_ADDRESS) &&
(id_hmc[0] == 'H' && id_hmc[1] == '4' && id_hmc[2] == '3')) {
dbgu_printf("Found digital IMU!\r\n");
IMU_accel_init();
IMU_gyro_init();
IMU_magnet_init();
TC_DelayMs(100);
double temp = IMU_get_temp();
dbgu_printf("Current temperature: %f °C\r\n", temp);
imu_digital = 1;
} else {
imu_digital = 0;
}
// Todo: load values from eeprom
IMU_Update();
IMU_Calibrate_Accel();
#ifdef USE_KALMAN_FILTER
init_Gyro1DKalman(&filter_roll, Q_ACCEL, Q_GYRO, R_ACCEL);
init_Gyro1DKalman(&filter_pitch, Q_ACCEL, Q_GYRO, R_ACCEL);
float accX = ((float)(imu_AccelZ_raw - 512)) / 128.0f;
float accY = ((float)(imu_AccelY_raw - 512)) / 128.0f;
float accZ = ((float)(imu_AccelX_raw - 512)) / 128.0f;
pitch = atan2(accX, sqrt(accY * accY + accZ * accZ));
roll = atan2(accY, -accZ);
filter_roll.x_angle = roll;
filter_pitch.x_angle = pitch;
#endif
}
int main()
{
char buffer[20];
int i;
float tmp, roll_angle;
float acc_gyro = 0, dt;
struct Gyro1DKalman filter_roll;
struct Gyro1DKalman filter_pitch;
init_microcontroller();
adc_init();
adc_start();
timer_init_ms();
uart1_open();
uart1_puts("Device initialized\n\r");
uart1_puts("Entering main loop\n\r");
init_Gyro1DKalman(&filter_roll, 0.0001, 0.0003, 0.69);
while(1)
{
dt = timer_dt(); // time passed in s since last call
// execute kalman filter
tmp = PredictAccG_roll(accelero_z(), accelero_y(), accelero_x());
ars_predict(&filter_roll, gyro_roll_rad(), dt); // Kalman predict
roll_angle = ars_update(&filter_roll, tmp); // Kalman update + result (angle)
//PrintAll(accelero_x(), accelero_y(), accelero_z(), gyro_roll_rad(), gyro_pitch_rad(), r);
PrintForVbApp(roll_angle / 3.14*180.0, tmp);
//PrintForStabilizer(r);
}
uart1_close();
return 0;
}
int main()
{
struct Gyro1DKalman filter_roll;
init_Gyro1DKalman(&filter_roll, 0.1,0.3, 1);
Init();
PIO_SODR = (1 << switch_pin);
/* 3초 대기 */
delay_ms(3000);
PIO_CODR = (1 << switch_pin);
INIT_UART0();
while(1)
{
if(cnt_ == 100)
{
Kalman(&filter_roll);
}
}
return 0;
}
