From 7abd02baf242fb31cd91cc5f15a61d3c3a08c17d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 26 Mar 2015 14:42:42 -0700 Subject: [PATCH] AC_PosControl: change int32/int16 to float in accel_to_throttle --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 934514e175..4228d2c455 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -335,7 +335,6 @@ void AC_PosControl::rate_to_accel_z() // consolidate and constrain target acceleration desired_accel = _accel_feedforward.z + p; - desired_accel = constrain_int32(desired_accel, -32000, 32000); // set target for accel based throttle controller accel_to_throttle(desired_accel); @@ -346,7 +345,7 @@ void AC_PosControl::rate_to_accel_z() void AC_PosControl::accel_to_throttle(float accel_target_z) { float z_accel_meas; // actual acceleration - int32_t p,i,d; // used to capture pid values for logging + float p,i,d; // used to capture pid values for logging // Calculate Earth Frame Z acceleration z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; @@ -359,7 +358,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) _flags.reset_accel_to_throttle = false; } else { // calculate accel error and Filter with fc = 2 Hz - _accel_error.z = _accel_error_filter.apply(constrain_float(accel_target_z - z_accel_meas, -32000, 32000)); + _accel_error.z = _accel_error_filter.apply(accel_target_z - z_accel_meas); } // set input to PID @@ -380,7 +379,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) // get d term d = _pid_accel_z.get_d(); - int16_t thr_out = (int16_t)p+i+d+_throttle_hover; + float thr_out = p+i+d+_throttle_hover; // send throttle to attitude controller with angle boost _attitude_control.set_throttle_out(thr_out, true);