From eda376c7f622c5f382a8c3b18a01c2e091f191a0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 30 Jan 2014 21:44:16 +0900 Subject: [PATCH] AC_AttControl: init_targets() clears body frame angle errors This should ensure that we don't get sudden jerks when entering acro mode --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 24d5045770..31461ac7c1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -45,9 +45,15 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { // init_targets - resets target angles to current angles void AC_AttitudeControl::init_targets() { + // set earth frame angle targets to current lean angles _angle_ef_target.x = _ahrs.roll_sensor; _angle_ef_target.y = _ahrs.pitch_sensor; _angle_ef_target.z = _ahrs.yaw_sensor; + + // clear body frame angle errors + _rate_stab_bf_error.x = 0; + _rate_stab_bf_error.y = 0; + _rate_stab_bf_error.z = 0; } // angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)