From 6561b07caf6c544708eaf05a11eeb65c527c2cd8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Oct 2020 17:42:53 +0200 Subject: [PATCH] vmount: support legacy gimbal v1 mission commands This is a hotfix to restore gimbal functionality for SITL set-ups that are set to MNT_MODE_IN 4 (gimbal protocol v2) but still need to accept gimbal v1 commands that might be part of a mission, e.g. uploaded by MAVSDK. --- src/modules/vmount/input_mavlink.cpp | 48 ++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index ec9e9134c4..d5b40ea955 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -825,9 +825,57 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); } + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + + switch ((int)vehicle_command.param7) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + _control_data.gimbal_shutter_retract = true; + + /* FALLTHROUGH */ + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + _control_data.type = ControlData::Type::Neutral; + + *control_data = &_control_data; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: + _control_data.type = ControlData::Type::Angle; + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + _control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F; + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + _control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F; + // both specs have yaw on channel 2 + _control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F; + + // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. + if (_control_data.type_data.angle.angles[2] > M_PI_F) { + _control_data.type_data.angle.angles[2] -= 2 * M_PI_F; + } + + *control_data = &_control_data; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4); + + *control_data = &_control_data; + break; + } + + _ack_vehicle_command(&vehicle_command); + + } else { exit_loop = false; } + } if (polls[4].revents & POLLIN) {