diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 249fc22dac..40b86d1421 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -331,7 +331,7 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL - || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { _control_cmd = cmd; _control_cmd_set = true; @@ -358,20 +358,21 @@ Gimbal::cycle() if (_control_cmd_set) { unsigned mountMode = _control_cmd.param7; - DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + DEVICE_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_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ 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; yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; - + updated = true; } if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { 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();