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