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

AC_AttControl: div-by-zero check for bf-to-ef conversion

Browse Source
This commit is contained in:
Randy Mackay 2014-08-22 22:50:10 +09:00
parent 87f0cb6168
commit c45338f080
2 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -382,10 +382,11 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
} else {
_acro_angle_switch = 4500;
integrate_bf_rate_error_to_angle_errors();
frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error);
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
}
if (_angle_ef_target.y > 9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
@ -432,12 +433,17 @@ void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Ve
}
// frame_conversion_bf_to_ef - converts body frame vector to earth frame vector
void AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
{
// convert earth frame rates to body frame rates
// avoid divide by zero
if (_ahrs.cos_pitch() == 0.0f) {
return false;
}
// convert earth frame angle or rates to body frame
ef_vector.x = bf_vector.x + _ahrs.sin_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.y + _ahrs.cos_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.z;
ef_vector.y = _ahrs.cos_roll() * bf_vector.y - _ahrs.sin_roll() * bf_vector.z;
ef_vector.z = (_ahrs.sin_roll() / _ahrs.cos_pitch()) * bf_vector.y + (_ahrs.cos_roll() / _ahrs.cos_pitch()) * bf_vector.z;
return true;
}
//

7
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -116,11 +116,12 @@ public:
//
// earth-frame <-> body-frame conversion functions
//
// frame_conversion_ef_to_bf - converts earth frame rate targets to body frame rate targets
// frame_conversion_ef_to_bf - converts earth frame angles or rates to body frame
void frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f &bf_vector);
// frame_conversion_bf_to_ef - converts body frame rate targets to earth frame rate targets
void frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector);
// frame_conversion_bf_to_ef - converts body frame angles or rates to earth frame
// returns false if conversion fails due to gimbal lock
bool frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector);
//
// public accessor functions