diff --git a/.github/workflows/win_build.yml b/.github/workflows/win_build.yml index 6887168..a29585e 100644 --- a/.github/workflows/win_build.yml +++ b/.github/workflows/win_build.yml @@ -4,8 +4,8 @@ on: [push, pull_request, workflow_dispatch] jobs: build: - runs-on: windows-latest - + runs-on: windows-2022 + steps: - name: Checkout plugin uses: actions/checkout@v4 @@ -29,36 +29,42 @@ jobs: shell: pwsh run: | conan profile detect - conan install . -of build --build=missing -pr:b=default -s build_type=Release + conan install . -of build --build=missing -pr:b=default ` + -s build_type=Release -s compiler.cppstd=20 ` + -c tools.build:jobs=$([Environment]::ProcessorCount) ` + -c tools.cmake.cmaketoolchain:generator=Ninja - name: Cache PlotJuggler install + id: cache-plotjuggler uses: actions/cache@v4 with: path: pj_install/ key: ${{ runner.os }}-plotjuggler-${{ hashFiles('plotjuggler/**') }} - name: Clone PlotJuggler + if: steps.cache-plotjuggler.outputs.cache-hit != 'true' run: git clone --depth 1 --branch 3.9.2 https://github.com/facontidavide/PlotJuggler.git plotjuggler - name: Build & install PlotJuggler if: steps.cache-plotjuggler.outputs.cache-hit != 'true' + shell: pwsh run: | mkdir pj_build cmake -S plotjuggler -B pj_build -DCMAKE_INSTALL_PREFIX=${{ github.workspace }}\pj_install -DCMAKE_BUILD_TYPE=RelWithDebInfo - cmake --build pj_build --config RelWithDebInfo --target install + cmake --build pj_build --config RelWithDebInfo --target install --parallel $([Environment]::ProcessorCount) - name: Build your plugin - run: | + shell: pwsh + run: | Remove-Item -Recurse -Force build -ErrorAction SilentlyContinue mkdir build cd build cmake .. -Dplotjuggler_DIR="${{ github.workspace }}\pj_install\lib\cmake\plotjuggler" -DADD_UNITS=OFF - cmake --build . --config Release + cmake --build . --config Release --parallel $([Environment]::ProcessorCount) cmake --install . --prefix "${{ github.workspace }}\plugin_install" - - name: Upload plugin artifact uses: actions/upload-artifact@v4 with: name: plugin-build - path: build/ + path: build/ \ No newline at end of file diff --git a/DataLoadAPBin/dataload_apbin.cpp b/DataLoadAPBin/dataload_apbin.cpp index c8a7596..9962b0e 100644 --- a/DataLoadAPBin/dataload_apbin.cpp +++ b/DataLoadAPBin/dataload_apbin.cpp @@ -23,18 +23,15 @@ #include #include - // Debugging //#define DEBUG_RUNTIME //#define DEBUG_MESSAGES //#define DEBUG_MULTIPLIERS //#define DEBUG_UNITS - // Config //#define LABEL_WITH_UNIT - bool is_nearly(double val, int val2) { const double epsilon = 1e-10; @@ -48,7 +45,6 @@ bool is_nearly(double val, int val2) } } - DataLoadAPBIN::DataLoadAPBIN() { extensions.push_back("BIN"); // TODO : this doesn't work for now as tolower() is hardcoded. @@ -159,7 +155,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da // get message-id from header const uint8_t type = buf[total_bytes_used + 2]; - // -------------------- handle FMT-message -------------------- // if (type == LOG_FORMAT_MSG) { @@ -277,7 +272,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da break; } - // -------------------- handle FMTU-message -------------------- // if ( memcmp(fmt.name, "FMTU", 4) == 0 ) { @@ -291,7 +285,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da struct log_Format_Units& fmtu = format_units[msg_id]; memcpy(&fmtu, &buf[total_bytes_used], sizeof(struct log_Format_Units)); - // handle instances // - check if units contain "#" (see also: logformat.h) if ( !has_instance[msg_id] ) @@ -326,7 +319,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da continue; } - // -------------------- handle MULT-message -------------------- // if ( memcmp(fmt.name, "MULT", 4) == 0 ) { @@ -354,7 +346,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da continue; } - // -------------------- handle UNIT-message -------------------- // if ( memcmp(fmt.name, "UNIT", 4) == 0 ) { @@ -382,7 +373,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da continue; } - // -------------------- handle any other message -------------------- // // discard some messages that should not be used: @@ -428,7 +418,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da #endif } - // -------------------- process UNITs -------------------- // #ifdef DEBUG_RUNTIME auto process_units_start = std::chrono::high_resolution_clock::now(); @@ -475,7 +464,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da process_units_ms += (process_units_end - process_units_start); #endif - // -------------------- apply multipliers -------------------- // #ifdef DEBUG_RUNTIME auto apply_mult_start = std::chrono::high_resolution_clock::now(); @@ -486,7 +474,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da apply_mult_ms += (apply_mult_end - apply_mult_start); #endif - // -------------------- apply timesync -------------------- // #ifdef DEBUG_RUNTIME auto apply_tsync_start = std::chrono::high_resolution_clock::now(); @@ -497,8 +484,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da apply_tsync_ms += (apply_tsync_end - apply_tsync_start); #endif - - #ifdef DEBUG_MESSAGES std::printf("\n--------- DEBUG_MESSAGES ---------"); for (int idx=0; idx < 256; idx++) @@ -548,8 +533,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da std::printf("-------------- END --------------\n\n"); #endif - - // -------------------- publish to plotjuggler -------------------- // #ifdef DEBUG_RUNTIME auto publish_start = std::chrono::high_resolution_clock::now(); @@ -659,10 +642,6 @@ bool DataLoadAPBIN::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_da return true; } - - - - void DataLoadAPBIN::handle_message_received(const struct log_Format& fmt, const uint8_t* msg) { // message id @@ -695,7 +674,6 @@ void DataLoadAPBIN::handle_message_received(const struct log_Format& fmt, const uint32_t msg_offset = LOG_PACKET_HEADER_LEN; // discard header - /* If you need to change this section, please also fix logformat.h (format_types)! AP_Logger: Format Types (https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_Logger#format-types) @@ -802,8 +780,6 @@ void DataLoadAPBIN::handle_message_received(const struct log_Format& fmt, const } } - - DataLoadAPBIN::message_data DataLoadAPBIN::create_message_data(const struct log_Format& fmt) { QString labelStr(fmt.labels); @@ -823,8 +799,6 @@ DataLoadAPBIN::message_data DataLoadAPBIN::create_message_data(const struct log_ return msg_data; } - - uint32_t DataLoadAPBIN::get_field_byte_offset(const uint8_t& msg_id, const uint8_t& field_idx) { // check if FMT exists @@ -863,8 +837,6 @@ uint32_t DataLoadAPBIN::get_field_byte_offset(const uint8_t& msg_id, const uint8 return total_offset; } - - uint32_t DataLoadAPBIN::get_field_byte_offset(const uint8_t& msg_id, const std::string& field_name) { const std::string& msg_name = msg_id2name[msg_id]; @@ -873,8 +845,6 @@ uint32_t DataLoadAPBIN::get_field_byte_offset(const uint8_t& msg_id, const std:: return get_field_byte_offset(msg_id, field_idx); } - - uint8_t DataLoadAPBIN::get_instance(const struct log_Format& fmt, const uint8_t* msg) { // Read sensor instance from raw message byte sequence @@ -892,8 +862,6 @@ uint8_t DataLoadAPBIN::get_instance(const struct log_Format& fmt, const uint8_t* return instance; } - - std::string DataLoadAPBIN::get_unit(const std::string& msg_name, const std::string& field_name) { // get message id for message name @@ -923,8 +891,6 @@ std::string DataLoadAPBIN::get_unit(const std::string& msg_name, const std::stri return unit_it->second; } - - void DataLoadAPBIN::apply_multipliers(void) { // Go through all messages, instances, fields and apply the correct multiplier from FMTU and MULT @@ -979,70 +945,172 @@ void DataLoadAPBIN::apply_multipliers(void) } +double DataLoadAPBIN::gps_to_unix_time(double gps_week, double gps_ms_of_week) +{ + static constexpr double SECONDS_PER_WEEK = 60 * 60 * 24 * 7; // 60 * 60 * 24 * 7 + static constexpr double MS_PER_SECOND = 1000.0; + static constexpr double GPS2UNIX_TIME_OFFSET = 315964800.0; // Unix epoch vs GPS epoch + static constexpr double GPS2UNIX_LEAP_SECONDS = -18.0; // Current leap seconds + + const double gps_week_seconds = gps_ms_of_week / MS_PER_SECOND; -void DataLoadAPBIN::apply_timesync(void) + return (gps_week * SECONDS_PER_WEEK) + + gps_week_seconds + + GPS2UNIX_TIME_OFFSET + + GPS2UNIX_LEAP_SECONDS; +} + +namespace { - // ArduPilot logs should be comparable with rosbags, therefore the same time basis is needed... - // - rosbag: unix time - // - ArduPilot log: local time since power on - // -> the logged GNSS time can be used for synchronisation - // extract GNSS time - const auto msg_it = messages_map.find("GPS"); - if ( msg_it == messages_map.end() ) + // Small struct to carry all four resolved GPS field indices together, + // instead of passing four separate uint8_t out-parameters around. + struct GpsFieldIndices { - std::printf("Skipping timesync because the logfile does not contain GNSS data\n"); - return; + uint8_t time_idx; + uint8_t week_idx; + uint8_t ms_idx; + uint8_t nsats_idx; + }; + + // Resolves the index of each required GPS field by name. + // Returns std::nullopt (and prints a clear message) if any field is + // missing, instead of silently defaulting to index 0 via + // std::map::operator[]. + std::optional resolve_gps_field_indices( + const std::map>& field_name2idx) + { + const auto gps_fields_it = field_name2idx.find("GPS"); + if (gps_fields_it == field_name2idx.end()) + { + std::printf("Skipping timesync because the logfile has no field definitions for GPS\n"); + return std::nullopt; + } + const auto& gps_fields = gps_fields_it->second; + + auto find_field = [&](const std::string& label) -> std::optional + { + const auto it = gps_fields.find(label); + if (it == gps_fields.end()) + { + std::printf("Skipping timesync because GPS message has no '%s' field!\n", label.c_str()); + return std::nullopt; + } + return it->second; + }; + + const auto time_idx = find_field("TimeUS"); + const auto week_idx = find_field("GWk"); + const auto ms_idx = find_field("GMS"); + const auto nsats_idx = find_field("NSats"); + + if (!time_idx || !week_idx || !ms_idx || !nsats_idx) + { + return std::nullopt; + } + + return GpsFieldIndices{ *time_idx, *week_idx, *ms_idx, *nsats_idx }; } - // take the first instance as reference - // todo: change that? - const message_data& gps_msg_data = messages_map["GPS"][0]; + // Scans the logged GPS timeline for the first sample with a usable fix: + // enough satellites, and non-zero TimeUS/GWk/GMS. GWk in particular must be + // checked, since a sample can report NSats > 4 and non-zero TimeUS/GMS + // while GWk is still 0 (week number not yet resolved) - accepting that + // sample would anchor the whole log to the GPS epoch (1980-01-06). + std::optional find_first_valid_gps_sample( + const std::vector& time_vec, + const std::vector& week_vec, + const std::vector& ms_vec, + const std::vector& nsats_vec, + double min_nsats) + { + for (size_t i = 0; i < time_vec.size(); ++i) + { + if (i < nsats_vec.size() && i < week_vec.size() && i < ms_vec.size() && + nsats_vec[i] >= min_nsats && + time_vec[i] != 0.0 && + week_vec[i] > 0.0 && + ms_vec[i] > 0.0) + { + return i; + } + } + return std::nullopt; + } - // counter - int idx; - - // get needed field indexes - const auto& gps_time_idx = field_name2idx["GPS"]["TimeUS"]; - const auto& gps_week_idx = field_name2idx["GPS"]["GWk"]; // GWk -> GPS week - const auto& gps_ms_idx = field_name2idx["GPS"]["GMS"]; // GMS -> GPS seconds in week (ms) + void shift_all_timestamps( + std::map>>>>& messages_map, + const std::map>& field_name2idx, + double time_offset_sec) + { + for (auto& [msg_name, instances_map] : messages_map) + { + const auto msg_fields_it = field_name2idx.find(msg_name); + if (msg_fields_it == field_name2idx.end()) + { + continue; + } - // constant time offset variables - static constexpr double GPS2UNIX_TIME_OFFSET = 315964800; // time offset between unix and gps time - static constexpr double GPS2UNIX_LEAP_SECONDS = -18; // additional time offset due to leap seconds (must be adjusted if number of leap seconds changes!) - static constexpr double SECONDS_PER_WEEK = 604800; // number of seconds per week + const auto time_idx_it = msg_fields_it->second.find("TimeUS"); + if (time_idx_it == msg_fields_it->second.end()) + { + continue; + } + const auto& msg_time_idx = time_idx_it->second; - const double& gps_week = gps_msg_data[gps_week_idx].second[1]; - const double gps_week_seconds = gps_msg_data[gps_ms_idx].second[1] * 0.001; + for (auto& [instance_id, msg_data] : instances_map) + { + std::vector& timestamps = msg_data[msg_time_idx].second; + std::transform(timestamps.begin(), timestamps.end(), timestamps.begin(), + [time_offset_sec](double t) { return t + time_offset_sec; }); + } + } + } - const double unix_time = gps_week * SECONDS_PER_WEEK + gps_week_seconds + GPS2UNIX_TIME_OFFSET + GPS2UNIX_LEAP_SECONDS; +} - const double& log_time = gps_msg_data[gps_time_idx].second[1]; +void DataLoadAPBIN::apply_timesync(void) +{ + static constexpr double MIN_VALID_NSATS = 4; - const double time_offset = unix_time - log_time; + const auto msg_it = messages_map.find("GPS"); + if (msg_it == messages_map.end() || msg_it->second.empty()) + { + std::printf("Skipping timesync because the logfile does not contain GNSS data\n"); + return; + } + // Target the first available GPS instance (typically Instance 0) + const auto& first_gps_instance = msg_it->second.begin()->second; - // iterate through messages - for (auto& msg_it : messages_map) + const auto field_indices = resolve_gps_field_indices(field_name2idx); + if (!field_indices) { - const auto& msg_name = msg_it.first; + return; + } - auto time_idx_it = field_name2idx[msg_name].find("TimeUS"); - if (time_idx_it == field_name2idx[msg_name].end()) - { - continue; - } - auto& time_idx = time_idx_it->second; + const auto& time_vec = first_gps_instance.at(field_indices->time_idx).second; + const auto& week_vec = first_gps_instance.at(field_indices->week_idx).second; + const auto& ms_vec = first_gps_instance.at(field_indices->ms_idx).second; + const auto& nsats_vec = first_gps_instance.at(field_indices->nsats_idx).second; - // iterate through instances - auto& instances_map = msg_it.second; - for (auto& inst_it : instances_map) - { - // add time offset - message_data& msg_data = inst_it.second; + const auto valid_sample_idx = + find_first_valid_gps_sample(time_vec, week_vec, ms_vec, nsats_vec, MIN_VALID_NSATS); - std::vector& timestamps = msg_data[time_idx].second; - std::transform(timestamps.begin(), timestamps.end(), timestamps.begin(), std::bind(std::plus(), std::placeholders::_1, time_offset)); - } + if (!valid_sample_idx) + { + std::printf("Skipping timesync because no sequential GPS sample with a valid fix was found\n"); + return; } + + const double gps_week = week_vec[*valid_sample_idx]; + const double gps_week_ms = ms_vec[*valid_sample_idx]; + + const double log_time_sec = time_vec[*valid_sample_idx]; + + const double unix_time_sec = gps_to_unix_time(gps_week, gps_week_ms); + const double time_offset_sec = unix_time_sec - log_time_sec; + + shift_all_timestamps(messages_map, field_name2idx, time_offset_sec); } \ No newline at end of file diff --git a/DataLoadAPBin/dataload_apbin.h b/DataLoadAPBin/dataload_apbin.h index 69e986d..ee5d638 100644 --- a/DataLoadAPBin/dataload_apbin.h +++ b/DataLoadAPBin/dataload_apbin.h @@ -107,4 +107,6 @@ class DataLoadAPBIN : public DataLoader // apply time synchronization to the messages_map void apply_timesync(void); + + static double gps_to_unix_time(double gps_week, double gps_ms_of_week); }; \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 92bac47..5c5adcd 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,4 @@ -ARG BASE_IMAGE=ubuntu:22.04 -FROM ${BASE_IMAGE} AS build-stage +FROM ubuntu:22.04 AS build-stage ARG PJ_TAG=3.9.2 @@ -7,7 +6,7 @@ ARG PJ_TAG=3.9.2 # Install dependencies. ############################################################################### # General dependencies. -RUN apt update && apt install -y build-essential cmake git +RUN apt update && apt install -y build-essential cmake git # PlotJuggler dependencies. RUN apt update && apt -y install qtbase5-dev libqt5svg5-dev libqt5websockets5-dev \ libqt5opengl5-dev libqt5x11extras5-dev libprotoc-dev libzmq3-dev \ @@ -21,7 +20,7 @@ RUN git clone --depth 1 --branch ${PJ_TAG} https://github.com/facontidavide/Plot # Build PlotJuggler. This will take a long time. WORKDIR /plotjuggler_ws RUN cmake -S src/PlotJuggler -B build/PlotJuggler -DCMAKE_INSTALL_PREFIX=install \ - && cmake --build build/PlotJuggler --config RelWithDebInfo --target install + && cmake --build build/PlotJuggler --config RelWithDebInfo --target install -j"$(nproc)" ############################################################################### # Compile the plugin @@ -29,18 +28,16 @@ RUN cmake -S src/PlotJuggler -B build/PlotJuggler -DCMAKE_INSTALL_PREFIX=install ARG ADD_UNITS=OFF COPY --link . /apbin_plugin + WORKDIR /apbin_plugin/build # Ensure a fresh build folder -RUN rm -R * \ +RUN rm -Rf * \ && cmake -Dplotjuggler_DIR="/plotjuggler_ws/install/lib/cmake/plotjuggler" -DADD_UNITS=${ADD_UNITS} .. \ - && make \ + && make -j"$(nproc)" \ && make install \ && mkdir /artifacts \ && cp libDataAPBin.so /artifacts -############################################################################### -# Export the plugin -############################################################################### FROM scratch AS export-stage # Move the plugin to a fresh filesystem that can be exported easily. -COPY --from=build-stage /artifacts / +COPY --from=build-stage /artifacts / \ No newline at end of file