diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5f2d5b4a0f..07d9be73b2 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -296,8 +296,6 @@ 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; @@ -364,8 +362,6 @@ 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)); @@ -430,8 +426,6 @@ 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; @@ -524,29 +518,8 @@ MavlinkMissionManager::send(const hrt_abstime now) } /* check for timed-out operations */ - 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_SENDLIST && (_time_last_sent > 0) - && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) { - - if (_transfer_seq == 0) { - /* try to send items count again after timeout */ - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count, _mission_type); - - } else { - /* try to send item again after timeout */ - PX4_DEBUG("WPM: item re-send timeout"); - - send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); - } - - } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) - && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) { - + if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) + && hrt_elapsed_time(&_time_last_recv) > _operation_timeout_us) { _mavlink->send_statustext_critical("Operation timeout"); PX4_DEBUG("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); @@ -558,7 +531,6 @@ 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 8fe20fb0c0..fd85e8c1ec 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -67,8 +67,6 @@ 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 = 500000; ///< Protocol retry timeout in us class Mavlink; @@ -94,12 +92,13 @@ 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