From 15be80a25dd88d02f12548dd657ff1e74e206681 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 20 Dec 2015 22:03:29 +1030 Subject: [PATCH] AC_PosControl: accel_to_throttle outputs 0 to 1 --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 5392893fea..211f08b40f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -480,7 +480,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) // get d term d = _pid_accel_z.get_d(); - float thr_out = p+i+d+_throttle_hover; + float thr_out = (p+i+d)/1000.0f +_throttle_hover; // send throttle to attitude controller with angle boost _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);