void Kalman(struct Gyro1DKalman * filter_roll)
{
float temp=0;
float roll_angle = 0;
float pich_angle = 0;
float dt= 0.02;
/* dt =timer_dt(); // time passed in s since last call */
// execute kalman filter
temp = PredictAccG_roll(accelero_z(), accelero_y());
ars_predict(filter_roll, gyro_roll_degrees(), dt); // Kalman predict
roll_angle = ars_update(filter_roll, temp); // Kalman update + result (angle)
motor_pid_ctrl(roll_angle); // pid È£Ãâ
return ;
}
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;
}
void IMU_Update(void) {
static int fb_array[ACCEL_WINDOW_SIZE] = {512,};
static int rl_array[ACCEL_WINDOW_SIZE] = {512,};
static int buf_idx = 0;
int sum, idx;
if (imu_digital == 1) {
IMU_accel_update();
IMU_gyro_update();
IMU_magnet_update();
imu_FBGyro = imu_GyroY_raw - imu_FBGyroCenter;
imu_RLGyro = imu_GyroZ_raw - imu_RLGyroCenter;
imu_FBAccel = imu_AccelZ_raw;// - imu_FBAccelCenter;
imu_RLAccel = imu_AccelY_raw;// - imu_RLAccelCenter;
imu_FBMagnet = imu_MagnetX_raw;
imu_RLMagnet = imu_MagnetY_raw;
imu_UDMagnet = imu_MagnetZ_raw;
} else {
// Our gyro: +-500°/sec, DARwIn gyro: +-2000°/sec
imu_FBGyro = (int)ADC_GetChannel(1) - imu_FBGyroCenter;
imu_RLGyro = (int)ADC_GetChannel(2) - imu_RLGyroCenter;
imu_FBAccel = (int)ADC_GetChannel(3);// - imu_FBAccelCenter;
imu_RLAccel = (int)ADC_GetChannel(4);// - imu_RLAccelCenter;
}
fb_array[buf_idx] = imu_FBAccel;
sum = 0;
for(idx = 0; idx < ACCEL_WINDOW_SIZE; idx++) {
sum += fb_array[idx];
}
imu_FBAccelAverage = sum / ACCEL_WINDOW_SIZE;
rl_array[buf_idx] = imu_RLAccel;
sum = 0;
for(idx = 0; idx < ACCEL_WINDOW_SIZE; idx++) {
sum += rl_array[idx];
}
imu_RLAccelAverage = sum / ACCEL_WINDOW_SIZE;
if( ++buf_idx >= ACCEL_WINDOW_SIZE) {
buf_idx = 0;
}
//dbgu_printf("[IMU] FBGyro: %d RLGyro: %d RLAccel: %d FBAccelAvg: %d\r\n", imu_FBGyro, imu_RLGyro, imu_RLAccel, imu_FBAccelAverage);
#ifdef USE_KALMAN_FILTER
float dt = TC_GetMsSinceTick(imu_lastUpdate) / 1000.0;
imu_lastUpdate = TC_GetSystemTicks();
//dbgu_printf("IMU_Update: dt=%5.3f\r\n", dt);
// Convert acceleration to G
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;
// dbgu_printf("IMU_Update: X=%11.8f, Y=%11.8f, Z=%11.8f\r\n", accX, accY, accZ);
pitch = atan2(accX, sqrt(accY * accY + accZ * accZ));
roll = atan2(accY, -accZ);
float roll_rate = imu_RLGyro;
float pitch_rate = imu_FBGyro;
ars_predict(&filter_roll, roll_rate, dt); // Kalman predict
float roll_est = ars_update(&filter_roll, roll);
ars_predict(&filter_pitch, pitch_rate, dt); // Kalman predict
float pitch_est = ars_update(&filter_pitch, pitch);
// Report the estimate pitch and roll values in degrees.
roll_output = (roll_est * RAD_TO_DEG_FACTOR);
pitch_output = (pitch_est * RAD_TO_DEG_FACTOR);
//dbgu_printf("IMU_Update: roll=%d°, pitch=%d°\r\n", roll_output, pitch_output);
#endif
}