diff options
author | Anton Matosov <anton.matosov@gmail.com> | 2015-03-06 18:52:52 -0800 |
---|---|---|
committer | Anton Matosov <anton.matosov@gmail.com> | 2015-03-06 18:57:38 -0800 |
commit | f8e471d95cbca9434e99b68a9f3e146c0a597b53 (patch) | |
tree | c8b25bdcebcb9d716e0c0fd2433dc3d53aa6311a | |
parent | e5a73f07951c21cefc5e8d6544d84c691903453c (diff) | |
download | px4-firmware-f8e471d95cbca9434e99b68a9f3e146c0a597b53.tar.gz px4-firmware-f8e471d95cbca9434e99b68a9f3e146c0a597b53.tar.bz2 px4-firmware-f8e471d95cbca9434e99b68a9f3e146c0a597b53.zip |
Implemented Quaternion position for Gimbal (not tested)
-rw-r--r-- | src/drivers/gimbal/gimbal.cpp | 13 |
1 files changed, 6 insertions, 7 deletions
diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 441e5b40c..ccda4c0a5 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -75,6 +75,7 @@ #include <board_config.h> #include <mathlib/math/test/test.hpp> +#include <mathlib/math/Quaternion.hpp> /* Configuration Constants */ @@ -325,18 +326,16 @@ Gimbal::cycle() } -#if 0 - // XXX change this to the real quaternion command if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4}; + math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); - /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ - roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1; - pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2; - yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3; + roll += gimablDirectionEuler(0); + pitch += gimablDirectionEuler(1); + yaw += gimablDirectionEuler(2); updated = true; } -#endif } if (updated) { |