diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 906a806810..4380ff9f5a 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -296,6 +296,8 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) void MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type) { + _time_last_sent = hrt_absolute_time(); + mavlink_mission_count_t wpc; wpc.target_system = sysid; @@ -362,6 +364,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } if (read_success) { + _time_last_sent = hrt_absolute_time(); + if (_int_mode) { mavlink_mission_item_int_t wp; format_mavlink_mission_item(&mission_item, reinterpret_cast(&wp)); @@ -426,6 +430,8 @@ void MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < current_max_item_count()) { + _time_last_sent = hrt_absolute_time(); + if (_int_mode) { mavlink_mission_request_int_t wpr; wpr.target_system = sysid; @@ -518,8 +524,15 @@ MavlinkMissionManager::send(const hrt_abstime now) } /* check for timed-out operations */ - if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) - && hrt_elapsed_time(&_time_last_recv) > _operation_timeout_us) { + if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0) + && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) { + + // try to request item again after timeout + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) + && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) { + _mavlink->send_statustext_critical("Operation timeout"); PX4_DEBUG("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); @@ -531,6 +544,7 @@ MavlinkMissionManager::send(const hrt_abstime now) } else if (_state == MAVLINK_WPM_STATE_IDLE) { // reset flags + _time_last_sent = 0; _time_last_recv = 0; } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index fd85e8c1ec..8a829a4d1f 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -67,6 +67,8 @@ enum MAVLINK_WPM_CODES { MAVLINK_WPM_CODE_ENUM_END }; +static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT = 5000000; ///< Protocol action timeout in us +static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 250000; ///< Protocol retry timeout in us class Mavlink; @@ -92,13 +94,12 @@ private: enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) uint64_t _time_last_recv{0}; + uint64_t _time_last_sent{0}; uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint bool _int_mode{false}; ///< Use accurate int32 instead of float - static constexpr uint32_t _operation_timeout_us = 5000000; - unsigned _filesystem_errcount{0}; ///< File system error count static dm_item_t _dataman_id; ///< Global Dataman storage ID for active mission