Line data Source code
1 : /*
2 : MAVLink logfile transfer functions
3 : */
4 :
5 : /*
6 : This program is free software: you can redistribute it and/or modify
7 : it under the terms of the GNU General Public License as published by
8 : the Free Software Foundation, either version 3 of the License, or
9 : (at your option) any later version.
10 :
11 : This program is distributed in the hope that it will be useful,
12 : but WITHOUT ANY WARRANTY; without even the implied warranty of
13 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 : GNU General Public License for more details.
15 :
16 : You should have received a copy of the GNU General Public License
17 : along with this program. If not, see <http://www.gnu.org/licenses/>.
18 : */
19 :
20 : #include <GCS_MAVLink/GCS_config.h>
21 : #include <AP_Logger/AP_Logger_config.h>
22 :
23 : #if HAL_LOGGING_ENABLED && HAL_GCS_ENABLED
24 :
25 : #include <AP_HAL/AP_HAL.h>
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <GCS_MAVLink/GCS.h> // for LOG_ENTRY
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : /**
32 : handle all types of log download requests from the GCS
33 : */
34 18491 : void AP_Logger::handle_log_message(GCS_MAVLINK &link, const mavlink_message_t &msg)
35 : {
36 18491 : if (!WritesEnabled()) {
37 : // this is currently used as a proxy for "in_mavlink_delay"
38 0 : return;
39 : }
40 18491 : if (vehicle_is_armed()) {
41 0 : if (!_warned_log_disarm) {
42 0 : link.send_text(MAV_SEVERITY_ERROR, "Disarm for log download");
43 0 : _warned_log_disarm = true;
44 : }
45 0 : return;
46 : }
47 18491 : _warned_log_disarm = false;
48 18491 : _last_mavlink_log_transfer_message_handled_ms = AP_HAL::millis();
49 18491 : switch (msg.msgid) {
50 38 : case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
51 38 : handle_log_request_list(link, msg);
52 38 : break;
53 18432 : case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
54 18432 : handle_log_request_data(link, msg);
55 18432 : break;
56 8 : case MAVLINK_MSG_ID_LOG_ERASE:
57 8 : handle_log_request_erase(link, msg);
58 8 : break;
59 13 : case MAVLINK_MSG_ID_LOG_REQUEST_END:
60 13 : handle_log_request_end(link, msg);
61 13 : break;
62 : }
63 : }
64 :
65 : /**
66 : handle all types of log download requests from the GCS
67 : */
68 38 : void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, const mavlink_message_t &msg)
69 : {
70 38 : WITH_SEMAPHORE(_log_send_sem);
71 :
72 38 : if (_log_sending_link != nullptr) {
73 0 : link.send_text(MAV_SEVERITY_INFO, "Log download in progress");
74 0 : return;
75 : }
76 :
77 : mavlink_log_request_list_t packet;
78 38 : mavlink_msg_log_request_list_decode(&msg, &packet);
79 :
80 38 : _log_num_logs = get_num_logs();
81 :
82 38 : if (_log_num_logs == 0) {
83 1 : _log_next_list_entry = 0;
84 1 : _log_last_list_entry = 0;
85 : } else {
86 37 : _log_next_list_entry = packet.start;
87 37 : _log_last_list_entry = packet.end;
88 :
89 37 : if (_log_last_list_entry > _log_num_logs) {
90 36 : _log_last_list_entry = _log_num_logs;
91 : }
92 37 : if (_log_next_list_entry < 1) {
93 36 : _log_next_list_entry = 1;
94 : }
95 : }
96 :
97 38 : transfer_activity = TransferActivity::LISTING;
98 38 : _log_sending_link = &link;
99 :
100 38 : handle_log_send_listing();
101 38 : }
102 :
103 :
104 : /**
105 : handle request for log data
106 : */
107 18432 : void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, const mavlink_message_t &msg)
108 : {
109 18432 : WITH_SEMAPHORE(_log_send_sem);
110 :
111 18432 : if (_log_sending_link != nullptr) {
112 : // some GCS (e.g. MAVProxy) attempt to stream request_data
113 : // messages when they're filling gaps in the downloaded logs.
114 : // This channel check avoids complaining to them, at the cost
115 : // of silently dropping any repeated attempts to start logging
116 215 : if (_log_sending_link->get_chan() != link.get_chan()) {
117 0 : link.send_text(MAV_SEVERITY_INFO, "Log download in progress");
118 : }
119 215 : return;
120 : }
121 :
122 : mavlink_log_request_data_t packet;
123 18217 : mavlink_msg_log_request_data_decode(&msg, &packet);
124 :
125 : // consider opening or switching logs:
126 18217 : if (transfer_activity != TransferActivity::SENDING || _log_num_data != packet.id) {
127 :
128 18217 : uint16_t num_logs = get_num_logs();
129 18217 : if (packet.id > num_logs || packet.id < 1) {
130 : // request for an invalid log; cancel any current download
131 1 : end_log_transfer();
132 1 : return;
133 : }
134 :
135 : uint32_t time_utc, size;
136 18216 : get_log_info(packet.id, size, time_utc);
137 18216 : _log_num_data = packet.id;
138 18216 : _log_data_size = size;
139 :
140 : uint32_t end;
141 18216 : get_log_boundaries(packet.id, _log_data_page, end);
142 : }
143 :
144 18216 : _log_data_offset = packet.ofs;
145 18216 : if (_log_data_offset >= _log_data_size) {
146 3 : _log_data_remaining = 0;
147 : } else {
148 18213 : _log_data_remaining = _log_data_size - _log_data_offset;
149 : }
150 18216 : if (_log_data_remaining > packet.count) {
151 18194 : _log_data_remaining = packet.count;
152 : }
153 :
154 18216 : transfer_activity = TransferActivity::SENDING;
155 18216 : _log_sending_link = &link;
156 :
157 18216 : handle_log_send();
158 18432 : }
159 :
160 : /**
161 : handle request to erase log data
162 : */
163 8 : void AP_Logger::handle_log_request_erase(GCS_MAVLINK &link, const mavlink_message_t &msg)
164 : {
165 : // mavlink_log_erase_t packet;
166 : // mavlink_msg_log_erase_decode(&msg, &packet);
167 :
168 8 : EraseAll();
169 8 : }
170 :
171 : /**
172 : handle request to stop transfer and resume normal logging
173 : */
174 13 : void AP_Logger::handle_log_request_end(GCS_MAVLINK &link, const mavlink_message_t &msg)
175 : {
176 13 : WITH_SEMAPHORE(_log_send_sem);
177 : // mavlink_log_request_end_t packet;
178 : // mavlink_msg_log_request_end_decode(&msg, &packet);
179 13 : end_log_transfer();
180 13 : }
181 :
182 18230 : void AP_Logger::end_log_transfer()
183 : {
184 18230 : transfer_activity = TransferActivity::IDLE;
185 18230 : _log_sending_link = nullptr;
186 18230 : backends[0]->end_log_transfer();
187 18230 : }
188 :
189 : /**
190 : trigger sending of log messages if there are some pending
191 : */
192 167800036 : void AP_Logger::handle_log_send()
193 : {
194 167800036 : WITH_SEMAPHORE(_log_send_sem);
195 :
196 167800036 : if (_log_sending_link == nullptr) {
197 167756850 : return;
198 : }
199 43186 : if (hal.util->get_soft_armed()) {
200 : // might be flying
201 0 : return;
202 : }
203 43186 : switch (transfer_activity) {
204 0 : case TransferActivity::IDLE:
205 0 : break;
206 3429 : case TransferActivity::LISTING:
207 3429 : handle_log_send_listing();
208 3429 : break;
209 39757 : case TransferActivity::SENDING:
210 39757 : handle_log_sending();
211 39757 : break;
212 : }
213 167800036 : }
214 :
215 39757 : void AP_Logger::handle_log_sending()
216 : {
217 39757 : WITH_SEMAPHORE(_log_send_sem);
218 :
219 : // we can spend a *long* time sending data on the main thread. We
220 : // can spen more than 1.9 seconds doing this depending on what the
221 : // SD card gets up to, moving us into the monitor thread's "you
222 : // are going to watchdog" code. Until we move all these
223 : // operations into a thread we will ask the timer thread to pat
224 : // the watchdog and have the monitor thread chill out by being in
225 : // an expected delay:
226 39757 : EXPECT_DELAY_MS(5000);
227 :
228 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
229 : // assume USB speeds in SITL for the purposes of log download
230 39757 : const uint8_t num_sends = 40;
231 : #else
232 0 : uint8_t num_sends = 1;
233 0 : if (_log_sending_link->is_high_bandwidth() && hal.gpio->usb_connected()) {
234 : // when on USB we can send a lot more data
235 0 : num_sends = 250;
236 0 : } else if (_log_sending_link->have_flow_control()) {
237 : #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
238 0 : num_sends = 80;
239 : #else
240 : num_sends = 10;
241 : #endif
242 : }
243 : #endif
244 :
245 405229 : for (uint8_t i=0; i<num_sends; i++) {
246 398534 : if (transfer_activity != TransferActivity::SENDING) {
247 : // may have completed sending data
248 18216 : break;
249 : }
250 380318 : if (!handle_log_send_data()) {
251 14846 : break;
252 : }
253 : }
254 39757 : }
255 :
256 : /**
257 : trigger sending of log messages if there are some pending
258 : */
259 3467 : void AP_Logger::handle_log_send_listing()
260 : {
261 3467 : WITH_SEMAPHORE(_log_send_sem);
262 :
263 3467 : if (!HAVE_PAYLOAD_SPACE(_log_sending_link->get_chan(), LOG_ENTRY)) {
264 : // no space
265 0 : return;
266 : }
267 3467 : if (AP_HAL::millis() - _log_sending_link->get_last_heartbeat_time() > 3000) {
268 : // give a heartbeat a chance
269 0 : return;
270 : }
271 :
272 : uint32_t size, time_utc;
273 3467 : if (_log_next_list_entry == 0) {
274 1 : size = 0;
275 1 : time_utc = 0;
276 : } else {
277 3466 : get_log_info(_log_next_list_entry, size, time_utc);
278 : }
279 3467 : mavlink_msg_log_entry_send(_log_sending_link->get_chan(),
280 3467 : _log_next_list_entry,
281 3467 : _log_num_logs,
282 3467 : _log_last_list_entry,
283 : time_utc,
284 : size);
285 3467 : if (_log_next_list_entry == _log_last_list_entry) {
286 38 : transfer_activity = TransferActivity::IDLE;
287 38 : _log_sending_link = nullptr;
288 : } else {
289 3429 : _log_next_list_entry++;
290 : }
291 3467 : }
292 :
293 : /**
294 : trigger sending of log data if there are some pending
295 : */
296 380318 : bool AP_Logger::handle_log_send_data()
297 : {
298 380318 : WITH_SEMAPHORE(_log_send_sem);
299 :
300 380318 : if (!HAVE_PAYLOAD_SPACE(_log_sending_link->get_chan(), LOG_DATA)) {
301 : // no space
302 14846 : return false;
303 : }
304 365472 : if (AP_HAL::millis() - _log_sending_link->get_last_heartbeat_time() > 3000) {
305 : // give a heartbeat a chance
306 0 : return false;
307 : }
308 :
309 365472 : int16_t nbytes = 0;
310 365472 : uint32_t len = _log_data_remaining;
311 : mavlink_log_data_t packet;
312 :
313 365472 : if (len > MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
314 347256 : len = MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;
315 : }
316 :
317 365472 : nbytes = get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
318 :
319 365472 : if (nbytes < 0) {
320 : // report as EOF on error
321 0 : nbytes = 0;
322 : }
323 365472 : if (nbytes < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
324 1618 : memset(&packet.data[nbytes], 0, MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN-nbytes);
325 : }
326 :
327 365472 : packet.ofs = _log_data_offset;
328 365472 : packet.id = _log_num_data;
329 365472 : packet.count = nbytes;
330 365472 : _mav_finalize_message_chan_send(_log_sending_link->get_chan(),
331 : MAVLINK_MSG_ID_LOG_DATA,
332 : (const char *)&packet,
333 : MAVLINK_MSG_ID_LOG_DATA_MIN_LEN,
334 : MAVLINK_MSG_ID_LOG_DATA_LEN,
335 : MAVLINK_MSG_ID_LOG_DATA_CRC);
336 :
337 365472 : _log_data_offset += nbytes;
338 365472 : _log_data_remaining -= nbytes;
339 365472 : if (nbytes < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN || _log_data_remaining == 0) {
340 18216 : end_log_transfer();
341 : }
342 365472 : return true;
343 380318 : }
344 :
345 : #endif // HAL_LOGGING_ENABLED && HAL_GCS_ENABLED
|
