aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount:Add GIMBAL_MANAGER_SET_ATTITUDE support

Browse Source
This commit is contained in:
Asif Khan 2023-03-24 09:20:40 +05:30 committed by Peter Barker
parent 21d7a8102e
commit 91935fc404
3 changed files with 59 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_Mount/AP_Mount.cpp
View File

@ -337,6 +337,60 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
return MAV_RESULT_FAILED;
}
void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){
mavlink_gimbal_manager_set_attitude_t packet;
mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet);
AP_Mount_Backend *backend;
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is
// 2nd gimbal, etc
const uint8_t instance = packet.gimbal_device_id;
if (instance == 0) {
backend = get_primary();
} else {
backend = get_instance(instance - 1);
}
if (backend == nullptr) {
return;
}
// check flags for change to RETRACT
const uint32_t flags = packet.flags;
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
backend->set_mode(MAV_MOUNT_MODE_RETRACT);
return;
}
// check flags for change to NEUTRAL
if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
return;
}
const Quaternion att_quat{packet.q};
if (!att_quat.is_nan()) {
// convert quaternion to euler angles
float roll_rad, pitch_rad, yaw_rad;
att_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
// radian to deg conversion
const float roll_deg = degrees(roll_rad);
const float pitch_deg = degrees(pitch_rad);
const float yaw_deg = degrees(yaw_rad);
backend->set_angle_target(roll_deg, pitch_deg, yaw_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
return;
}
if (!isnan(packet.angular_velocity_x) && !isnan(packet.angular_velocity_y) && !isnan(packet.angular_velocity_y)) {
const float roll_rate_degs = degrees(packet.angular_velocity_x);
const float pitch_rate_degs = degrees(packet.angular_velocity_y);
const float yaw_rate_degs = degrees(packet.angular_velocity_z);
backend->set_rate_target(roll_rate_degs, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
return;
}
}
MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
{
@ -624,6 +678,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
handle_global_position_int(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
handle_gimbal_manager_set_attitude(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
handle_gimbal_device_information(msg);
break;

1
libraries/AP_Mount/AP_Mount.h
View File

@ -215,6 +215,7 @@ private:
MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet);
void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg);
void handle_global_position_int(const mavlink_message_t &msg);
void handle_gimbal_device_information(const mavlink_message_t &msg);
void handle_gimbal_device_attitude_status(const mavlink_message_t &msg);

1
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3868,6 +3868,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_GIMBAL_REPORT:
case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
handle_mount_message(msg);
break;
#endif