From 3ea3a4af2a2c3c110776058e9708613e62230070 Mon Sep 17 00:00:00 2001 From: Jair-F Date: Tue, 30 Jun 2026 23:06:52 +0300 Subject: [PATCH 1/3] CI: Pin Windows version and fix caching and multicore build with units restore restore fix win build try fix fix plotjuggler multicore build fix linux build try again try fix fix run on windows 22 removed not needed windows files --- .github/workflows/win_build.yml | 22 ++++++++++++++-------- Dockerfile | 8 ++++---- 2 files changed, 18 insertions(+), 12 deletions(-) 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/Dockerfile b/Dockerfile index 92bac47..b97772f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -7,7 +7,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 +21,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 @@ -31,7 +31,7 @@ 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 install \ @@ -43,4 +43,4 @@ RUN rm -R * \ ############################################################################### 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 From 12f452b23de8da846b5ee7c6a342d1384bdb328d Mon Sep 17 00:00:00 2001 From: Jair-F Date: Tue, 30 Jun 2026 23:12:55 +0300 Subject: [PATCH 2/3] Catch case where GPS time is not available at start of log faster build + fix take first valid gps msg fixes Refactor GPS time conversion and synchronization Refactor gps_to_unix_time function and update time synchronization logic to improve clarity and accuracy. Add support for tag-based builds in CI workflow Fix formatting issues in win_build.yml Update GitHub Actions workflow for PlotJuggler build fix time range --- DataLoadAPBin/dataload_apbin.cpp | 195 +++++++++++++++++++------------ DataLoadAPBin/dataload_apbin.h | 2 + Dockerfile | 9 +- 3 files changed, 128 insertions(+), 78 deletions(-) diff --git a/DataLoadAPBin/dataload_apbin.cpp b/DataLoadAPBin/dataload_apbin.cpp index c8a7596..b2ef982 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,155 @@ 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; + + return (gps_week * SECONDS_PER_WEEK) + + gps_week_seconds + + GPS2UNIX_TIME_OFFSET + + GPS2UNIX_LEAP_SECONDS; +} void DataLoadAPBIN::apply_timesync(void) { - // 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 + static constexpr double MIN_VALID_NSATS = 4; - // extract GNSS time const auto msg_it = messages_map.find("GPS"); - if ( msg_it == messages_map.end() ) + if (msg_it == messages_map.end() || msg_it->second.empty()) { std::printf("Skipping timesync because the logfile does not contain GNSS data\n"); return; } - // take the first instance as reference - // todo: change that? - const message_data& gps_msg_data = messages_map["GPS"][0]; + // Target the first available GPS instance (typically Instance 0) + const auto& first_gps_instance = msg_it->second.begin()->second; - // 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) + // Safely look up the GPS field-name -> index map first + 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; + } + const auto& gps_fields = gps_fields_it->second; - // 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 + // Safely resolve each required field index, bailing out with a clear + // error if any label is missing instead of silently defaulting to 0 + // (which is what operator[] on a std::map would otherwise do). + auto require_field = [&](const std::string& label, uint8_t& out_idx) -> bool + { + 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 false; + } + out_idx = it->second; + return true; + }; - 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; + uint8_t time_idx{ 0 }; + uint8_t week_idx{ 0 }; + uint8_t ms_idx{ 0 }; + uint8_t nsats_idx{ 0 }; - const double unix_time = gps_week * SECONDS_PER_WEEK + gps_week_seconds + GPS2UNIX_TIME_OFFSET + GPS2UNIX_LEAP_SECONDS; + if (!require_field("TimeUS", time_idx) || + !require_field("GWk", week_idx) || + !require_field("GMS", ms_idx) || + !require_field("NSats", nsats_idx)) + { + return; + } - const double& log_time = gps_msg_data[gps_time_idx].second[1]; + // Extract the actual vectors of logged data points + const auto& time_vec = first_gps_instance.at(time_idx).second; + const auto& week_vec = first_gps_instance.at(week_idx).second; + const auto& ms_vec = first_gps_instance.at(ms_idx).second; + const auto& nsats_vec = first_gps_instance.at(nsats_idx).second; + + // DEBUG: confirm units of time_vec before trusting any conversion assumption. + // If these print as small fractional numbers (e.g. 45.231), TimeUS is already + // in seconds after apply_multipliers() ran. If they print as huge raw integers + // (e.g. 45231000), TimeUS is still in microseconds at this point. + if (!time_vec.empty()) + { + std::printf("DEBUG: raw time_vec.front()=%.6f time_vec.back()=%.6f\n", + time_vec.front(), time_vec.back()); + } - const double time_offset = unix_time - log_time; + size_t valid_sample_idx = 0; + bool found_valid_fix = false; + // Loop THROUGH THE TIMELINE of logged GPS samples + 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_VALID_NSATS && // relaxed to >= + time_vec[i] != 0.0 && // added missing check + week_vec[i] > 0.0 && + ms_vec[i] > 0.0) + { + valid_sample_idx = i; + found_valid_fix = true; + break; + } + } - // iterate through messages - for (auto& msg_it : messages_map) + if (!found_valid_fix) { - const auto& msg_name = msg_it.first; + std::printf("Skipping timesync because no sequential GPS sample with a valid fix was found\n"); + return; + } - auto time_idx_it = field_name2idx[msg_name].find("TimeUS"); - if (time_idx_it == field_name2idx[msg_name].end()) + // Extract values at the valid chronological index found + const double gps_week = week_vec[valid_sample_idx]; + const double gps_week_ms = ms_vec[valid_sample_idx]; + + // TimeUS is already in SECONDS by this point, because apply_multipliers() + // runs before apply_timesync() and rescales TimeUS using its FMTU multiplier + // (commonly 0.000001, converting raw microseconds to seconds). Dividing by + // 1e6 again here was the bug that collapsed the whole log into a + // sub-one-second time range. + const double log_time_sec = time_vec[valid_sample_idx]; + + // Get Unix time directly in SECONDS (e.g., 1750692063.6) + const double unix_time_sec = gps_to_unix_time(gps_week, gps_week_ms); + + // Calculate offset purely in SECONDS + const double time_offset_sec = unix_time_sec - log_time_sec; + + std::printf("DEBUG: idx=%zu gps_week=%.3f gps_week_ms=%.3f log_time_sec=%.6f unix_time_sec=%.3f offset=%.3f\n", + valid_sample_idx, gps_week, gps_week_ms, log_time_sec, unix_time_sec, time_offset_sec); + + // Apply the offset to every message's TimeUS vector (already in seconds) + 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; } - auto& time_idx = time_idx_it->second; - // iterate through instances - auto& instances_map = msg_it.second; - for (auto& inst_it : instances_map) + const auto time_idx_it = msg_fields_it->second.find("TimeUS"); + if (time_idx_it == msg_fields_it->second.end()) { - // add time offset - message_data& msg_data = inst_it.second; + continue; + } + const auto& msg_time_idx = time_idx_it->second; + + for (auto& [instance_id, msg_data] : instances_map) + { + std::vector& timestamps = msg_data[msg_time_idx].second; - 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)); + // No conversion needed here anymore — just shift by the computed offset + std::transform(timestamps.begin(), timestamps.end(), timestamps.begin(), + [time_offset_sec](double t) { return t + 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 b97772f..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 @@ -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 -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 / \ No newline at end of file From e56de8137e55aef48b7262c50036f18aa66bcd8a Mon Sep 17 00:00:00 2001 From: Jair-F Date: Fri, 3 Jul 2026 21:49:10 +0300 Subject: [PATCH 3/3] refactor of time sync implementation --- DataLoadAPBin/dataload_apbin.cpp | 235 +++++++++++++++++-------------- 1 file changed, 126 insertions(+), 109 deletions(-) diff --git a/DataLoadAPBin/dataload_apbin.cpp b/DataLoadAPBin/dataload_apbin.cpp index b2ef982..9962b0e 100644 --- a/DataLoadAPBin/dataload_apbin.cpp +++ b/DataLoadAPBin/dataload_apbin.cpp @@ -947,10 +947,10 @@ 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 + 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; @@ -960,140 +960,157 @@ double DataLoadAPBIN::gps_to_unix_time(double gps_week, double gps_ms_of_week) + GPS2UNIX_LEAP_SECONDS; } -void DataLoadAPBIN::apply_timesync(void) +namespace { - static constexpr double MIN_VALID_NSATS = 4; - - 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; - - // Safely look up the GPS field-name -> index map first - const auto gps_fields_it = field_name2idx.find("GPS"); - if (gps_fields_it == field_name2idx.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 has no field definitions for 'GPS'\n"); - return; - } - const auto& gps_fields = gps_fields_it->second; + uint8_t time_idx; + uint8_t week_idx; + uint8_t ms_idx; + uint8_t nsats_idx; + }; - // Safely resolve each required field index, bailing out with a clear - // error if any label is missing instead of silently defaulting to 0 - // (which is what operator[] on a std::map would otherwise do). - auto require_field = [&](const std::string& label, uint8_t& out_idx) -> bool + // 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 it = gps_fields.find(label); - if (it == gps_fields.end()) + const auto gps_fields_it = field_name2idx.find("GPS"); + if (gps_fields_it == field_name2idx.end()) { - std::printf("Skipping timesync because GPS message has no '%s' field!\n", label.c_str()); - return false; + std::printf("Skipping timesync because the logfile has no field definitions for GPS\n"); + return std::nullopt; } - out_idx = it->second; - return true; - }; + 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; + }; - uint8_t time_idx{ 0 }; - uint8_t week_idx{ 0 }; - uint8_t ms_idx{ 0 }; - uint8_t nsats_idx{ 0 }; + 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 (!require_field("TimeUS", time_idx) || - !require_field("GWk", week_idx) || - !require_field("GMS", ms_idx) || - !require_field("NSats", nsats_idx)) - { - return; + if (!time_idx || !week_idx || !ms_idx || !nsats_idx) + { + return std::nullopt; + } + + return GpsFieldIndices{ *time_idx, *week_idx, *ms_idx, *nsats_idx }; } - // Extract the actual vectors of logged data points - const auto& time_vec = first_gps_instance.at(time_idx).second; - const auto& week_vec = first_gps_instance.at(week_idx).second; - const auto& ms_vec = first_gps_instance.at(ms_idx).second; - const auto& nsats_vec = first_gps_instance.at(nsats_idx).second; - - // DEBUG: confirm units of time_vec before trusting any conversion assumption. - // If these print as small fractional numbers (e.g. 45.231), TimeUS is already - // in seconds after apply_multipliers() ran. If they print as huge raw integers - // (e.g. 45231000), TimeUS is still in microseconds at this point. - if (!time_vec.empty()) + // 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) { - std::printf("DEBUG: raw time_vec.front()=%.6f time_vec.back()=%.6f\n", - time_vec.front(), time_vec.back()); + 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; } - size_t valid_sample_idx = 0; - bool found_valid_fix = false; - - // Loop THROUGH THE TIMELINE of logged GPS samples - for (size_t i = 0; i < time_vec.size(); ++i) + void shift_all_timestamps( + std::map>>>>& messages_map, + const std::map>& field_name2idx, + double time_offset_sec) { - if (i < nsats_vec.size() && i < week_vec.size() && i < ms_vec.size() && - nsats_vec[i] >= MIN_VALID_NSATS && // relaxed to >= - time_vec[i] != 0.0 && // added missing check - week_vec[i] > 0.0 && - ms_vec[i] > 0.0) + for (auto& [msg_name, instances_map] : messages_map) { - valid_sample_idx = i; - found_valid_fix = true; - break; + const auto msg_fields_it = field_name2idx.find(msg_name); + if (msg_fields_it == field_name2idx.end()) + { + continue; + } + + 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; + + 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; }); + } } } - if (!found_valid_fix) +} + +void DataLoadAPBIN::apply_timesync(void) +{ + static constexpr double MIN_VALID_NSATS = 4; + + const auto msg_it = messages_map.find("GPS"); + if (msg_it == messages_map.end() || msg_it->second.empty()) { - std::printf("Skipping timesync because no sequential GPS sample with a valid fix was found\n"); + std::printf("Skipping timesync because the logfile does not contain GNSS data\n"); return; } - // Extract values at the valid chronological index found - const double gps_week = week_vec[valid_sample_idx]; - const double gps_week_ms = ms_vec[valid_sample_idx]; - - // TimeUS is already in SECONDS by this point, because apply_multipliers() - // runs before apply_timesync() and rescales TimeUS using its FMTU multiplier - // (commonly 0.000001, converting raw microseconds to seconds). Dividing by - // 1e6 again here was the bug that collapsed the whole log into a - // sub-one-second time range. - const double log_time_sec = time_vec[valid_sample_idx]; + // Target the first available GPS instance (typically Instance 0) + const auto& first_gps_instance = msg_it->second.begin()->second; - // Get Unix time directly in SECONDS (e.g., 1750692063.6) - const double unix_time_sec = gps_to_unix_time(gps_week, gps_week_ms); + const auto field_indices = resolve_gps_field_indices(field_name2idx); + if (!field_indices) + { + return; + } - // Calculate offset purely in SECONDS - const double time_offset_sec = unix_time_sec - log_time_sec; + 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; - std::printf("DEBUG: idx=%zu gps_week=%.3f gps_week_ms=%.3f log_time_sec=%.6f unix_time_sec=%.3f offset=%.3f\n", - valid_sample_idx, gps_week, gps_week_ms, log_time_sec, unix_time_sec, time_offset_sec); + const auto valid_sample_idx = + find_first_valid_gps_sample(time_vec, week_vec, ms_vec, nsats_vec, MIN_VALID_NSATS); - // Apply the offset to every message's TimeUS vector (already in seconds) - for (auto& [msg_name, instances_map] : messages_map) + if (!valid_sample_idx) { - const auto msg_fields_it = field_name2idx.find(msg_name); - if (msg_fields_it == field_name2idx.end()) - { - continue; - } + std::printf("Skipping timesync because no sequential GPS sample with a valid fix was found\n"); + return; + } - 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 = week_vec[*valid_sample_idx]; + const double gps_week_ms = ms_vec[*valid_sample_idx]; - for (auto& [instance_id, msg_data] : instances_map) - { - std::vector& timestamps = msg_data[msg_time_idx].second; + const double log_time_sec = time_vec[*valid_sample_idx]; - // No conversion needed here anymore — just shift by the computed offset - std::transform(timestamps.begin(), timestamps.end(), timestamps.begin(), - [time_offset_sec](double t) { return t + time_offset_sec; }); - } - } + 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