zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Mount_MAVLink: remove code now in AP_Gimbal

master
Arthur Benemann 10 years ago committed by Randy Mackay
parent
2053d5e42e
commit
558a69bdca
2 changed files with 1 additions and 288 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 250
      libraries/AP_Mount/AP_Mount_MAVLink.cpp
  2. 39
      libraries/AP_Mount/AP_Mount_MAVLink.h

250
libraries/AP_Mount/AP_Mount_MAVLink.cpp
Unescape View File

@ -4,22 +4,13 @@ @@ -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) @@ -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 @@ -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
Unescape View File

@ -50,45 +50,6 @@ public: @@ -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

Write Preview
Loading…
Cancel
Save