diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp index d8f6ce762a..3bc29b592a 100644 --- a/libraries/AP_Mount/AP_Gimbal.cpp +++ b/libraries/AP_Mount/AP_Gimbal.cpp @@ -119,9 +119,9 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst) float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate; if (vehicle_rate_mag_ef > maxRate) { 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 { - 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); } } diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 28fa70a039..0383842e4c 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -130,7 +130,7 @@ void AP_Mount_Servo::stabilize() 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 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(); cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z); gimbal_target = m * cam;