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

Moved Health to DCM, Centripetal correct

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@988 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-01 22:52:11 +00:00
parent 275624358d
commit 93206020cb
4 changed files with 16 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_DCM/AP_DCM.cpp
View File

@ -97,10 +97,11 @@ AP_DCM::matrix_update(float _G_Dt)
(fabs(_gyro_vector.z) >= radians(300)))
gyro_sat_count++;
if(_centrifugal){
_omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega)
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
accel_adjust(); // Remove centrifugal acceleration.
if(_centripetal){
accel_adjust(); // Remove _centripetal acceleration.
}
#if OUTPUTMODE == 1
@ -242,7 +243,7 @@ AP_DCM::drift_correction(void)
accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1); //
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
_imu->set_health(0.02 * (accel_weight-.5));
_health += constrain((0.02 * (accel_weight - .5)), 0, 1);
// adjust the ground of reference
_error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
@ -338,5 +339,10 @@ AP_DCM::euler_angles(void)
/**************************************************/
float
AP_DCM::get_health(void)
{
return _health;
}

7
libraries/AP_DCM/AP_DCM.h
View File

@ -42,11 +42,13 @@ public:
Vector3f get_accel(void);
Matrix3f get_dcm_matrix(void);
void set_centrifugal(bool b);
void set_centripetal(bool b);
// Methods
void update_DCM(float _G_Dt);
float get_health(void);
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100
@ -88,7 +90,8 @@ private:
float _errorCourse;
float _course_over_ground_x; // Course overground X axis
float _course_over_ground_y; // Course overground Y axis
bool _centrifugal;
float _health;
bool _centripetal;
};
#endif

12
libraries/AP_IMU/AP_IMU.cpp
View File

@ -55,18 +55,6 @@ const float AP_IMU::_gyro_temp_curve[3][3] = {
}; // To Do - make additional constructors to pass this in.
/**************************************************/
void
AP_IMU::set_health(float health)
{
_health += constrain(health, 0, 1);
}
float
AP_IMU::get_health(void)
{
return _health;
}
void
AP_IMU::init(void)

4
libraries/AP_IMU/AP_IMU.h
View File

@ -33,9 +33,6 @@ public:
Vector3f get_gyro(void); // Radians/second
Vector3f get_accel(void); // meters/seconds squared
void set_health(float health);
float get_health(void);
// Members
uint8_t adc_constraints; // a check of how many times we get non-sensical values
@ -48,7 +45,6 @@ private:
uint16_t _address; // EEPROM start address for saving/retrieving offsets
float _adc_in[6]; // array that store the 6 ADC channels used by IMU
float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers
float _health;
Vector3f _accel_vector; // Store the acceleration in a vector
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
AP_ADC * _adc; // Analog to digital converter pointer