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

AP_Mount: reflect renamed function in AP_AHRS

Browse Source
This commit is contained in:
Jonathan Challinger 2015-12-10 14:07:15 -08:00 committed by Andrew Tridgell
parent ee8090e25f
commit 63caca1d3f
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Mount/AP_Gimbal.cpp
View File

@ -119,9 +119,9 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate; float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate;
if (vehicle_rate_mag_ef > maxRate) { if (vehicle_rate_mag_ef > maxRate) {
if (vehicle_rate_ef.z>0.0f){ if (vehicle_rate_ef.z>0.0f){
gimbalRateDemVecYaw += _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction); gimbalRateDemVecYaw += _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction);
} else { } else {
gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction); gimbalRateDemVecYaw -= _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction);
} }
} }

2
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -130,7 +130,7 @@ void AP_Mount_Servo::stabilize()
Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs
Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input. Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input.
Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos. Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
m = _frontend._ahrs.get_dcm_matrix(); m = _frontend._ahrs.get_rotation_body_to_ned();
m.transpose(); m.transpose();
cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z); cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z);
gimbal_target = m * cam; gimbal_target = m * cam;