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

AC_AttControl: add high level angle controllers

Browse Source
This commit is contained in:
Randy Mackay 2014-01-11 17:02:42 +09:00 committed by Andrew Tridgell
parent 784f7385b5
commit 0e0a15f4a8
2 changed files with 63 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -18,6 +18,59 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
AP_GROUPEND
};
//
// high level controllers
//
// init_targets - resets target angles to current angles
void AC_AttitudeControl::init_targets()
{
_angle_ef_target.x = _ahrs.roll_sensor;
_angle_ef_target.y = _ahrs.pitch_sensor;
_angle_ef_target.z = _ahrs.yaw_sensor;
}
// angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
void AC_AttitudeControl::angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef)
{
// set earth-frame angle targets
_angle_ef_target.x = roll_angle_ef;
_angle_ef_target.y = pitch_angle_ef;
// convert earth-frame angle targets to earth-frame rate targets
angle_to_rate_ef_roll();
angle_to_rate_ef_pitch();
// set earth-frame rate stabilize target for yaw
_rate_stab_ef_target.z = yaw_rate_ef;
// convert earth-frame stabilize rate to regular rate target
rate_stab_ef_to_rate_ef_yaw();
// convert earth-frame rates to body-frame rates
rate_ef_targets_to_bf();
// body-frame to motor outputs should be called separately
}
// angleef_rpy - attempts to maintain a roll, pitch and yaw angle (all earth frame)
void AC_AttitudeControl::angleef_rpy(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef)
{
// set earth-frame angle targets
_angle_ef_target.x = roll_angle_ef;
_angle_ef_target.y = pitch_angle_ef;
// convert earth-frame angle targets to earth-frame rate targets
angle_to_rate_ef_roll();
angle_to_rate_ef_pitch();
angle_to_rate_ef_yaw();
// convert earth-frame rates to body-frame rates
rate_ef_targets_to_bf();
// body-frame to motor outputs should be called separately
}
//
// angle controller methods
//
@ -400,4 +453,4 @@ int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
_angle_boost = throttle_out - throttle_pwm;
return throttle_out;
}
}

9
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -73,6 +73,15 @@ public:
void set_dt(float delta_sec) { _dt = delta_sec; }
float get_dt() { return _dt; }
// init_targets - resets target angles to current angles
void init_targets();
// angleef_rp_rateef_y - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
void angleef_rp_rateef_y(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef);
// 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_rate_ef);
//
// angle controller (earth-frame) methods
//