Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
rmackay9 2012-12-12 16:29:46 +09:00 committed by Andrew Tridgell
parent e9fa5dec0f
commit 7ea34d4fb7
1 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/Attitude.pde
View File

@ -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);