Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -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() void AC_AttitudeControl::update_rate_bf_targets()
{ {
// stab roll calculation // 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 // constrain roll rate request
if (_flags.limit_angle_to_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); _rate_bf_target.x = constrain_float(_rate_bf_target.x,-_angle_rate_rp_max,_angle_rate_rp_max);
} }
// stab pitch calculation // 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 // constrain pitch rate request
if (_flags.limit_angle_to_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); _rate_bf_target.y = constrain_float(_rate_bf_target.y,-_angle_rate_rp_max,_angle_rate_rp_max);
} }
// stab yaw calculation // 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 // constrain yaw rate request
if (_flags.limit_angle_to_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); _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 <AP_Motors.h>
#include <DataFlash.h> #include <DataFlash.h>
#include <AC_PID.h> #include <AC_PID.h>
#include <APM_PI.h> #include <AC_P.h>
// To-Do: change the name or move to AP_Math? // 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 #define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
@ -40,16 +40,16 @@ public:
AP_InertialSensor& ins, AP_InertialSensor& ins,
const AP_Vehicle::MultiCopter &aparm, const AP_Vehicle::MultiCopter &aparm,
AP_Motors& motors, 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 AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw
) : ) :
_ahrs(ahrs), _ahrs(ahrs),
_ins(ins), _ins(ins),
_aparm(aparm), _aparm(aparm),
_motors(motors), _motors(motors),
_pi_angle_roll(pi_angle_roll), _p_angle_roll(pi_angle_roll),
_pi_angle_pitch(pi_angle_pitch), _p_angle_pitch(pi_angle_pitch),
_pi_angle_yaw(pi_angle_yaw), _p_angle_yaw(pi_angle_yaw),
_pid_rate_roll(pid_rate_roll), _pid_rate_roll(pid_rate_roll),
_pid_rate_pitch(pid_rate_pitch), _pid_rate_pitch(pid_rate_pitch),
_pid_rate_yaw(pid_rate_yaw), _pid_rate_yaw(pid_rate_yaw),
@ -205,9 +205,9 @@ protected:
const AP_InertialSensor& _ins; const AP_InertialSensor& _ins;
const AP_Vehicle::MultiCopter &_aparm; const AP_Vehicle::MultiCopter &_aparm;
AP_Motors& _motors; AP_Motors& _motors;
APM_PI& _pi_angle_roll; AC_P& _p_angle_roll;
APM_PI& _pi_angle_pitch; AC_P& _p_angle_pitch;
APM_PI& _pi_angle_yaw; AC_P& _p_angle_yaw;
AC_PID& _pid_rate_roll; AC_PID& _pid_rate_roll;
AC_PID& _pid_rate_pitch; AC_PID& _pid_rate_pitch;
AC_PID& _pid_rate_yaw; AC_PID& _pid_rate_yaw;