dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Implemented Quaternion position for Gimbal (not tested)

This commit is contained in:
Anton Matosov 2015-03-06 18:52:52 -08:00
parent e5a73f0795
commit f8e471d95c
1 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/drivers/gimbal/gimbal.cpp
View File

@ -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) {