diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 0729925537..a0f206d27f 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -803,21 +803,16 @@ void throttle_accel_deactivate() static int16_t get_throttle_accel(int16_t z_target_accel) { - static float accel_one_g = -980; // filtered estimate of 1G in cm/s/s int32_t p,i,d; // used to capture pid values for logging int16_t z_accel_error, output; float z_accel_meas_temp; Vector3f accel = ins.get_accel(); - Matrix3f dcm_matrix = ahrs.get_dcm_matrix(); + //Matrix3f dcm_matrix = ahrs.get_dcm_matrix(); // Calculate Earth Frame Z acceleration - z_accel_meas_temp = (dcm_matrix.c.x * accel.x + dcm_matrix.c.y * accel.y + dcm_matrix.c.z * accel.z)* 100.0; - - // Filter Earth Frame Z acceleration with fc = 0.01 Hz to find 1 g - accel_one_g = accel_one_g + 0.00062792 * (z_accel_meas_temp - accel_one_g); - - z_accel_meas_temp = z_accel_meas_temp - accel_one_g; + //z_accel_meas_temp = (dcm_matrix.c.x * accel.x + dcm_matrix.c.y * accel.y + dcm_matrix.c.z * accel.z)* 100.0; + z_accel_meas_temp = ahrs.get_accel_ef().z; // Filter Earth Frame Z acceleration with fc = 1 Hz z_accel_meas = z_accel_meas + 0.059117 * (z_accel_meas_temp - z_accel_meas);