diff options
Diffstat (limited to 'src/drivers/gimbal/gimbal.cpp')
| -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) { |