コード例 #1
0
ファイル: AP_Mount_MAVLink.cpp プロジェクト: APM602/APM602
Vector3f AP_Mount_MAVLink::getGimbalRateDemVecYaw(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst, const Vector3f &joint_angles)
{
    // Define rotation from vehicle to gimbal using a 312 rotation sequence
    Matrix3f Tvg = vector312_to_rotation_matrix(joint_angles);

    // multiply the yaw joint angle by a gain to calculate a
    // demanded vehicle frame relative rate vector required to
    // keep the yaw joint centred
    Vector3f gimbalRateDemVecYaw(0, 0, - K_gimbalRate * joint_angles.z);

    // Get filtered vehicle turn rate in earth frame
    vehicleYawRateFilt = (1.0f - yawRateFiltPole * delta_time) * vehicleYawRateFilt + yawRateFiltPole * delta_time * _frontend._ahrs.get_yaw_rate_earth();
    Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
    
    // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
    float maxRate = K_gimbalRate * yawErrorLimit;
    float vehicle_rate_mag_ef = vehicle_rate_ef.length();
    float excess_rate_correction = fabs(vehicle_rate_mag_ef) - maxRate; 
    if (vehicle_rate_mag_ef > maxRate) {
        if (vehicle_rate_ef.z>0.0f) {
            gimbalRateDemVecYaw += _frontend._ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
        } else {
            gimbalRateDemVecYaw -= _frontend._ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
        }            
    }        
    
    // rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
    gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
    return gimbalRateDemVecYaw;
}
コード例 #2
0
ファイル: AP_Gimbal.cpp プロジェクト: danil2009/ardupilot
Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
{
        // Define rotation from vehicle to gimbal using a 312 rotation sequence
        Matrix3f Tvg;
        float cosPhi = cosf(_measurement.joint_angles.x);
        float cosTheta = cosf(_measurement.joint_angles.y);
        float sinPhi = sinf(_measurement.joint_angles.x);
        float sinTheta = sinf(_measurement.joint_angles.y);
        float sinPsi = sinf(_measurement.joint_angles.z);
        float cosPsi = cosf(_measurement.joint_angles.z);
        Tvg[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta;
        Tvg[1][0] = -sinPsi*cosPhi;
        Tvg[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi;
        Tvg[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta;
        Tvg[1][1] = cosPsi*cosPhi;
        Tvg[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi;
        Tvg[0][2] = -sinTheta*cosPhi;
        Tvg[1][2] = sinPhi;
        Tvg[2][2] = cosTheta*cosPhi;

        // multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centred
        Vector3f gimbalRateDemVecYaw;
        gimbalRateDemVecYaw.z = - _gimbalParams.K_gimbalRate * _measurement.joint_angles.z;

        // Get filtered vehicle turn rate in earth frame
        vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurement.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurement.delta_time * _ahrs.get_yaw_rate_earth();
        Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);

         // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
        float maxRate = _gimbalParams.K_gimbalRate * yawErrorLimit;
        float vehicle_rate_mag_ef = vehicle_rate_ef.length();
        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);    
            }else{
                gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
            }            
        }        

        // rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
        gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
        return gimbalRateDemVecYaw;
}
コード例 #3
0
Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
{
        // define the rotation from vehicle to earth
        Matrix3f Tve = _ahrs.get_dcm_matrix();
        Matrix3f Teg;
        quatEst.inverse().rotation_matrix(Teg);

        // multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centered
        Vector3f gimbalRateDemVecYaw;
        gimbalRateDemVecYaw.z = - _gimbalParams.get_K_rate() * filtered_joint_angles.z;

        // Get filtered vehicle turn rate in earth frame
        vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurement.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurement.delta_time * _ahrs.get_yaw_rate_earth();
        Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);

         // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
        float maxRate = _gimbalParams.get_K_rate() * yawErrorLimit;
        float vehicle_rate_mag_ef = vehicle_rate_ef.length();
        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);
            } else {
                gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
            }
        }

        // rotate the rate demand into earth frame
        gimbalRateDemVecYaw = Tve * gimbalRateDemVecYaw;
        // zero the tilt components
        gimbalRateDemVecYaw.x = 0;
        gimbalRateDemVecYaw.y = 0;
        // rotate the rate demand into gimbal frame
        gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw;
        return gimbalRateDemVecYaw;
}