Browse Source

ArduCopter: bug fix to accel based throttle controller (was using m/s instead of cm/s)

mission-4.1.18
rmackay9 12 years ago committed by Andrew Tridgell
parent
commit
e583ade62d
  1. 4
      ArduCopter/Attitude.pde

4
ArduCopter/Attitude.pde

@ -808,10 +808,8 @@ get_throttle_accel(int16_t z_target_accel) @@ -808,10 +808,8 @@ get_throttle_accel(int16_t z_target_accel)
int16_t output;
float z_accel_meas;
Vector3f accel = ins.get_accel();
// Calculate Earth Frame Z acceleration
z_accel_meas = ahrs.get_accel_ef().z;
z_accel_meas = (ahrs.get_accel_ef().z - gravity) * 100;
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel + z_accel_meas, -32000, 32000) - z_accel_error);

Loading…
Cancel
Save