void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
mavlink_gimbal_report_t report_msg;
mavlink_msg_gimbal_report_decode(msg, &report_msg);
_gimbalParams.set_channel(chan);
if(report_msg.target_system != 1) {
// gimbal must have been power cycled or reconnected
_gimbalParams.reset();
_gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1);
return;
}
_gimbalParams.update();
if(!_gimbalParams.initialized()){
return;
}
_last_report_msg_ms = hal.scheduler->millis();
extract_feedback(report_msg);
update_mode();
update_state();
switch(_mode) {
case GIMBAL_MODE_IDLE:
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
break;
case GIMBAL_MODE_POS_HOLD:
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
break;
case GIMBAL_MODE_POS_HOLD_FF:
case GIMBAL_MODE_STABILIZE:
send_control(chan);
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
default:
break;
}
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);
}
}
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_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);
}
}
