コード例 #1
0
ファイル: AccelGyro.cpp プロジェクト: Ryccoo/quad_libraries
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);
}
コード例 #2
0
ファイル: imu.cpp プロジェクト: taiit/NCKH_2015
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
}