예제 #1
0
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
0
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);
 
    // Add the control rate vectors
    gimbalRateDemVec.zero();
    gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
    gimbalRateDemVec += getGimbalRateDemVecTilt(quatEst);
    gimbalRateDemVec += getGimbalRateDemVecForward(quatEst);
    gimbalRateDemVec += getGimbalRateDemVecGyroBias();
}
예제 #3
0
/*
  calculate the demanded rates for the mount, running the controller
 */
Vector3f AP_Mount_MAVLink::gimbal_update_control2(const Vector3f &ef_target_euler_rad, 
                                                  float delta_time, 
                                                  const Vector3f &delta_angles,
                                                  const Vector3f &delta_velocity,
                                                  const Vector3f &joint_angles)
{
    // get the gimbal quaternion estimate
    Quaternion quatEst;
    _ekf.getQuat(quatEst);
 
    // Add the control rate vectors
    Vector3f gimbalRateDemVec = 
        getGimbalRateDemVecYaw(ef_target_euler_rad, delta_time, quatEst, joint_angles) + 
        getGimbalRateDemVecTilt(ef_target_euler_rad, quatEst) + 
        getGimbalRateDemVecForward(ef_target_euler_rad, delta_time, quatEst);

    Vector3f gyroBias;
    _ekf.getGyroBias(gyroBias);

    gimbalRateDemVec += gyroBias;
    return gimbalRateDemVec;
}
예제 #4
0
void AP_Gimbal::send_controls(mavlink_channel_t chan)
{
    if (_state == GIMBAL_STATE_PRESENT_RUNNING) {
        // get the gimbal quaternion estimate
        Quaternion quatEst;
        _ekf.getQuat(quatEst);

        // run rate controller
        gimbalRateDemVec.zero();
        switch(get_mode()) {
            case GIMBAL_MODE_POS_HOLD_FF: {
                gimbalRateDemVec += getGimbalRateBodyLock();
                gimbalRateDemVec += getGimbalRateDemVecGyroBias();
                float gimbalRateDemVecLen = gimbalRateDemVec.length();
                if (gimbalRateDemVecLen > radians(400)) {
                    gimbalRateDemVec *= radians(400)/gimbalRateDemVecLen;
                }
                mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
                                                gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
                break;
            }
            case GIMBAL_MODE_STABILIZE: {
                gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
                gimbalRateDemVec += getGimbalRateDemVecTilt(quatEst);
                gimbalRateDemVec += getGimbalRateDemVecForward(quatEst);
                gimbalRateDemVec += getGimbalRateDemVecGyroBias();
                float gimbalRateDemVecLen = gimbalRateDemVec.length();
                if (gimbalRateDemVecLen > radians(400)) {
                    gimbalRateDemVec *= radians(400)/gimbalRateDemVecLen;
                }
                mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
                                                gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
                break;
            }
            default:
            case GIMBAL_MODE_IDLE:
            case GIMBAL_MODE_POS_HOLD:
                break;
        }
    }

    // set GMB_POS_HOLD
    if (get_mode() == GIMBAL_MODE_POS_HOLD) {
        _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
    } else {
        _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
    }

    // set GMB_MAX_TORQUE
    float max_torque;
    _gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
    if (max_torque != _max_torque && max_torque != 0) {
        _max_torque = max_torque;
    }

    if (!hal.util->get_soft_armed() || joints_near_limits()) {
        _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque);
    } else {
        _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0);
    }
}