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);
}
}
/*
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 AP_Gimbal::decode_feedback(mavlink_message_t *msg)
{
mavlink_gimbal_report_t report_msg;
mavlink_msg_gimbal_report_decode(msg, &report_msg);
_measurement.delta_time = report_msg.delta_time;
_measurement.delta_angles.x = report_msg.delta_angle_x;
_measurement.delta_angles.y = report_msg.delta_angle_y,
_measurement.delta_angles.z = report_msg.delta_angle_z;
_measurement.delta_velocity.x = report_msg.delta_velocity_x,
_measurement.delta_velocity.y = report_msg.delta_velocity_y,
_measurement.delta_velocity.z = report_msg.delta_velocity_z;
_measurement.joint_angles.x = report_msg.joint_roll;
_measurement.joint_angles.y = report_msg.joint_el,
_measurement.joint_angles.z = report_msg.joint_az;
//apply joint angle compensation
_measurement.joint_angles -= _gimbalParams.joint_angles_offsets;
_measurement.delta_velocity -= _gimbalParams.delta_velocity_offsets;
_measurement.delta_angles -= _gimbalParams.delta_angles_offsets;
}
