aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttControl: add rateef_rpy

Browse Source 
rateef_rpy takes earth frame rates and applies them to the earth frame
angle targets and then converts them to body frame rates
This commit is contained in:
Randy Mackay 2014-01-28 17:30:33 +09:00 committed by Andrew Tridgell
parent 468be05867
commit a2b017abd2
2 changed files with 24 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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

5
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -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);
//