From 99c9e09624427e4ab0dd1a6b20e393919bb3d8c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 19 Jan 2018 10:50:32 +0100 Subject: [PATCH] vmount mavlink input: continue to poll for vehicle_command until reaching the timeout or receiving a command that we need to handle. This makes sure that we don't drop commands, when the output mode is set to mavlink (in that case the input topic is rate-limited, and each input change results in a vehicle_command publication, which results in queueing up orb items if we get vehicle_commands from other sources too). --- src/drivers/vmount/input_mavlink.cpp | 177 +++++++++++++++------------ 1 file changed, 97 insertions(+), 80 deletions(-) diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index a8dc521e67..ee6c1ce863 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -221,92 +221,109 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con polls[0].fd = _vehicle_command_sub; polls[0].events = POLLIN; - int ret = px4_poll(polls, num_poll, timeout_ms); + int poll_timeout = (int)timeout_ms; - if (ret < 0) { - return -errno; - } + bool exit_loop = false; - if (ret == 0) { - // Timeout control_data already null. + while (!exit_loop && poll_timeout >= 0) { + hrt_abstime poll_start = hrt_absolute_time(); - } else { - if (polls[0].revents & POLLIN) { - vehicle_command_s vehicle_command; - orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + int ret = px4_poll(polls, num_poll, poll_timeout); - // 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) { - return 0; - } - - 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); - } + 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;