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;
}
}
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);
}
}