|
| 1 | +#include <signal.h> |
| 2 | +#include <stdio.h> // for snprintf |
| 3 | +#include <fcntl.h> |
| 4 | +#include <errno.h> |
| 5 | +#include <stdlib.h> |
| 6 | +#include <unistd.h> |
| 7 | + |
| 8 | +#include "la-log.h" |
| 9 | +#include "util.h" |
| 10 | +#include "mavlink_c_library/ardupilotmega/mavlink.h" |
| 11 | + |
| 12 | +#include "log_download.h" |
| 13 | + |
| 14 | +bool Log_Download::configure(INIReader *config) |
| 15 | +{ |
| 16 | + if (!MAVLink_Message_Handler::configure(config)) { |
| 17 | + return false; |
| 18 | + } |
| 19 | + return true; |
| 20 | +} |
| 21 | + |
| 22 | +void Log_Download::handle_message(uint64_t timestamp, mavlink_message_t &msg) |
| 23 | +{ |
| 24 | + most_recent_sender_system_id = msg.sysid; |
| 25 | + most_recent_sender_component_id = msg.compid; |
| 26 | + MAVLink_Message_Handler::handle_message(timestamp, msg); |
| 27 | +} |
| 28 | + |
| 29 | +void Log_Download::handle_decoded_message(uint64_t T UNUSED, mavlink_log_entry_t &msg) |
| 30 | +{ |
| 31 | + if (msg.id != id_to_fetch) { |
| 32 | + return; |
| 33 | + } |
| 34 | + |
| 35 | + log_info = new LogEntry(); |
| 36 | + log_info->id = msg.id; |
| 37 | + log_info->size = msg.size; |
| 38 | + log_info->time_utc = msg.time_utc; |
| 39 | +} |
| 40 | + |
| 41 | +void Log_Download::write_data(mavlink_log_data_t &msg) |
| 42 | +{ |
| 43 | + if (output_fh == -1) { |
| 44 | + ::fprintf(stderr, "opening output_fh\n"); |
| 45 | + char filename[120]; |
| 46 | + snprintf(filename, sizeof(filename), "log%u.bin", id_to_fetch); |
| 47 | + output_fh = open(filename, O_WRONLY|O_TRUNC|O_CREAT, 0644); |
| 48 | + if (output_fh == -1) { |
| 49 | + ::fprintf(stderr, "Failed to open (%s): %s", filename, strerror(errno)); |
| 50 | + abort(); |
| 51 | + } |
| 52 | + // expand the file: |
| 53 | + if (lseek(output_fh, log_info->size, SEEK_SET) == -1) { |
| 54 | + ::fprintf(stderr, "Failed to seek\n"); |
| 55 | + abort(); |
| 56 | + } |
| 57 | + } |
| 58 | + last_data_receieved = clock_gettime_us(CLOCK_MONOTONIC); |
| 59 | + if (lseek(output_fh, msg.ofs, SEEK_SET) == -1) { |
| 60 | + ::fprintf(stderr, "Failed to seek\n"); |
| 61 | + abort(); |
| 62 | + } |
| 63 | + // ::fprintf(stderr, "Writing %u bytes at offset %u\n", msg.count, msg.ofs); |
| 64 | + if (write(output_fh, msg.data, msg.count) != msg.count) { |
| 65 | + ::fprintf(stderr, "Short write\n"); |
| 66 | + abort(); |
| 67 | + } |
| 68 | +} |
| 69 | + |
| 70 | +void Log_Download::handle_decoded_message(uint64_t T UNUSED, mavlink_log_data_t &msg) |
| 71 | +{ |
| 72 | + if (msg.id != id_to_fetch) { |
| 73 | + return; |
| 74 | + } |
| 75 | + // la_log(LOG_ERR, "data: ofs=%u count=%u", msg.ofs, msg.count); |
| 76 | +// ::fprintf(stderr, "data: ofs=%u count=%u\n", msg.ofs, msg.count); |
| 77 | + |
| 78 | + for (auto it=gaps.begin(); it != gaps.end(); it++) { |
| 79 | + Gap *gap = *it; |
| 80 | + if (msg.ofs >= gap->start && |
| 81 | + msg.ofs < gap->start+gap->len) { |
| 82 | + write_data(msg); |
| 83 | + } |
| 84 | + // ::fprintf(stderr, "msg.ofs=%u gap->start=%u\n", msg.ofs, gap->start); |
| 85 | + if (msg.ofs == gap->start) { |
| 86 | + // msg is at start of gap |
| 87 | + if (msg.count >= gap->len) { |
| 88 | + // this gap is all done! |
| 89 | + gap->len = 0; |
| 90 | + } else { |
| 91 | + gap->start += msg.count; |
| 92 | + if (msg.count > gap->len) { |
| 93 | + gap->len = 0; |
| 94 | + } else { |
| 95 | + gap->len -= msg.count; |
| 96 | + } |
| 97 | + } |
| 98 | + } else if (msg.ofs + msg.count >= gap->start+gap->len) { |
| 99 | + // msg is at end of gap |
| 100 | + if (msg.ofs + msg.count == gap->start+gap->len) { |
| 101 | + // right at end of buffer |
| 102 | + gap->len -= (gap->start+gap->len)-msg.ofs; |
| 103 | + } |
| 104 | + } else { |
| 105 | + ::fprintf(stderr, "Split required\n"); |
| 106 | + abort(); |
| 107 | + } |
| 108 | + } |
| 109 | +} |
| 110 | + |
| 111 | + |
| 112 | +void Log_Download::send_log_request_list(uint16_t id) |
| 113 | +{ |
| 114 | + la_log(LOG_INFO, "mh-ld: sending log_request_list (%u) to %u/%u", id, target_system_id, target_component_id); |
| 115 | + |
| 116 | + mavlink_message_t msg; |
| 117 | + mavlink_msg_log_request_list_pack(system_id, |
| 118 | + component_id, |
| 119 | + &msg, |
| 120 | + target_system_id, |
| 121 | + target_component_id, |
| 122 | + id, |
| 123 | + id); |
| 124 | + |
| 125 | + _mavlink_writer->send_message(msg); |
| 126 | +} |
| 127 | + |
| 128 | +uint64_t Log_Download::time_in_state() const |
| 129 | +{ |
| 130 | + return (clock_gettime_us(CLOCK_MONOTONIC) - state_start_time); |
| 131 | +} |
| 132 | + |
| 133 | +void Log_Download::send_data_request(Gap *gap) |
| 134 | +{ |
| 135 | + mavlink_message_t msg; |
| 136 | + mavlink_msg_log_request_data_pack(system_id, |
| 137 | + component_id, |
| 138 | + &msg, |
| 139 | + target_system_id, |
| 140 | + target_component_id, |
| 141 | + id_to_fetch, |
| 142 | + gap->start, |
| 143 | + gap->len); |
| 144 | + |
| 145 | + _mavlink_writer->send_message(msg); |
| 146 | +} |
| 147 | + |
| 148 | +void Log_Download::sort_gaps() |
| 149 | +{ |
| 150 | + if (gaps.front()->len == 0) { |
| 151 | + gaps.erase(gaps.begin()); |
| 152 | + } |
| 153 | +} |
| 154 | + |
| 155 | + |
| 156 | +void Log_Download::run_statemachine() |
| 157 | +{ |
| 158 | + switch(state) { |
| 159 | + case WAITING_FOR_SYSTEM: |
| 160 | + if (target_system_id == 0) { |
| 161 | + if (most_recent_sender_component_id == 0) { |
| 162 | + return; |
| 163 | + } |
| 164 | + } |
| 165 | + target_system_id = most_recent_sender_system_id; |
| 166 | + target_component_id = most_recent_sender_component_id; |
| 167 | + state = NEXT_LOG; |
| 168 | + // fall-through |
| 169 | + case NEXT_LOG: { |
| 170 | + if (logs_to_download.size() == 0) { |
| 171 | + state = DONE; |
| 172 | + return; |
| 173 | + } |
| 174 | + id_to_fetch = logs_to_download.back(); |
| 175 | + logs_to_download.pop_back(); |
| 176 | + ::fprintf(stderr, "Downloading log %u\n", id_to_fetch); |
| 177 | + delete log_info; |
| 178 | + log_info = nullptr; |
| 179 | + state = SEND_GET_LOG_INFO; |
| 180 | + } |
| 181 | + // fall-through |
| 182 | + case SEND_GET_LOG_INFO: |
| 183 | + send_log_request_list(id_to_fetch); |
| 184 | + state = SENT_GET_LOG_INFO; |
| 185 | + return; |
| 186 | + case SENT_GET_LOG_INFO: { |
| 187 | + if (log_info == nullptr) { |
| 188 | + if (time_in_state() > 1000000) { |
| 189 | + state = SEND_GET_LOG_INFO; |
| 190 | + } |
| 191 | + return; |
| 192 | + } |
| 193 | + Gap *gap = new Gap(); |
| 194 | + gap->start = 0; |
| 195 | + gap->len = log_info->size; |
| 196 | + gaps.push_back(gap); |
| 197 | + state = FILL_GAPS; |
| 198 | + download_start_time = clock_gettime_us(CLOCK_MONOTONIC); |
| 199 | + } |
| 200 | + // fall-through |
| 201 | + case FILL_GAPS: { |
| 202 | + bool new_gap = false; |
| 203 | + if (gaps.front()->len == 0) { |
| 204 | + sort_gaps(); |
| 205 | + new_gap = true; |
| 206 | + } |
| 207 | + if (gaps.size() == 0) { |
| 208 | + state = DONE; |
| 209 | + close(output_fh); |
| 210 | + output_fh = -1; |
| 211 | + return; |
| 212 | + } |
| 213 | + if (new_gap || |
| 214 | + clock_gettime_us(CLOCK_MONOTONIC) - last_data_receieved > 100000) { |
| 215 | + send_data_request(gaps.front()); |
| 216 | + } |
| 217 | + return; |
| 218 | + } |
| 219 | + case DONE: { |
| 220 | + const uint64_t delta_time = clock_gettime_us(CLOCK_MONOTONIC) - download_start_time; |
| 221 | + const float rate = (log_info->size/1024.0)/(delta_time/1000000.0); |
| 222 | + ::fprintf(stderr, "Download done (%u bytes) (%lus) (%fkB/s)\n", |
| 223 | + log_info->size, |
| 224 | + delta_time/1000000, |
| 225 | + rate); |
| 226 | + exit(0); |
| 227 | + } |
| 228 | + } |
| 229 | +} |
| 230 | + |
| 231 | +void Log_Download::idle_10Hz() |
| 232 | +{ |
| 233 | + run_statemachine(); |
| 234 | + if (state != last_state) { |
| 235 | + last_state = state; |
| 236 | + state_start_time = clock_gettime_us(CLOCK_MONOTONIC); |
| 237 | + } |
| 238 | +} |
0 commit comments