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