From 9f914d9e7d26f6ee956bfd3900532f3d0322677a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 Jan 2021 13:43:26 +0900 Subject: [PATCH] AC_PosControl: add get_z_accel_cmss helper function --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 5 ++--- libraries/AC_AttitudeControl/AC_PosControl.h | 3 +++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 2670205270..66b6304f83 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -629,11 +629,10 @@ void AC_PosControl::run_z_controller() _accel_target.z += _accel_desired.z; - // the following section calculates a desired throttle needed to achieve the acceleration target - float z_accel_meas; // actual acceleration // Calculate Earth Frame Z acceleration - z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; + const float z_accel_meas = get_z_accel_cmss(); + // ensure imax is always large enough to overpower hover throttle if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 617cb42f4a..4c6cea981c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -342,6 +342,9 @@ protected: // init_takeoff void run_z_controller(); + // get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up + float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; } + /// /// xy controller private methods ///