diff options
author | Andreas Antener <antener_a@gmx.ch> | 2015-03-13 19:00:16 +0100 |
---|---|---|
committer | Andreas Antener <antener_a@gmx.ch> | 2015-03-13 19:00:16 +0100 |
commit | 76aed9e58dcf7345e41b8e0e43d7af5f76418075 (patch) | |
tree | fdf5bb155ac48eb37e59eae661598953ff91a43f /src/drivers/gimbal/gimbal.cpp | |
parent | 030a96aa2c4a512a71b32dbd616eb8411ac67c6e (diff) | |
download | px4-firmware-76aed9e58dcf7345e41b8e0e43d7af5f76418075.tar.gz px4-firmware-76aed9e58dcf7345e41b8e0e43d7af5f76418075.tar.bz2 px4-firmware-76aed9e58dcf7345e41b8e0e43d7af5f76418075.zip |
- implemented configuration with mount config cmd
- remember recieved mount control values
Diffstat (limited to 'src/drivers/gimbal/gimbal.cpp')
-rw-r--r-- | src/drivers/gimbal/gimbal.cpp | 83 |
1 files changed, 64 insertions, 19 deletions
diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index ccda4c0a5..2e254c45d 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -121,8 +121,12 @@ private: int _vehicle_command_sub; int _att_sub; - bool _attitude_compensation; + bool _attitude_compensation_roll; + bool _attitude_compensation_pitch; + bool _attitude_compensation_yaw; bool _initialized; + bool _control_cmd_set; + bool _config_cmd_set; orb_advert_t _actuator_controls_2_topic; @@ -130,6 +134,9 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + struct vehicle_command_s _control_cmd; + struct vehicle_command_s _config_cmd; + /** * Initialise the automatic measurement state machine and start it. * @@ -169,7 +176,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), - _attitude_compensation(true), + _attitude_compensation_pitch(true), + _attitude_compensation_yaw(true), + _attitude_compensation_roll(true), _initialized(false), _actuator_controls_2_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), @@ -221,7 +230,9 @@ Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case GIMBALIOCATTCOMPENSATE: - _attitude_compensation = (arg != 0); + _attitude_compensation_roll = (arg != 0); + _attitude_compensation_pitch = (arg != 0); + _attitude_compensation_yaw = (arg != 0); return OK; default: @@ -286,22 +297,30 @@ Gimbal::cycle() float yaw = 0.0f; - if (_attitude_compensation) { - if (_att_sub < 0) { - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - } + if (_att_sub < 0) { + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + } - vehicle_attitude_s att; + vehicle_attitude_s att; - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + if (_attitude_compensation_roll) { roll = -att.roll; + updated = true; + } + + if (_attitude_compensation_pitch) { pitch = -att.pitch; - yaw = att.yaw; + updated = true; + } + if (_attitude_compensation_yaw) { + yaw = att.yaw; updated = true; } + struct vehicle_command_s cmd; bool cmd_updated; @@ -312,22 +331,47 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); - VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7; - debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2); + if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL + || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + + _control_cmd = cmd; + _control_cmd_set = true; + + } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + _config_cmd = cmd; + _config_cmd_set = true; + } + + } + + if (_config_cmd_set) { + + _config_cmd_set = false; + + _attitude_compensation_roll = _config_cmd.param2 == 1; + _attitude_compensation_pitch = _config_cmd.param3 == 1; + _attitude_compensation_yaw = _config_cmd.param4 == 1; + + } + + if (_control_cmd_set) { + + VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; + 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_CMD_DO_MOUNT_CONTROL && mountMode == 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 * 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 += 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 (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { - float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4}; + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == 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(); roll += gimablDirectionEuler(0); @@ -336,6 +380,7 @@ Gimbal::cycle() updated = true; } + } if (updated) { |