Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS_DCM: sanity check AHRS_RP_P and AHRS_YAW_P

Browse Source
This commit is contained in:
Randy Mackay 2014-10-21 21:40:16 +09:00
parent 2bc74934b8
commit 39c8535223
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -32,6 +32,8 @@
#include <AP_Param.h>
#define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees
#define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter
#define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter
enum AHRS_VehicleClass {
AHRS_VEHICLE_UNKNOWN,

10
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -442,6 +442,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// integration at higher rates
float spin_rate = _omega.length();
// sanity check _kp_yaw
if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
_kp_yaw = AP_AHRS_YAW_P_MIN;
}
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
@ -715,6 +720,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
// base the P gain on the spin rate
float spin_rate = _omega.length();
// sanity check _kp value
if (_kp < AP_AHRS_RP_P_MIN) {
_kp = AP_AHRS_RP_P_MIN;
}
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.