int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { // already_active is unused, we don't care what happened previously. // Default to no change, set if we receive anything. *control_data = nullptr; const int num_poll = 2; px4_pollfd_struct_t polls[num_poll]; polls[0].fd = _vehicle_roi_sub; polls[0].events = POLLIN; polls[1].fd = _position_setpoint_triplet_sub; polls[1].events = POLLIN; int ret = px4_poll(polls, num_poll, timeout_ms); if (ret < 0) { return -errno; } if (ret == 0) { // Timeout, _control_data is already null } else { if (polls[0].revents & POLLIN) { vehicle_roi_s vehicle_roi; orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); _control_data.gimbal_shutter_retract = false; if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { _control_data.type = ControlData::Type::Neutral; *control_data = &_control_data; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { _control_data.type = ControlData::Type::LonLat; _read_control_data_from_position_setpoint_sub(); _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; *control_data = &_control_data; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); *control_data = &_control_data; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { //TODO is this even suported? } _cur_roi_mode = vehicle_roi.mode; //set all other control data fields to defaults for (int i = 0; i < 3; ++i) { _control_data.stabilize_axis[i] = false; } } // check whether the position setpoint got updated if (polls[1].revents & POLLIN) { if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { _read_control_data_from_position_setpoint_sub(); *control_data = &_control_data; } else { // must do an orb_copy() in *every* case position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); } } } return 0; }
int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data) { px4_pollfd_struct_t polls[3]; polls[0].fd = _vehicle_roi_sub; polls[0].events = POLLIN; polls[1].fd = _position_setpoint_triplet_sub; polls[1].events = POLLIN; int num_poll = 2; 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 { if (polls[0].revents & POLLIN) { vehicle_roi_s vehicle_roi; orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) { _allow_manual_control = true; if (!_manual_control) { _control_data.type = ControlData::Type::Neutral; } } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) { _allow_manual_control = false; _read_control_data_from_position_setpoint_sub(); _control_data.type_data.lonlat.roll_angle = 0.f; _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) { _allow_manual_control = false; //TODO how to do this? } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) { _allow_manual_control = false; control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) { _allow_manual_control = false; //TODO is this even suported? } _cur_roi_mode = vehicle_roi.mode; //set all other control data fields to defaults for (int i = 0; i < 3; ++i) { _control_data.stabilize_axis[i] = false; } _control_data.gimbal_shutter_retract = false; } else if (num_poll > 2 && (polls[2].revents & POLLIN)) { _manual_control->_read_control_data_from_subscription(_control_data); } // check whether the position setpoint got updated if ((polls[1].revents & POLLIN) && _cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) { _read_control_data_from_position_setpoint_sub(); } *control_data = &_control_data; } return 0; }