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

Implemented yaw componsation

This commit is contained in:
Anton Matosov 2015-03-03 23:21:31 -08:00
parent 610a714e85
commit c76839df73
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -92,7 +92,7 @@ static const int ERROR = -1;
#define GIMBAL_UPDATE_INTERVAL (50 * 1000) #define GIMBAL_UPDATE_INTERVAL (50 * 1000)
#define GIMBALIOCATTCOMPENSATE 1 #define GIMBALIOCATTCOMPENSATE 1
class Gimbal : public device::CDev class Gimbal : public device::CDev
{ {
@ -282,6 +282,7 @@ Gimbal::cycle()
float roll = 0.0f; float roll = 0.0f;
float pitch = 0.0f; float pitch = 0.0f;
float yaw = 0.0f;
if (_attitude_compensation) { if (_attitude_compensation) {
@ -295,6 +296,7 @@ Gimbal::cycle()
roll = -att.roll; roll = -att.roll;
pitch = -att.pitch; pitch = -att.pitch;
yaw = att.yaw;
updated = true; updated = true;
} }
@ -311,36 +313,43 @@ Gimbal::cycle()
debug("cmd: %d, param7 %d | param1: %8.4f param2: %8.4f", cmd.command, (int)cmd.param7, (double)cmd.param1, (double)cmd.param2); debug("cmd: %d, param7 %d | param1: %8.4f param2: %8.4f", cmd.command, (int)cmd.param7, (double)cmd.param1, (double)cmd.param2);
if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && equal(cmd.param7, VEHICLE_MOUNT_MODE_MAVLINK_TARGETING)) { VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7;
if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == 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 * cmd.param1; 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; 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;
updated = true; updated = true;
} }
#if 0
// XXX change this to the real quaternion command // XXX change this to the real quaternion command
if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && equal(cmd.param7, VEHICLE_MOUNT_MODE_MAVLINK_TARGETING)) { if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == 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 * cmd.param1; 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; 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;
updated = true; updated = true;
} }
#endif
} }
if (updated) { if (updated) {
struct actuator_controls_s controls; struct actuator_controls_s controls;
debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */ /* fill in the final control values */
controls.timestamp = hrt_absolute_time(); controls.timestamp = hrt_absolute_time();
controls.control[0] = roll; controls.control[0] = roll;
controls.control[1] = pitch; controls.control[1] = pitch;
controls.control[2] = yaw;
/* publish it */ /* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls); orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
@ -529,5 +538,5 @@ gimbal_main(int argc, char *argv[])
gimbal::info(); gimbal::info();
} }
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); errx(1, "unrecognized command, try 'start', 'stop', 'reset', 'test' or 'info'");
} }