/** handle request for log data */ void DataFlash_Class::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *msg) { if (_log_sending_chan >= 0) { // some GCS (e.g. MAVProxy) attempt to stream request_data // messages when they're filling gaps in the downloaded logs. // This channel check avoids complaining to them, at the cost // of silently dropping any repeated attempts to start logging if (_log_sending_chan != link.get_chan()) { link.send_text(MAV_SEVERITY_INFO, "Log download in progress"); } return; } mavlink_log_request_data_t packet; mavlink_msg_log_request_data_decode(msg, &packet); _in_log_download = true; _log_listing = false; if (!_log_sending || _log_num_data != packet.id) { _log_sending = false; uint16_t num_logs = get_num_logs(); if (packet.id > num_logs || packet.id < 1) { return; } uint32_t time_utc, size; get_log_info(packet.id, size, time_utc); _log_num_data = packet.id; _log_data_size = size; uint16_t end; get_log_boundaries(packet.id, _log_data_page, end); } _log_data_offset = packet.ofs; if (_log_data_offset >= _log_data_size) { _log_data_remaining = 0; } else { _log_data_remaining = _log_data_size - _log_data_offset; } if (_log_data_remaining > packet.count) { _log_data_remaining = packet.count; } _log_sending = true; _log_sending_chan = link.get_chan(); handle_log_send(link); }
/** handle request for log data */ void DataFlash_Class::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *msg) { if (_log_sending_link != nullptr) { // some GCS (e.g. MAVProxy) attempt to stream request_data // messages when they're filling gaps in the downloaded logs. // This channel check avoids complaining to them, at the cost // of silently dropping any repeated attempts to start logging if (_log_sending_link->get_chan() != link.get_chan()) { link.send_text(MAV_SEVERITY_INFO, "Log download in progress"); } return; } mavlink_log_request_data_t packet; mavlink_msg_log_request_data_decode(msg, &packet); // consider opening or switching logs: if (transfer_activity != SENDING || _log_num_data != packet.id) { uint16_t num_logs = get_num_logs(); if (packet.id > num_logs || packet.id < 1) { // request for an invalid log; cancel any current download transfer_activity = IDLE; return; } uint32_t time_utc, size; get_log_info(packet.id, size, time_utc); _log_num_data = packet.id; _log_data_size = size; uint16_t end; get_log_boundaries(packet.id, _log_data_page, end); } _log_data_offset = packet.ofs; if (_log_data_offset >= _log_data_size) { _log_data_remaining = 0; } else { _log_data_remaining = _log_data_size - _log_data_offset; } if (_log_data_remaining > packet.count) { _log_data_remaining = packet.count; } transfer_activity = SENDING; _log_sending_link = &link; handle_log_send(); }
//------------------------------------------------------------------- void MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) { //-- If we haven't listed, we can't do much if (!_pLogHandlerHelper) { PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n"); return; } mavlink_log_request_data_t request; mavlink_msg_log_request_data_decode(msg, &request); //-- Does the requested log exist? if (request.id >= _pLogHandlerHelper->log_count) { PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id, _pLogHandlerHelper->log_count); return; } //-- If we were sending log entries, stop it _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; if (_pLogHandlerHelper->current_log_index != request.id) { //-- Init send log dataset _pLogHandlerHelper->current_log_filename[0] = 0; _pLogHandlerHelper->current_log_index = request.id; uint32_t time_utc = 0; _pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename); _pLogHandlerHelper->open_for_transmit(); } _pLogHandlerHelper->current_log_data_offset = request.ofs; if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) { _pLogHandlerHelper->current_log_data_remaining = 0; } else { _pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs; } if (_pLogHandlerHelper->current_log_data_remaining > request.count) { _pLogHandlerHelper->current_log_data_remaining = request.count; } //-- Enable streaming _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; }
/** handle request for log data */ void GCS_MAVLINK::handle_log_request_data(mavlink_message_t *msg, DataFlash_Class &dataflash) { mavlink_log_request_data_t packet; mavlink_msg_log_request_data_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) return; _log_listing = false; if (!_log_sending || _log_num_data != packet.id) { _log_sending = false; uint16_t num_logs = dataflash.get_num_logs(); int16_t last_log_num = dataflash.find_last_log(); if (packet.id > last_log_num || packet.id < last_log_num + 1 - num_logs) { return; } uint32_t time_utc, size; dataflash.get_log_info(packet.id, size, time_utc); _log_num_data = packet.id; _log_data_size = size; uint16_t end; dataflash.get_log_boundaries(packet.id, _log_data_page, end); } _log_data_offset = packet.ofs; if (_log_data_offset >= _log_data_size) { _log_data_remaining = 0; } else { _log_data_remaining = _log_data_size - _log_data_offset; } if (_log_data_remaining > packet.count) { _log_data_remaining = packet.count; } _log_sending = true; handle_log_send(dataflash); }