5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-10 00:23:57 -03:00

AC_PosControl: add get_z_accel_cmss helper function

This commit is contained in:
Randy Mackay 2021-01-29 13:43:26 +09:00
parent 3ae8af6b21
commit 9f914d9e7d
2 changed files with 5 additions and 3 deletions
libraries/AC_AttitudeControl

View File

@ -629,11 +629,10 @@ void AC_PosControl::run_z_controller()
_accel_target.z += _accel_desired.z; _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 // 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 // ensure imax is always large enough to overpower hover throttle
if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) { if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {

View File

@ -342,6 +342,9 @@ protected:
// init_takeoff // init_takeoff
void run_z_controller(); 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 /// xy controller private methods
/// ///