diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 9edba14a36..8e10371e1a 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -5,6 +5,7 @@ #include #define MOUNT_DEBUG 0 +#define TILT_CONTROL_ONLY 0 #if MOUNT_DEBUG #include @@ -13,7 +14,12 @@ AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_Backend(frontend, state, instance), _initialised(false), - _ekf(frontend._ahrs) + _ekf(frontend._ahrs), + K_gimbalRate(0.1f), + angRateLimit(0.5f), + yawRateFiltPole(10.0f), + yawErrorLimit(0.1f), + vehicleYawRateFilt(0) {} // init - performs any required initialisation for this instance @@ -108,45 +114,25 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess Vector3f joint_angles(_gimbal_report.joint_roll, _gimbal_report.joint_pitch, _gimbal_report.joint_yaw); + _ekf.RunEKF(_gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles); - // get the gyro bias data - Vector3f gyroBias; - _ekf.getGyroBias(gyroBias); + /* + we have two different gimbal control algorithms. One does tilt + control only, but has better control characteristics. The other + does roll/tilt/yaw, but has worset control characteristics + */ +#if TILT_CONTROL_ONLY + Vector3f rateDemand = gimbal_update_control2(_angle_ef_target_rad, + _gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles); +#else + Vector3f rateDemand = gimbal_update_control1(_angle_ef_target_rad, + _gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles); +#endif - // get the gimbal estimated quaternion - Quaternion quatEst; - _ekf.getQuat(quatEst); - - // set the demanded quaternion - tilt down with a roll and yaw of zero - Quaternion quatDem; - quatDem.from_euler(_angle_ef_target_rad.x, - _angle_ef_target_rad.y, - _angle_ef_target_rad.z); - - //divide the demanded quaternion by the estimated to get the error - Quaternion quatErr = quatDem / quatEst; - - // convert the quaternion to an angle error vector - Vector3f deltaAngErr; - float scaler = 1.0f-quatErr[0]*quatErr[0]; - if (scaler > 1e-12) { - scaler = 1.0f/sqrtf(scaler); - deltaAngErr.x = quatErr[1] * scaler; - deltaAngErr.y = quatErr[2] * scaler; - deltaAngErr.z = quatErr[3] * scaler; - } else { - deltaAngErr.zero(); - } - - // multiply the angle error vector by a gain to calculate a demanded gimbal rate - Vector3f rateDemand = deltaAngErr * 1.0f; - - // Constrain the demanded rate to a length of 0.5 rad /sec - float length = rateDemand.length(); - if (length > 0.5f) { - rateDemand = rateDemand * (0.5f / length); - } + // for now send a zero gyro bias update and incorporate into the + // demanded rates + Vector3f gyroBias(0,0,0); // send the gimbal control message mavlink_msg_gimbal_control_send(chan, @@ -184,4 +170,184 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan) #endif } +/* + calculate demanded rates for the gimbal + */ +Vector3f AP_Mount_MAVLink::gimbal_update_control1(const Vector3f &ef_target_euler_rad, + float delta_time, + const Vector3f &delta_angles, + const Vector3f &delta_velocity, + const Vector3f &joint_angles) +{ + // get the gyro bias data + Vector3f gyroBias; + _ekf.getGyroBias(gyroBias); + + // get the gimbal estimated quaternion + Quaternion quatEst; + _ekf.getQuat(quatEst); + + // set the demanded quaternion - tilt down with a roll and yaw of zero + Quaternion quatDem; + quatDem.from_euler(ef_target_euler_rad.x, ef_target_euler_rad.y, ef_target_euler_rad.z); + + //divide the demanded quaternion by the estimated to get the error + Quaternion quatErr = quatDem / quatEst; + + // convert the quaternion to an angle error vector + Vector3f deltaAngErr; + float scaler = 1.0f-quatErr[0]*quatErr[0]; + if (scaler > 1e-12) { + scaler = 1.0f/sqrtf(scaler); + deltaAngErr.x = quatErr[1] * scaler; + deltaAngErr.y = quatErr[2] * scaler; + deltaAngErr.z = quatErr[3] * scaler; + } else { + deltaAngErr.zero(); + } + + // multiply the angle error vector by a gain to calculate a demanded gimbal rate + Vector3f rateDemand = deltaAngErr * 1.0f; + + // Constrain the demanded rate to a length of 0.5 rad /sec + float length = rateDemand.length(); + if (length > 0.5f) { + rateDemand = rateDemand * (0.5f / length); + } + + return rateDemand; +} + + +// convert the quaternion to rotation vector +Vector3f AP_Mount_MAVLink::quaternion_to_vector(const Quaternion &quat) +{ + Vector3f vector; + float scaler = 1.0f-quat[0]*quat[0]; + if (scaler > 1e-12f) { + scaler = 1.0f/sqrtf(scaler); + if (quat[0] < 0.0f) { + scaler *= -1.0f; + } + vector.x = quat[1] * scaler; + vector.y = quat[2] * scaler; + vector.z = quat[3] * scaler; + } else { + vector.zero(); + } + return vector; +} + +// Define rotation matrix using a 312 rotation sequence vector +Matrix3f AP_Mount_MAVLink::vector312_to_rotation_matrix(const Vector3f &vector) +{ + Matrix3f matrix; + float cosPhi = cosf(vector.x); + float cosTheta = cosf(vector.y); + float sinPhi = sinf(vector.x); + float sinTheta = sinf(vector.y); + float sinPsi = sinf(vector.z); + float cosPsi = cosf(vector.z); + matrix[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta; + matrix[1][0] = -sinPsi*cosPhi; + matrix[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi; + matrix[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta; + matrix[1][1] = cosPsi*cosPhi; + matrix[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi; + matrix[0][2] = -sinTheta*cosPhi; + matrix[1][2] = sinPhi; + matrix[2][2] = cosTheta*cosPhi; + return matrix; +} + + +/* + 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; +} + +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; +} + +Vector3f AP_Mount_MAVLink::getGimbalRateDemVecTilt(const Vector3f &ef_target_euler_rad, const Quaternion &quatEst) +{ + // Calculate the gimbal 321 Euler angle estimates relative to earth frame + Vector3f eulerEst; + quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z); + + // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle) + Quaternion quatDem; + //TODO receive target from AP_Mount + quatDem.from_euler(0, ef_target_euler_rad.y, eulerEst.z); + + //divide the demanded quaternion by the estimated to get the error + Quaternion quatErr = quatDem / quatEst; + + // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt + Vector3f gimbalRateDemVecTilt = quaternion_to_vector(quatErr) * K_gimbalRate; + return gimbalRateDemVecTilt; +} + +Vector3f AP_Mount_MAVLink::getGimbalRateDemVecForward(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst) +{ + // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation + Quaternion quatDemForward; + quatDemForward.from_euler(0, ef_target_euler_rad.y, 0); + Quaternion deltaQuat = quatDemForward / lastQuatDem; + lastQuatDem = quatDemForward; + + // convert to a rotation vector and divide by delta time to obtain a forward path rate demand + Vector3f gimbalRateDemVecForward = quaternion_to_vector(deltaQuat) * (1.0f / delta_time); + return gimbalRateDemVecForward; +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 21d6c1fe07..660b94c967 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -56,7 +56,41 @@ private: // keep last gimbal report mavlink_gimbal_report_t _gimbal_report; + + float vehicleYawRateFilt; + const float K_gimbalRate; + const float angRateLimit; + + // circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate + // this frequency must not be larger than the update rate (Hz). + // reducing it makes the gimbal yaw less responsive to vehicle yaw + // increasing it makes the gimbal yawe more responsive to vehicle yaw + const float yawRateFiltPole; + + // amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode + // reducing this makes the gimbal respond more to vehicle yaw disturbances + const float yawErrorLimit; + + // quaternion demanded at the previous time step + Quaternion lastQuatDem; + + Vector3f quaternion_to_vector(const Quaternion &quat); + Matrix3f vector312_to_rotation_matrix(const Vector3f &vector); + Vector3f gimbal_update_control1(const Vector3f &ef_target_euler_rad, + float delta_time, + const Vector3f &delta_angles, + const Vector3f &delta_velocity, + const Vector3f &joint_angles); + Vector3f gimbal_update_control2(const Vector3f &ef_target_euler_rad, + float delta_time, + const Vector3f &delta_angles, + const Vector3f &delta_velocity, + const Vector3f &joint_angles); + Vector3f getGimbalRateDemVecYaw(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst, const Vector3f &joint_angles); + Vector3f getGimbalRateDemVecTilt(const Vector3f &ef_target_euler_rad, const Quaternion &quatEst); + Vector3f getGimbalRateDemVecForward(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst); }; + #endif // AP_AHRS_NAVEKF_AVAILABLE #endif // __AP_MOUNT_MAVLINK_H__