void SoloGimbal::receive_feedback(mavlink_channel_t chan, const mavlink_message_t *msg)
{
mavlink_gimbal_report_t report_msg;
mavlink_msg_gimbal_report_decode(msg, &report_msg);
uint32_t tnow_ms = AP_HAL::millis();
_last_report_msg_ms = tnow_ms;
_gimbalParams.set_channel(chan);
if (report_msg.target_system != 1) {
_state = GIMBAL_STATE_NOT_PRESENT;
} else {
GCS_MAVLINK::set_channel_private(chan);
}
switch(_state) {
case GIMBAL_STATE_NOT_PRESENT:
// gimbal was just connected or we just rebooted, transition to PRESENT_INITIALIZING
_gimbalParams.reset();
_gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1);
_state = GIMBAL_STATE_PRESENT_INITIALIZING;
break;
case GIMBAL_STATE_PRESENT_INITIALIZING:
_gimbalParams.update();
if (_gimbalParams.initialized()) {
// parameters done initializing, finalize initialization and transition to aligning
extract_feedback(report_msg);
_ang_vel_mag_filt = 20;
_filtered_joint_angles = _measurement.joint_angles;
_vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
_ekf.reset();
_state = GIMBAL_STATE_PRESENT_ALIGNING;
}
break;
case GIMBAL_STATE_PRESENT_ALIGNING:
_gimbalParams.update();
extract_feedback(report_msg);
update_estimators();
if (_ekf.getStatus()) {
// EKF done aligning, transition to running
_state = GIMBAL_STATE_PRESENT_RUNNING;
}
break;
case GIMBAL_STATE_PRESENT_RUNNING:
_gimbalParams.update();
extract_feedback(report_msg);
update_estimators();
break;
}
send_controls(chan);
}
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);
}
}
