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;
}
