// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "AP_Gimbal.h" #if AP_AHRS_NAVEKF_AVAILABLE #include #include #include #include #include void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg) { decode_feedback(msg); update_state(); if (_ekf.getStatus() && !isCopterFlipped() && !is_zero(_gimbalParams.K_gimbalRate)){ send_control(chan); } Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z); //::printf("est=%1.1f %1.1f %1.1f %d\t", eulerEst.x,eulerEst.y,eulerEst.z,_ekf.getStatus()); //::printf("joint_angles=(%+1.2f %+1.2f %+1.2f)\t", _measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z); //::printf("delta_ang=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_angles.x,_measurement.delta_angles.y,_measurement.delta_angles.z); //::printf("delta_vel=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_velocity.x,_measurement.delta_velocity.y,_measurement.delta_velocity.z); //::printf("rate=(%+1.3f %+1.3f %+1.3f)\t",gimbalRateDemVec.x,gimbalRateDemVec.y,gimbalRateDemVec.z); //::printf("target=(%+1.3f %+1.3f %+1.3f)\t",_angle_ef_target_rad.x,_angle_ef_target_rad.y,_angle_ef_target_rad.z); //::printf("\n"); } void AP_Gimbal::decode_feedback(mavlink_message_t *msg) { mavlink_msg_gimbal_report_decode(msg, &_report_msg); _measurement.delta_time = _report_msg.delta_time; _measurement.delta_angles.x = _report_msg.delta_angle_x; _measurement.delta_angles.y = _report_msg.delta_angle_y; _measurement.delta_angles.z = _report_msg.delta_angle_z; _measurement.delta_velocity.x = _report_msg.delta_velocity_x, _measurement.delta_velocity.y = _report_msg.delta_velocity_y; _measurement.delta_velocity.z = _report_msg.delta_velocity_z; _measurement.joint_angles.x = _report_msg.joint_roll; _measurement.joint_angles.y = _report_msg.joint_el; _measurement.joint_angles.z = _report_msg.joint_az; //apply joint angle compensation _measurement.joint_angles -= _gimbalParams.joint_angles_offsets; _measurement.delta_velocity -= _gimbalParams.delta_velocity_offsets; _measurement.delta_angles -= _gimbalParams.delta_angles_offsets; } /* send a gimbal report to the GCS for display purposes */ void AP_Gimbal::send_report(mavlink_channel_t chan) const { mavlink_msg_gimbal_report_send(chan, 0, 0, // send as broadcast _report_msg.delta_time, _report_msg.delta_angle_x, _report_msg.delta_angle_y, _report_msg.delta_angle_z, _report_msg.delta_velocity_x, _report_msg.delta_velocity_y, _report_msg.delta_velocity_z, _report_msg.joint_roll, _report_msg.joint_el, _report_msg.joint_az); } 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(); } 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; } Vector3f AP_Gimbal::getGimbalRateDemVecTilt(const Quaternion &quatEst) { // Calculate the gimbal 321 Euler angle estimates relative to earth frame Vector3f eulerEst = quatEst.to_vector312(); // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle) Quaternion quatDem; quatDem.from_vector312( _angle_ef_target_rad.x, _angle_ef_target_rad.y, eulerEst.z); //divide the demanded quaternion by the estimated to get the error Quaternion quatErr = quatDem / quatEst; // Convert to a delta rotation using a small angle approximation quatErr.normalize(); Vector3f deltaAngErr; float scaler; if (quatErr[0] >= 0.0f) { scaler = 2.0f; } else { scaler = -2.0f; } deltaAngErr.x = scaler * quatErr[1]; deltaAngErr.y = scaler * quatErr[2]; deltaAngErr.z = scaler * quatErr[3]; // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.K_gimbalRate; return gimbalRateDemVecTilt; } Vector3f AP_Gimbal::getGimbalRateDemVecForward(const Quaternion &quatEst) { // quaternion demanded at the previous time step static float lastDem; // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation float delta = _angle_ef_target_rad.y - lastDem; lastDem = _angle_ef_target_rad.y; Vector3f gimbalRateDemVecForward; gimbalRateDemVecForward.y = delta / _measurement.delta_time; return gimbalRateDemVecForward; } Vector3f AP_Gimbal::getGimbalRateDemVecGyroBias() { Vector3f gyroBias; _ekf.getGyroBias(gyroBias); return gyroBias; } void AP_Gimbal::send_control(mavlink_channel_t chan) { mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid, gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z); } void AP_Gimbal::update_target(Vector3f newTarget) { // Low-pass filter _angle_ef_target_rad.y = _angle_ef_target_rad.y + 0.02f*(newTarget.y - _angle_ef_target_rad.y); // Update tilt _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y,radians(-90),radians(0)); } Vector3f AP_Gimbal::getGimbalEstimateEF() { Quaternion quatEst; _ekf.getQuat(quatEst); return quatEst.to_vector312(); } bool AP_Gimbal::isCopterFlipped() { return (_ahrs.cos_roll()*_ahrs.cos_pitch() < 0.5f); } #endif // AP_AHRS_NAVEKF_AVAILABLE