From a2b017abd2ad9cb6b8daec8e6baeadb799c0e2ba Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Jan 2014 17:30:33 +0900 Subject: [PATCH] AC_AttControl: add rateef_rpy rateef_rpy takes earth frame rates and applies them to the earth frame angle targets and then converts them to body frame rates --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 21 ++++++++++++++++++- .../AC_AttitudeControl/AC_AttitudeControl.h | 5 ++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 4f1d3736de..ff12b5a7e5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -72,7 +72,26 @@ void AC_AttitudeControl::angleef_rpy(float roll_angle_ef, float pitch_angle_ef, // body-frame to motor outputs should be called separately } -// rateef_rpy - attempts to maintain a roll, pitch and yaw angle rate (all body frame) +// rateef_rpy - attempts to maintain a roll, pitch and yaw rate (all earth frame) +void AC_AttitudeControl::rateef_rpy(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef) +{ + // set stabilized earth-frame rate targets + _rate_stab_ef_target.x = roll_rate_ef; + _rate_stab_ef_target.y = pitch_rate_ef; + _rate_stab_ef_target.z = yaw_rate_ef; + + // convert stabilized earth-frame rates to (regular) earth-frames rates + rate_stab_ef_to_rate_ef_roll(); + rate_stab_ef_to_rate_ef_pitch(); + rate_stab_ef_to_rate_ef_yaw(); + + // convert earth-frame rates to body-frame rates + rate_ef_targets_to_bf(_rate_ef_target, _rate_bf_target); + + // body-frame to motor outputs should be called separately +} + +// ratebf_rpy - attempts to maintain a roll, pitch and yaw rate (all body frame) void AC_AttitudeControl::ratebf_rpy(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf) { // Update angle error diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 0c3c2a3766..106c8ae270 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -82,7 +82,10 @@ public: // angleef_rpy - attempts to maintain a roll, pitch and yaw angle (all earth frame) void angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef); - // rateef_rpy - attempts to maintain a roll, pitch and yaw angle rate (all body frame) + // rateef_rpy - attempts to maintain a roll, pitch and yaw rate (all earth frame) + void rateef_rpy(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef); + + // ratebf_rpy - attempts to maintain a roll, pitch and yaw rate (all body frame) void ratebf_rpy(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf); //