void AccelGyro::calculate() {
// Gyro data receiver
int error;
double dT;
error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
// Get the time of reading for rotation computations
unsigned long t_now = millis();
// Convert gyro values to degrees/sec
float FS_SEL = 131;
float gyro_x = (accel_t_gyro.value.x_gyro - accel_t_gyro_global.base_x_gyro)/FS_SEL;
float gyro_y = (accel_t_gyro.value.y_gyro - accel_t_gyro_global.base_y_gyro)/FS_SEL;
float gyro_z = (accel_t_gyro.value.z_gyro - accel_t_gyro_global.base_z_gyro)/FS_SEL;
// Get raw acceleration values
//float G_CONVERT = 16384;
float accel_x = accel_t_gyro.value.x_accel;
float accel_y = accel_t_gyro.value.y_accel;
float accel_z = accel_t_gyro.value.z_accel;
// Get angle values from accelerometer
float RADIANS_TO_DEGREES = 180/3.14159;
float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
float accel_angle_z = 0;
// Compute the (filtered) gyro angles
float dt =(t_now - get_last_time())/1000.0;
float gyro_angle_x = gyro_x*dt + get_last_x_angle();
float gyro_angle_y = gyro_y*dt + get_last_y_angle();
float gyro_angle_z = gyro_z*dt + get_last_z_angle();
// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();
// Apply the complementary filter to figure out the change in angle - choice of alpha is
// estimated now. Alpha depends on the sampling rate...
float alpha = 0.96;
float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle
// Update the saved data with the latest values
accel_t_gyro_global.x_angle = angle_x;
accel_t_gyro_global.y_angle = angle_y;
accel_t_gyro_global.z_angle = angle_z;
accel_t_gyro_global.x_last_five[accel_t_gyro_global.position] = angle_x;
accel_t_gyro_global.position = (accel_t_gyro_global.position + 1) % 5;
set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
}
void complement_filter(unsigned long t_now){
int error;
accel_t_gyro_union accel_t_gyro;
// Read the raw values.
error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro);
//out_draw_data(accel_t_gyro);
float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL;
float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL;
float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL;
// Get raw acceleration values
//float G_CONVERT = 16384;
float accel_x = accel_t_gyro.value.x_accel;
float accel_y = accel_t_gyro.value.y_accel;
float accel_z = accel_t_gyro.value.z_accel;
// Get angle values from accelerometer
// float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
float unfiltered_accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RAD_TO_DEG;
float unfiltered_accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RAD_TO_DEG;
float unfiltered_accel_angle_z = 0;
// Compute the (filtered) gyro angles
float dt =(t_now - get_last_time())/MILLION; // dt is in micro second
float gyro_angle_x = gyro_x*dt + get_last_x_angle();
float gyro_angle_y = gyro_y*dt + get_last_y_angle();
float gyro_angle_z = gyro_z*dt + get_last_z_angle();
// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();
// Apply the complementary filter to figure out the change in angle - choice of alpha is
// estimated now. Alpha depends on the sampling rate...
angle[AXIS_X] = alpha*gyro_angle_x + (1.0 - alpha)*unfiltered_accel_angle_x;
angle[AXIS_Y] = alpha*gyro_angle_y + (1.0 - alpha)*unfiltered_accel_angle_y;
angle[AXIS_Z] = gyro_angle_z; //Accelerometer doesn't give z-angle
// Update the saved data with the latest values
set_last_read_angle_data(t_now, angle[AXIS_X], angle[AXIS_Y], angle[AXIS_Z], unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
#if 0
// Send the data to the serial port
Serial.print(F("DEL:")); //Delta T
Serial.print(dt, DEC);
Serial.print(F("#ACC:")); //Accelerometer angle
Serial.print(unfiltered_accel_angle_x, 2);
Serial.print(F(","));
Serial.print(unfiltered_accel_angle_y, 2);
Serial.print(F(","));
Serial.print(unfiltered_accel_angle_z, 2);
Serial.print(F("#GYR:"));
Serial.print(unfiltered_gyro_angle_x, 2); //Gyroscope angle
Serial.print(F(","));
Serial.print(unfiltered_gyro_angle_y, 2);
Serial.print(F(","));
Serial.print(unfiltered_gyro_angle_z, 2);
Serial.print(F("#FIL:")); //Filtered angle
Serial.print(angle_x, 2);
Serial.print(F(","));
Serial.print(angle_y, 2);
Serial.print(F(","));
Serial.print(angle_z, 2);
Serial.println(F(""));
#endif
}