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