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