C++ (Cpp) update_joint_angle_est示例

示例#1

文件： AP_Gimbal.cpp 项目： raulshepherd/ardupilot-solo

void AP_Gimbal::update_state()
{
    // Run the gimbal attitude and gyro bias estimator
    _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);

    // get the gimbal quaternion estimate
    Quaternion quatEst;
    _ekf.getQuat(quatEst);

    update_joint_angle_est();

    gimbalRateDemVec.zero();
    switch(_mode) {
        case GIMBAL_MODE_POS_HOLD_FF:
            gimbalRateDemVec += getGimbalRateBodyLock();
            gimbalRateDemVec += getGimbalRateDemVecGyroBias();
            break;
        case GIMBAL_MODE_STABILIZE:
            gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
            gimbalRateDemVec += getGimbalRateDemVecTilt(quatEst);
            gimbalRateDemVec += getGimbalRateDemVecForward(quatEst);
            gimbalRateDemVec += getGimbalRateDemVecGyroBias();
            break;
        default:
        case GIMBAL_MODE_IDLE:
        case GIMBAL_MODE_POS_HOLD:
            break;
    }

    float gimbalRateDemVecLen = gimbalRateDemVec.length();
    if (gimbalRateDemVecLen > radians(400)) {
        gimbalRateDemVec *= radians(400)/gimbalRateDemVecLen;
    }
}

示例#2

文件： SoloGimbal.cpp 项目： DroneBuster/ardupilot

void SoloGimbal::update_estimators()
{
    if (_state == GIMBAL_STATE_NOT_PRESENT || _state == GIMBAL_STATE_PRESENT_INITIALIZING) {
        return;
    }

    // Run the gimbal attitude and gyro bias estimator
    _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);
    update_joint_angle_est();
}