Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: fixed reporting of MAVLink gimbal position to GCS

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-23 18:39:12 +10:00
parent 7a0e07e67a
commit 19419fd901
3 changed files with 34 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Mount/AP_Gimbal.cpp
View File

@ -30,19 +30,18 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
{
mavlink_gimbal_report_t report_msg;
mavlink_msg_gimbal_report_decode(msg, &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;
_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;
@ -50,6 +49,25 @@ void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
_measurement.delta_angles -= _gimbalParams.delta_angles_offsets;
}
/*
send a gimbal report to the GCS for display purposes
*/
void AP_Gimbal::send_report(mavlink_channel_t chan) const
{
mavlink_msg_gimbal_report_send(chan,
0, 0, // send as broadcast
_report_msg.delta_time,
_report_msg.delta_angle_x,
_report_msg.delta_angle_y,
_report_msg.delta_angle_z,
_report_msg.delta_velocity_x,
_report_msg.delta_velocity_y,
_report_msg.delta_velocity_z,
_report_msg.joint_roll,
_report_msg.joint_el,
_report_msg.joint_az);
}
void AP_Gimbal::update_state()
{
// Run the gimbal attitude and gyro bias estimator

3
libraries/AP_Mount/AP_Gimbal.h
View File

@ -39,6 +39,7 @@ public:
void update_target(Vector3f newTarget);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
void send_report(mavlink_channel_t chan) const;
Vector3f getGimbalEstimateEF();
@ -72,6 +73,8 @@ private:
uint8_t _compid;
mavlink_gimbal_report_t _report_msg;
void send_control(mavlink_channel_t chan);
void update_state();
void decode_feedback(mavlink_message_t *msg);

1
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -105,6 +105,7 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
*/
void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
{
_gimbal.send_report(chan);
}
#endif // AP_AHRS_NAVEKF_AVAILABLE