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

Copter: do-mount-control ignored if no mount

Browse Source 
Co-authored-by: srirajshukla <srirajshukla@gmail.com>
Co-authored-by: Yash Vadi <yvadi9274@yahoo.com>
This commit is contained in:
Randy Mackay 2021-03-10 12:28:29 +09:00
parent 29043d62e9
commit be29b23195
2 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/GCS_Mavlink.cpp
View File

@ -685,15 +685,17 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet)
{
// if the mount doesn't do pan control then yaw the entire vehicle instead:
switch (packet.command) {
#if HAL_MOUNT_ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
if (!copter.camera_mount.has_pan_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() != copter.camera_mount.MountType::Mount_Type_None) &&
!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 * 0.01f,
0.0f,
0,0);
0,
false);
}
break;
#endif
@ -955,13 +957,14 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
switch (msg.msgid) {
#if HAL_MOUNT_ENABLED
case MAVLINK_MSG_ID_MOUNT_CONTROL:
if (!copter.camera_mount.has_pan_control()) {
// if the mount doesn't do pan control then yaw the entire vehicle instead:
// 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() != copter.camera_mount.MountType::Mount_Type_None) &&
!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
mavlink_msg_mount_control_get_input_c(&msg) * 0.01f,
0.0f,
0,
0);
false);
break;
}

7
ArduCopter/mode_auto.cpp
View File

@ -1452,9 +1452,12 @@ void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if HAL_MOUNT_ENABLED
if (!copter.camera_mount.has_pan_control()) {
auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0);
// 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() != copter.camera_mount.MountType::Mount_Type_None) &&
!copter.camera_mount.has_pan_control()) {
auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,false);
}
// pass the target angles to the camera mount
copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
#endif
}