Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Handle DO_MOUNT_CONTROL yaw angle as body frame 

And only accept if the mode is MAV_MOUNT_MODE_MAVLINK_TARGETING. This matches the handlers in AP_Mount.
This commit is contained in:
Nick Exton 2024-05-16 11:25:51 +10:00 committed by Randy Mackay
parent 3bac3618e1
commit 03db86427a
2 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/GCS_Mavlink.cpp
View File

@ -860,9 +860,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t
switch (packet.command) {
case MAV_CMD_DO_MOUNT_CONTROL:
// if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
if (((MAV_MOUNT_MODE)packet.z == MAV_MOUNT_MODE_MAVLINK_TARGETING) &&
(copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_yaw_angle_rate((float)packet.param3, 0.0f);
// Per the handler in AP_Mount, DO_MOUNT_CONTROL yaw angle is in body frame, which is
// equivalent to an offset to the current yaw demand.
copter.flightmode->auto_yaw.set_yaw_angle_offset(packet.param3);
}
break;
default:
@ -1131,11 +1134,12 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_MOUNT_CONTROL:
// if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
(copter.camera_mount.get_mode() == MAV_MOUNT_MODE_MAVLINK_TARGETING) &&
!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_yaw_angle_rate(
mavlink_msg_mount_control_get_input_c(&msg) * 0.01f,
0.0f);
// Per the handler in AP_Mount, MOUNT_CONTROL yaw angle is in body frame, which is
// equivalent to an offset to the current yaw demand.
const float yaw_offset_d = mavlink_msg_mount_control_get_input_c(&msg) * 0.01f;
copter.flightmode->auto_yaw.set_yaw_angle_offset(yaw_offset_d);
break;
}
}

4
ArduCopter/mode_auto.cpp
View File

@ -1983,7 +1983,9 @@ void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
// if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
!copter.camera_mount.has_pan_control()) {
auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f);
// Per the handler in AP_Mount, DO_MOUNT_CONTROL yaw angle is in body frame, which is
// equivalent to an offset to the current yaw demand.
auto_yaw.set_yaw_angle_offset(cmd.content.mount_control.yaw);
}
// pass the target angles to the camera mount
copter.camera_mount.set_angle_target(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw, false);