Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl: replace APM_PI with AC_P

Browse Source
This commit is contained in:
Randy Mackay 2014-02-14 16:06:21 +09:00 committed by Andrew Tridgell
parent 94fb9c4274
commit ff9f4fe6e7
2 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -337,21 +337,21 @@ void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors()
void AC_AttitudeControl::update_rate_bf_targets()
{
// stab roll calculation
_rate_bf_target.x = _pi_angle_roll.kP() * _angle_bf_error.x;
_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x;
// constrain roll rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.x = constrain_float(_rate_bf_target.x,-_angle_rate_rp_max,_angle_rate_rp_max);
}
// stab pitch calculation
_rate_bf_target.y = _pi_angle_pitch.kP() * _angle_bf_error.y;
_rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y;
// constrain pitch rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.y = constrain_float(_rate_bf_target.y,-_angle_rate_rp_max,_angle_rate_rp_max);
}
// stab yaw calculation
_rate_bf_target.z = _pi_angle_yaw.kP() * _angle_bf_error.z;
_rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z;
// constrain yaw rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max);

16
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -14,7 +14,7 @@
#include <AP_Motors.h>
#include <DataFlash.h>
#include <AC_PID.h>
#include <APM_PI.h>
#include <AC_P.h>
// To-Do: change the name or move to AP_Math?
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
@ -40,16 +40,16 @@ public:
AP_InertialSensor& ins,
const AP_Vehicle::MultiCopter &aparm,
AP_Motors& motors,
APM_PI& pi_angle_roll, APM_PI& pi_angle_pitch, APM_PI& pi_angle_yaw,
AC_P& pi_angle_roll, AC_P& pi_angle_pitch, AC_P& pi_angle_yaw,
AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw
) :
_ahrs(ahrs),
_ins(ins),
_aparm(aparm),
_motors(motors),
_pi_angle_roll(pi_angle_roll),
_pi_angle_pitch(pi_angle_pitch),
_pi_angle_yaw(pi_angle_yaw),
_p_angle_roll(pi_angle_roll),
_p_angle_pitch(pi_angle_pitch),
_p_angle_yaw(pi_angle_yaw),
_pid_rate_roll(pid_rate_roll),
_pid_rate_pitch(pid_rate_pitch),
_pid_rate_yaw(pid_rate_yaw),
@ -205,9 +205,9 @@ protected:
const AP_InertialSensor& _ins;
const AP_Vehicle::MultiCopter &_aparm;
AP_Motors& _motors;
APM_PI& _pi_angle_roll;
APM_PI& _pi_angle_pitch;
APM_PI& _pi_angle_yaw;
AC_P& _p_angle_roll;
AC_P& _p_angle_pitch;
AC_P& _p_angle_yaw;
AC_PID& _pid_rate_roll;
AC_PID& _pid_rate_pitch;
AC_PID& _pid_rate_yaw;