Browse Source

AP_Mount: reflect renamed function in AP_AHRS

mission-4.1.18
Jonathan Challinger 9 years ago committed by Andrew Tridgell
parent
commit
63caca1d3f
  1. 4
      libraries/AP_Mount/AP_Gimbal.cpp
  2. 2
      libraries/AP_Mount/AP_Mount_Servo.cpp

4
libraries/AP_Mount/AP_Gimbal.cpp

@ -119,9 +119,9 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst) @@ -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);
}
}

2
libraries/AP_Mount/AP_Mount_Servo.cpp

@ -130,7 +130,7 @@ void AP_Mount_Servo::stabilize() @@ -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;

Loading…
Cancel
Save