From 8ace1bfad1d15ce374dd3e1eabde553425fe3f68 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 10 Feb 2021 11:38:47 +0900 Subject: [PATCH] AC_PosControl: const local vars and remove todo --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 5892148143..1e9d3659d3 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1115,12 +1115,9 @@ void AC_PosControl::run_xy_controller(float dt) // get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const { - float accel_right, accel_forward; - // rotate accelerations into body forward-right frame - // todo: this should probably be based on the desired heading not the current heading - accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw(); - accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw(); + const float accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw(); + const float accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);