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

DCM: tidy up use of error_course and in_motion

Browse Source 
in_motion is not a good name now it is also used for the compass

The error_course and heading component values don't need to be part of
the DCM object, they can be on the stack to reduce the memory usage a bit
This commit is contained in:
Andrew Tridgell 2012-02-24 11:44:21 +11:00
parent d5b619218c
commit c63ca9c697
2 changed files with 18 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_DCM/AP_DCM.cpp
View File

@ -208,8 +208,6 @@ AP_DCM::matrix_reset(bool recover_eulers)
_omega_integ_corr = _omega_I;
_omega = _omega_I;
_error_roll_pitch = _omega_I;
_error_yaw = _omega_I;
_errorCourse = 0;
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
@ -358,9 +356,9 @@ AP_DCM::drift_correction(void)
float accel_magnitude;
float accel_weight;
float integrator_magnitude;
Vector3f error_yaw;
//static float scaled_omega_P[3];
//static float scaled_omega_I[3];
static bool in_motion = false;
//*****Roll and Pitch***************
@ -390,7 +388,7 @@ AP_DCM::drift_correction(void)
//*****YAW***************
if (_compass && _compass->healthy) {
if (in_motion) {
if (_have_initial_yaw) {
// Equation 23, Calculating YAW error
// We make the gyro YAW drift correction based
// on compass magnetic heading
@ -406,37 +404,42 @@ AP_DCM::drift_correction(void)
// now construct a new DCM matrix
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
in_motion = true;
_have_initial_yaw = true;
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) {
_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
_course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
if(in_motion) {
error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x); // Equation 23, Calculating YAW error
if (_have_initial_yaw) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// Equation 23, Calculating YAW error
error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
} else {
// when we first start moving, set the
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
in_motion = true;
_have_initial_yaw = true;
error_course = 0;
}
} else {
// we are moving very slowly. Stop doing
// course correction with the GPS, and instead
// grab the next GPS course value once we
// start moving again.
error_course = 0;
in_motion = false;
_have_initial_yaw = false;
}
}
_error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
_omega_P += _error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector.
_omega_I += _error_yaw * _ki_yaw; // adding yaw correction to integrator correction vector.
_omega_P += error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector.
_omega_I += error_yaw * _ki_yaw; // adding yaw correction to integrator correction vector.
// Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second
integrator_magnitude = _omega_I.length();

7
libraries/AP_DCM/AP_DCM.h
View File

@ -31,8 +31,6 @@ public:
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1),
_course_over_ground_x(0),
_course_over_ground_y(1),
_health(1.),
_kp_roll_pitch(0.05967),
_ki_roll_pitch(0.00001278),
@ -94,6 +92,7 @@ private:
float _ki_roll_pitch;
float _kp_yaw;
float _ki_yaw;
bool _have_initial_yaw;
// Methods
void read_adc_raw(void);
@ -128,10 +127,6 @@ private:
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
Vector3f _error_roll_pitch;
Vector3f _error_yaw;
float _errorCourse;
float _course_over_ground_x; // Course overground X axis
float _course_over_ground_y; // Course overground Y axis
float _health;
bool _centripetal;
uint8_t _toggle;