/*
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_pitch,
_gimbal_report.joint_yaw);
_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, // demanded rates
gyroBias.x, gyroBias.y, gyroBias.z);
}
void SoloGimbal::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
_ang_vel_dem_rads.zero();
switch(get_mode()) {
case GIMBAL_MODE_POS_HOLD_FF: {
_ang_vel_dem_rads += get_ang_vel_dem_body_lock();
_ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
float _ang_vel_dem_radsLen = _ang_vel_dem_rads.length();
if (_ang_vel_dem_radsLen > radians(400)) {
_ang_vel_dem_rads *= radians(400)/_ang_vel_dem_radsLen;
}
if (HAVE_PAYLOAD_SPACE(chan, GIMBAL_CONTROL)) {
mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
_ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
}
break;
}
case GIMBAL_MODE_STABILIZE: {
_ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_tilt(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
float ang_vel_dem_norm = _ang_vel_dem_rads.length();
if (ang_vel_dem_norm > radians(400)) {
_ang_vel_dem_rads *= radians(400)/ang_vel_dem_norm;
}
if (HAVE_PAYLOAD_SPACE(chan, GIMBAL_CONTROL)) {
mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
_ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.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 (!is_equal(max_torque,_max_torque) && !is_zero(max_torque)) {
_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);
}
}
void AP_Gimbal::send_control(mavlink_channel_t chan)
{
mavlink_msg_gimbal_control_send(chan,_sysid, _compid,
gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
}
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);
}
}
