Browse Source

ArduCopter: updated accel based throttle to use ahrs's get_accel_ef method

Also removed slow low pass filter meant to correct for accelerometer calibration.  This is no longer required now that we have the improved calibration method.
mission-4.1.18
rmackay9 12 years ago committed by Andrew Tridgell
parent
commit
7ea34d4fb7
  1. 11
      ArduCopter/Attitude.pde

11
ArduCopter/Attitude.pde

@ -803,21 +803,16 @@ void throttle_accel_deactivate() @@ -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);

Loading…
Cancel
Save