コード例 #1
0
int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
{
	// Default to notify that there was no change.
	*control_data = nullptr;

	const int num_poll = 1;
	px4_pollfd_struct_t polls[num_poll];
	polls[0].fd = 		_vehicle_command_sub;
	polls[0].events = 	POLLIN;

	int poll_timeout = (int)timeout_ms;

	bool exit_loop = false;

	while (!exit_loop && poll_timeout >= 0) {
		hrt_abstime poll_start = hrt_absolute_time();

		int ret = px4_poll(polls, num_poll, poll_timeout);

		if (ret < 0) {
			return -errno;
		}

		poll_timeout -= (hrt_absolute_time() - poll_start) / 1000;

		// if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout
		exit_loop = true;

		if (ret == 0) {
			// Timeout control_data already null.

		} else {
			if (polls[0].revents & POLLIN) {
				vehicle_command_s vehicle_command;
				orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);

				// Process only if the command is for us or for anyone (component id 0).
				const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id);
				const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) ||
							     (vehicle_command.target_component == 0));

				if (!sysid_correct || !compid_correct) {
					exit_loop = false;
					continue;
				}

				for (int i = 0; i < 3; ++i) {
					_control_data.stabilize_axis[i] = _stabilize[i];
				}

				_control_data.gimbal_shutter_retract = false;

				if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {

					switch ((int)vehicle_command.param7) {
					case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
						_control_data.gimbal_shutter_retract = true;

					/* FALLTHROUGH */

					case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
						_control_data.type = ControlData::Type::Neutral;

						*control_data = &_control_data;
						break;

					case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
						_control_data.type = ControlData::Type::Angle;
						_control_data.type_data.angle.is_speed[0] = false;
						_control_data.type_data.angle.is_speed[1] = false;
						_control_data.type_data.angle.is_speed[2] = false;
						// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
						_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
						// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
						_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
						// both specs have yaw on channel 2
						_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;

						// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
						if (_control_data.type_data.angle.angles[2] > M_PI_F) {
							_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
						}

						*control_data = &_control_data;
						break;

					case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
						break;

					case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
						control_data_set_lon_lat((double)vehicle_command.param2, (double)vehicle_command.param1, vehicle_command.param3);

						*control_data = &_control_data;
						break;
					}

					_ack_vehicle_command(&vehicle_command);

				} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
					_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
					_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
					_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
					_control_data.type = ControlData::Type::Neutral; //always switch to neutral position

					*control_data = &_control_data;
					_ack_vehicle_command(&vehicle_command);

				} else {
					exit_loop = false;
				}
			}

		}
	}

	return 0;
}
コード例 #2
0
int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data)
{
	px4_pollfd_struct_t polls[2];
	polls[0].fd = 		_vehicle_command_sub;
	polls[0].events = 	POLLIN;
	int num_poll = 1;

	if (_manual_control && _allow_manual_control) {
		polls[num_poll].fd = 		_manual_control->_get_subscription_fd();
		polls[num_poll].events = 	POLLIN;
		++num_poll;
	}

	int ret = px4_poll(polls, num_poll, timeout_ms);

	if (ret < 0) {
		return -errno;
	}

	if (ret == 0) {
		*control_data = nullptr;

	} else {
		*control_data = &_control_data;

		if (polls[0].revents & POLLIN) {
			vehicle_command_s vehicle_command;
			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);

			for (int i = 0; i < 3; ++i) {
				_control_data.stabilize_axis[i] = _stabilize[i];
			}

			_control_data.gimbal_shutter_retract = false;

			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
				_allow_manual_control = false;

				switch ((int)vehicle_command.param7) {
				case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
					_control_data.gimbal_shutter_retract = true;

				//no break
				case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
					_control_data.type = ControlData::Type::Neutral;
					break;

				case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
					_control_data.type = ControlData::Type::Angle;
					_control_data.type_data.angle.is_speed[0] = false;
					_control_data.type_data.angle.is_speed[1] = false;
					_control_data.type_data.angle.is_speed[2] = false;
					_control_data.type_data.angle.angles[0] = vehicle_command.param1;
					_control_data.type_data.angle.angles[1] = vehicle_command.param2;
					_control_data.type_data.angle.angles[2] = vehicle_command.param3;

					break;

				case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
					_allow_manual_control = true;
					*control_data = nullptr;

					break;

				case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
					control_data_set_lon_lat(vehicle_command.param2, vehicle_command.param1, vehicle_command.param3);

					break;
				}

				_ack_vehicle_command(vehicle_command.command);

			} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
				_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
				_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
				_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
				_control_data.type = ControlData::Type::Neutral; //always switch to neutral position

				_ack_vehicle_command(vehicle_command.command);
			}

		} else if (num_poll > 1 && (polls[1].revents & POLLIN)) {
			_manual_control->_read_control_data_from_subscription(_control_data);
		}

	}

	return 0;
}