From 8e6cde25dce0028c2c2953de4284ddb18d74bd4a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 1 Aug 2019 10:08:23 +1000 Subject: [PATCH] AP_Logger: keep pointer to link rather than using its ->chan --- libraries/AP_Logger/AP_Logger.cpp | 2 +- libraries/AP_Logger/AP_Logger_Backend.h | 2 +- libraries/AP_Logger/AP_Logger_MAVLink.cpp | 25 +++++++++++++++-------- libraries/AP_Logger/AP_Logger_MAVLink.h | 7 ++++--- 4 files changed, 22 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 27a7a2c3b3..ffb434e5c3 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -727,7 +727,7 @@ void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, const mavlink_message_t &m { switch (msg.msgid) { case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: - FOR_EACH_BACKEND(remote_log_block_status_msg(link.get_chan(), msg)); + FOR_EACH_BACKEND(remote_log_block_status_msg(link, msg)); break; case MAVLINK_MSG_ID_LOG_REQUEST_LIST: FALLTHROUGH; diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index 1c492d0ccb..e9ae074ea8 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -72,7 +72,7 @@ public: #endif // for Logger_MAVlink - virtual void remote_log_block_status_msg(const mavlink_channel_t chan, + virtual void remote_log_block_status_msg(const GCS_MAVLINK &link, const mavlink_message_t &msg) { } // end for Logger_MAVlink diff --git a/libraries/AP_Logger/AP_Logger_MAVLink.cpp b/libraries/AP_Logger/AP_Logger_MAVLink.cpp index af34553f93..82832f2844 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLink.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLink.cpp @@ -220,7 +220,7 @@ void AP_Logger_MAVLink::stop_logging() } } -void AP_Logger_MAVLink::handle_ack(const mavlink_channel_t chan, +void AP_Logger_MAVLink::handle_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, uint32_t seqno) { @@ -245,7 +245,7 @@ void AP_Logger_MAVLink::handle_ack(const mavlink_channel_t chan, _sending_to_client = true; _target_system_id = msg.sysid; _target_component_id = msg.compid; - _chan = chan; + _link = &link; _next_seq_num = 0; start_new_log_reset_variables(); _last_response_time = AP_HAL::millis(); @@ -266,7 +266,7 @@ void AP_Logger_MAVLink::handle_ack(const mavlink_channel_t chan, } } -void AP_Logger_MAVLink::remote_log_block_status_msg(const mavlink_channel_t chan, +void AP_Logger_MAVLink::remote_log_block_status_msg(const GCS_MAVLINK &link, const mavlink_message_t& msg) { mavlink_remote_log_block_status_t packet; @@ -277,7 +277,7 @@ void AP_Logger_MAVLink::remote_log_block_status_msg(const mavlink_channel_t chan if(packet.status == 0){ handle_retry(packet.seqno); } else{ - handle_ack(chan, msg, packet.seqno); + handle_ack(link, msg, packet.seqno); } semaphore.give(); } @@ -537,17 +537,24 @@ void AP_Logger_MAVLink::periodic_1Hz() //TODO: handle full txspace properly bool AP_Logger_MAVLink::send_log_block(struct dm_block &block) { - mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0); if (!_initialised) { return false; } - if (!HAVE_PAYLOAD_SPACE(chan, REMOTE_LOG_DATA_BLOCK)) { + if (_link == nullptr) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return false; } - if (comm_get_txspace(chan) < 500) { + // don't completely fill buffers - and also ensure there's enough + // room to send at least one packet: + const uint16_t min_payload_space = 500; + static_assert(MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN <= min_payload_space, + "minimum allocated space is less than payload length"); + if (_link->txspace() < min_payload_space) { return false; } + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // deliberately fail 10% of the time in SITL: if (rand() < 0.1) { return false; } @@ -561,7 +568,7 @@ bool AP_Logger_MAVLink::send_log_block(struct dm_block &block) hal.util->perf_begin(_perf_packing); mavlink_message_t msg; - mavlink_status_t *chan_status = mavlink_get_channel_status(chan); + mavlink_status_t *chan_status = mavlink_get_channel_status(_link->get_chan()); uint8_t saved_seq = chan_status->current_tx_seq; chan_status->current_tx_seq = mavlink_seq++; // Debug("Sending block (%d)", block.seqno); @@ -587,7 +594,7 @@ bool AP_Logger_MAVLink::send_log_block(struct dm_block &block) // problem and stop attempting to log _last_send_time = AP_HAL::millis(); - _mavlink_resend_uart(chan, &msg); + _mavlink_resend_uart(_link->get_chan(), &msg); return true; } diff --git a/libraries/AP_Logger/AP_Logger_MAVLink.h b/libraries/AP_Logger/AP_Logger_MAVLink.h index 590156a4ba..5169e0198e 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLink.h +++ b/libraries/AP_Logger/AP_Logger_MAVLink.h @@ -59,7 +59,7 @@ public: int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override { return 0; } uint16_t get_num_logs(void) override { return 0; } - void remote_log_block_status_msg(const mavlink_channel_t chan, const mavlink_message_t& msg) override; + void remote_log_block_status_msg(const GCS_MAVLINK &link, const mavlink_message_t& msg) override; void vehicle_was_disarmed() override {} protected: @@ -76,7 +76,7 @@ private: struct dm_block *next; }; bool send_log_block(struct dm_block &block); - void handle_ack(const mavlink_channel_t chan, const mavlink_message_t &msg, uint32_t seqno); + void handle_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, uint32_t seqno); void handle_retry(uint32_t block_num); void do_resends(uint32_t now); void free_all_blocks(); @@ -123,7 +123,8 @@ private: bool logging_enabled() const override { return true; } bool logging_failed() const override; - mavlink_channel_t _chan; + const GCS_MAVLINK *_link; + uint8_t _target_system_id; uint8_t _target_component_id;