aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mount_MAVLink: remove code now in AP_Gimbal

Browse Source
This commit is contained in:
Arthur Benemann 2015-04-10 00:06:01 -07:00 committed by Randy Mackay
parent 2053d5e42e
commit 558a69bdca
2 changed files with 1 additions and 288 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

250
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -4,22 +4,13 @@
#if AP_AHRS_NAVEKF_AVAILABLE
#include <GCS_MAVLink.h>
#define MOUNT_DEBUG 0
#define TILT_CONTROL_ONLY 0
#if MOUNT_DEBUG
#include <stdio.h>
#endif
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),
K_gimbalRate(0.1f),
angRateLimit(0.5f),
yawRateFiltPole(10.0f),
yawErrorLimit(0.1f),
vehicleYawRateFilt(0)
_initialised(false)
{}
// init - performs any required initialisation for this instance
@ -96,49 +87,11 @@ void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
// do nothing - we rely on the mount sending the messages directly
}
/*
handle a GIMBAL_REPORT message
*/
void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
{
// just save it for future processing and reporting to GCS for now
mavlink_msg_gimbal_report_decode(msg, &_gimbal_report);
Vector3f delta_angles(_gimbal_report.delta_angle_x,
_gimbal_report.delta_angle_y,
_gimbal_report.delta_angle_z);
Vector3f delta_velocity(_gimbal_report.delta_velocity_x,
_gimbal_report.delta_velocity_y,
_gimbal_report.delta_velocity_z);
Vector3f joint_angles(_gimbal_report.joint_roll,
_gimbal_report.joint_el,
_gimbal_report.joint_az);
_ekf.RunEKF(_gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles);
/*
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
// 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,
msg->sysid,
msg->compid,
rateDemand.x, rateDemand.y, rateDemand.z);
}
/*
@ -146,207 +99,6 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
*/
void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
{
mavlink_msg_gimbal_report_send(chan,
0, 0, // send as broadcast
_gimbal_report.delta_time,
_gimbal_report.delta_angle_x,
_gimbal_report.delta_angle_y,
_gimbal_report.delta_angle_z,
_gimbal_report.delta_velocity_x,
_gimbal_report.delta_velocity_y,
_gimbal_report.delta_velocity_z,
_gimbal_report.joint_roll,
_gimbal_report.joint_el,
_gimbal_report.joint_az);
float tilt;
Vector3f velocity, euler, gyroBias;
_ekf.getDebug(tilt, velocity, euler, gyroBias);
#if MOUNT_DEBUG
::printf("tilt=%.2f euler=(%.2f, %.2f, %.2f) vel=(%.2f, %.2f %.2f)\n",
tilt,
degrees(euler.x), degrees(euler.y), degrees(euler.z),
(velocity.x), (velocity.y), (velocity.z));
#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 using a first order approximation
Vector3f deltaAngErr;
float scaler;
if (quatErr[0] >= 0.0f) {
scaler = 2.0f;
} else {
scaler = -2.0f;
}
deltaAngErr.x = quatErr[1] * scaler;
deltaAngErr.y = quatErr[2] * scaler;
deltaAngErr.z = quatErr[3] * scaler;
// 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

39
libraries/AP_Mount/AP_Mount_MAVLink.h
View File

@ -50,45 +50,6 @@ public:
private:
// internal variables
bool _initialised; // true once the driver has been initialised
// state of small EKF for gimbal
SmallEKF _ekf;
// 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