fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 06:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

gimbal driver: Update topic names

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-27 15:21:33 -07:00
parent aa531aec64
commit ffc6bb7e85
1 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gimbal/gimbal.cpp
+8 -6
View File
@@ -331,13 +331,13 @@ Gimbal::cycle()
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd; _control_cmd = cmd;
_control_cmd_set = true; _control_cmd_set = true;
} else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_config_cmd = cmd; _config_cmd = cmd;
_config_cmd_set = true; _config_cmd_set = true;
@@ -358,10 +358,11 @@ Gimbal::cycle()
if (_control_cmd_set) { if (_control_cmd_set) {
VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; unsigned mountMode = _control_cmd.param7;
debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
@@ -370,7 +371,8 @@ Gimbal::cycle()
updated = true; updated = true;
} }
if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();