Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: added AHRS_YAW_P parameter

Browse Source 
this allows users to change the yaw gain in DCM
This commit is contained in:
Andrew Tridgell 2012-04-16 20:51:46 +10:00
parent 9e36b9b672
commit 8e90aeea4e
3 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -82,6 +82,8 @@ public:
// attitude
virtual Matrix3f get_dcm_matrix(void) = 0;
static const struct AP_Param::GroupInfo var_info[];
protected:
// pointer to compass object, if enabled
Compass * _compass;

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -23,6 +23,12 @@
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
// table of user settable parameters
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
AP_GROUPINFO("YAW_P", 0, AP_AHRS_DCM, _kp_yaw),
AP_GROUPEND
};
// run a full DCM update round
void
AP_AHRS_DCM::update(void)
@ -453,7 +459,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// allow the yaw reference source to affect all 3 components
// of _omega_yaw_P as we need to be able to correctly hold a
// heading when roll and pitch are non-zero
_omega_yaw_P = error * _kp_yaw;
_omega_yaw_P = error * _kp_yaw.get();
// add yaw correction to integrator correction vector, but
// only for the z gyro. We rely on the accelerometers for x

6
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -17,7 +17,7 @@ public:
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
_kp_roll_pitch = 0.13;
_kp_yaw = 0.4;
_kp_yaw.set(0.4);
_dcm_matrix(Vector3f(1, 0, 0),
Vector3f(0, 1, 0),
Vector3f(0, 0, 1));
@ -42,10 +42,12 @@ public:
float get_error_rp(void);
float get_error_yaw(void);
// settable parameters
AP_Float _kp_yaw;
private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _kp_yaw;
float _ki_yaw;
bool _have_initial_yaw;