fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 12:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

vmount: support legacy gimbal v1 mission commands

Browse Source 
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.
This commit is contained in:
Julian Oes
2021-02-17 13:54:34 -05:00
committed by Daniel Agar
parent a3e0e2e84f
commit 6561b07caf
1 changed files with 48 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/input_mavlink.cpp
+48
View File
@@ -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) {