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); } }