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

Added a state machine intended to run at 250hz to the DCM. Spread load of DCM out to lower CPU.

Browse Source
This commit is contained in:
Jason Short 2011-09-11 11:03:55 -07:00
parent b0c3cda23b
commit e8fcf72966
2 changed files with 69 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_DCM/AP_DCM.cpp
View File

@ -41,7 +41,7 @@ AP_DCM::set_compass(Compass *compass)
/**************************************************/
void
AP_DCM::update_DCM(float _G_Dt)
AP_DCM::update_DCM_fast(float _G_Dt)
{
_imu->update();
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
@ -49,13 +49,48 @@ AP_DCM::update_DCM(float _G_Dt)
matrix_update(_G_Dt); // Integrate the DCM matrix
//if(_toggle){
normalize(); // Normalize the DCM matrix
//}else{
drift_correction(); // Perform drift correction
//}
//_toggle = !_toggle;
switch(_toggle++){
case 0:
normalize(); // Normalize the DCM matrix
break;
case 1:
//drift_correction(); // Normalize the DCM matrix
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
break;
case 2:
drift_correction(); // Normalize the DCM matrix
break;
case 3:
//drift_correction(); // Normalize the DCM matrix
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
break;
case 4:
euler_yaw();
break;
default:
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
_toggle = 0;
//drift_correction(); // Normalize the DCM matrix
break;
}
}
/**************************************************/
void
AP_DCM::update_DCM(float _G_Dt)
{
_imu->update();
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
_accel_vector = _imu->get_accel(); // Get current values for IMU sensors
matrix_update(_G_Dt); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
}
@ -83,11 +118,12 @@ AP_DCM::matrix_update(float _G_Dt)
Matrix3f temp_matrix;
//Record when you saturate any of the gyros.
/*
if( (fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300))){
gyro_sat_count++;
}
}*/
_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
@ -335,6 +371,25 @@ AP_DCM::euler_angles(void)
yaw_sensor += 36000;
}
void
AP_DCM::euler_rp(void)
{
pitch = -asin(_dcm_matrix.c.x);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
}
void
AP_DCM::euler_yaw(void)
{
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
yaw_sensor = degrees(yaw) * 100;
if (yaw_sensor < 0)
yaw_sensor += 36000;
}
/**************************************************/
float

7
libraries/AP_DCM/AP_DCM.h
View File

@ -50,6 +50,7 @@ public:
// Methods
void update_DCM(float _G_Dt);
void update_DCM_fast(float _G_Dt);
float get_health(void);
@ -98,6 +99,10 @@ private:
void drift_correction(void);
void euler_angles(void);
void euler_rp(void);
void euler_yaw(void);
// members
Compass * _compass;
@ -122,7 +127,7 @@ private:
float _course_over_ground_y; // Course overground Y axis
float _health;
bool _centripetal;
bool _toggle;
uint8_t _toggle;
};
#endif