优化完空速表

CMakeLists.txt

@@ -22,6 +22,7 @@ add_library(instrument_reader_core
     src/dispatcher.cpp
     src/instrument_classifier.cpp
     src/lut_mapper.cpp
+    src/non_linear_gauge_reader.cpp
     src/types.cpp
 )
 
@@ -48,13 +49,16 @@ target_link_libraries(instrument_reader_cli PRIVATE instrument_reader_core)
 add_executable(airspeed_lut_cli apps/airspeed_lut_cli.cpp)
 target_link_libraries(airspeed_lut_cli PRIVATE instrument_reader_core)
 
+add_executable(non_linear_gauge_demo apps/non_linear_gauge_demo.cpp)
+target_link_libraries(non_linear_gauge_demo PRIVATE instrument_reader_core)
+
 add_executable(clion_demo apps/clion_demo.cpp)
 target_link_libraries(clion_demo PRIVATE instrument_reader_core)
 
 if(WIN32 AND OpenCV_DIR)
     get_filename_component(INSTRUMENT_READER_OPENCV_BIN "${OpenCV_DIR}/../bin" ABSOLUTE)
     set(INSTRUMENT_READER_RUN_ENV "PATH=${INSTRUMENT_READER_OPENCV_BIN};$ENV{PATH}")
-    foreach(target_name instrument_reader_cli airspeed_lut_cli clion_demo)
+    foreach(target_name instrument_reader_cli airspeed_lut_cli non_linear_gauge_demo clion_demo)
         set_target_properties(${target_name} PROPERTIES
             VS_DEBUGGER_ENVIRONMENT "${INSTRUMENT_READER_RUN_ENV}"
         )
@@ -107,7 +111,16 @@ add_custom_target(run_lut_example
     VERBATIM
 )
 
-install(TARGETS instrument_reader_core instrument_reader_cli airspeed_lut_cli
+add_custom_target(run_non_linear_gauge_demo
+    COMMAND ${CMAKE_COMMAND} -E make_directory "${INSTRUMENT_READER_OUTPUT_ROOT}"
+    COMMAND ${CMAKE_COMMAND} -E env "${INSTRUMENT_READER_RUN_ENV}"
+            $<TARGET_FILE:non_linear_gauge_demo>
+    DEPENDS non_linear_gauge_demo
+    WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}"
+    VERBATIM
+)
+
+install(TARGETS instrument_reader_core instrument_reader_cli airspeed_lut_cli non_linear_gauge_demo
     RUNTIME DESTINATION bin
     LIBRARY DESTINATION lib
     ARCHIVE DESTINATION lib

apps/instrument_reader_cli.cpp

 #include "instrument_reader/dispatcher.hpp"
 
 #include <filesystem>
+#include <initializer_list>
 #include <iostream>
 #include <stdexcept>
 #include <string>
@@ -26,6 +27,28 @@ fs::path firstExisting(std::initializer_list<fs::path> paths) {
     return {};
 }
 
+fs::path findAirspeedAssetNearInputs(const std::vector<fs::path>& inputs, std::initializer_list<fs::path> relativePaths) {
+    std::vector<fs::path> roots;
+    for (const fs::path& input : inputs) {
+        if (!fs::exists(input)) continue;
+
+        fs::path root = input;
+        if (fs::is_regular_file(root)) root = root.parent_path();
+        if (!fs::is_directory(root)) continue;
+
+        roots.push_back(root);
+        if (root.has_parent_path()) roots.push_back(root.parent_path());
+    }
+
+    for (const fs::path& root : roots) {
+        for (const fs::path& relativePath : relativePaths) {
+            const fs::path candidate = root / relativePath;
+            if (fs::exists(candidate)) return candidate;
+        }
+    }
+    return {};
+}
+
 }  // namespace
 
 int main(int argc, char** argv) {
@@ -57,14 +80,27 @@ int main(int argc, char** argv) {
         }
     }
 
+    if (inputs.empty()) {
+        inputs.push_back(firstExisting({"../2/original_crops", "D:/chuav/gague/2/original_crops"}));
+    }
+
+    const fs::path localAirspeedBase = findAirspeedAssetNearInputs(inputs, {
+        "_base_no_pointer.png",
+        "_restored_base_no_pointer.png",
+        "01_airspeed_indicator/_base_no_pointer.png",
+        "01_airspeed_indicator/_restored_base_no_pointer.png",
+        "generated_scales/01_airspeed_indicator/_restored_base_no_pointer.png",
+    });
+
     if (options.airspeedTemplatePath.empty()) {
         options.airspeedTemplatePath = firstExisting({
+            localAirspeedBase,
             "../2/generated_scales/01_airspeed_indicator/_restored_base_no_pointer.png",
             "D:/chuav/gague/2/generated_scales/01_airspeed_indicator/_restored_base_no_pointer.png",
         });
     }
     if (options.airspeedBasePath.empty()) {
-        options.airspeedBasePath = options.airspeedTemplatePath;
+        options.airspeedBasePath = firstExisting({localAirspeedBase, options.airspeedTemplatePath});
     }
     if (options.airspeedLutPath.empty()) {
         options.airspeedLutPath = firstExisting({
@@ -74,10 +110,6 @@ int main(int argc, char** argv) {
         });
     }
 
-    if (inputs.empty()) {
-        inputs.push_back(firstExisting({"../2/original_crops", "D:/chuav/gague/2/original_crops"}));
-    }
-
     try {
         InstrumentDispatcher dispatcher(options);
         DispatchSummary summary = dispatcher.run(inputs);

apps/non_linear_gauge_demo.cpp

+#include "instrument_reader/non_linear_gauge_reader.hpp"
+
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace {
+
+constexpr float kPi = 3.14159265358979323846f;
+
+std::filesystem::path nextOutputDir(const std::filesystem::path& outputRoot) {
+    std::filesystem::create_directories(outputRoot);
+
+    int maxRunNumber = 0;
+    constexpr const char* prefix = "output";
+    for (const auto& entry : std::filesystem::directory_iterator(outputRoot)) {
+        const std::string name = entry.path().filename().string();
+        if (name.rfind(prefix, 0) != 0 || name.size() == std::char_traits<char>::length(prefix)) continue;
+
+        int runNumber = 0;
+        bool valid = true;
+        for (std::size_t i = std::char_traits<char>::length(prefix); i < name.size(); ++i) {
+            if (name[i] < '0' || name[i] > '9') {
+                valid = false;
+                break;
+            }
+            const int digit = name[i] - '0';
+            if (runNumber > (std::numeric_limits<int>::max() - digit) / 10) {
+                valid = false;
+                break;
+            }
+            runNumber = runNumber * 10 + digit;
+        }
+        if (valid && runNumber > maxRunNumber) maxRunNumber = runNumber;
+    }
+
+    for (int runNumber = maxRunNumber + 1;; ++runNumber) {
+        const std::filesystem::path outputDir = outputRoot / ("output" + std::to_string(runNumber));
+        if (std::filesystem::create_directory(outputDir)) return outputDir;
+    }
+}
+
+cv::Point pointOnDial(const cv::Point2f& center, float radius, float angleDeg) {
+    const float radians = angleDeg * kPi / 180.0f;
+    return {
+        cvRound(center.x + std::cos(radians) * radius),
+        cvRound(center.y + std::sin(radians) * radius),
+    };
+}
+
+void putCenteredText(cv::Mat& image, const std::string& text, const cv::Point& anchor, double scale, cv::Scalar color,
+                     int thickness = 1) {
+    int baseline = 0;
+    const cv::Size size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, scale, thickness, &baseline);
+    cv::putText(image, text, {anchor.x - size.width / 2, anchor.y + size.height / 2}, cv::FONT_HERSHEY_SIMPLEX, scale,
+                color, thickness, cv::LINE_AA);
+}
+
+bool writeResultImage(const std::filesystem::path& imagePath,
+                      const std::vector<instrument_reader::nonlinear::CalibrationPoint>& calibration,
+                      float currentAngle, float currentValue) {
+    constexpr int width = 640;
+    constexpr int height = 560;
+    const cv::Point2f center(width * 0.5f, 315.0f);
+    const cv::Point centerPoint(cvRound(center.x), cvRound(center.y));
+    constexpr float radius = 210.0f;
+
+    cv::Mat image(height, width, CV_8UC3, cv::Scalar(248, 249, 250));
+
+    cv::circle(image, centerPoint, cvRound(radius), cv::Scalar(40, 45, 52), 3, cv::LINE_AA);
+    cv::circle(image, centerPoint, 8, cv::Scalar(40, 45, 52), -1, cv::LINE_AA);
+
+    for (const auto& point : calibration) {
+        const cv::Point outer = pointOnDial(center, radius, point.angle);
+        const cv::Point inner = pointOnDial(center, radius - 22.0f, point.angle);
+        cv::line(image, inner, outer, cv::Scalar(38, 92, 180), 3, cv::LINE_AA);
+
+        std::ostringstream valueLabel;
+        valueLabel << std::fixed << std::setprecision(0) << point.value;
+        const cv::Point labelPoint = pointOnDial(center, radius - 55.0f, point.angle);
+        putCenteredText(image, valueLabel.str(), labelPoint, 0.55, cv::Scalar(35, 40, 48), 1);
+    }
+
+    const cv::Point pointerTip = pointOnDial(center, radius - 28.0f, currentAngle);
+    cv::line(image, centerPoint, pointerTip, cv::Scalar(20, 20, 210), 5, cv::LINE_AA);
+    cv::circle(image, centerPoint, 13, cv::Scalar(20, 20, 210), -1, cv::LINE_AA);
+
+    putCenteredText(image, "Non-linear airspeed interpolation", {width / 2, 42}, 0.8, cv::Scalar(30, 35, 42), 2);
+
+    std::ostringstream resultLabel;
+    resultLabel << "theta=" << std::fixed << std::setprecision(2) << std::fmod(currentAngle, 360.0f)
+                << " deg  value=" << std::setprecision(1) << currentValue << " km/h";
+    putCenteredText(image, resultLabel.str(), {width / 2, 82}, 0.68, cv::Scalar(20, 20, 210), 2);
+
+    return cv::imwrite(imagePath.string(), image);
+}
+
+} // namespace
+
+int main() {
+    using instrument_reader::nonlinear::BoundaryMode;
+    using instrument_reader::nonlinear::CalibrationPoint;
+    using instrument_reader::nonlinear::NonLinearGaugeReader;
+
+    const std::vector<CalibrationPoint> airspeedCalibration = {
+        {334.1f, 80.0f},
+        {406.1f, 140.0f},
+        {489.9f, 200.0f},
+        {586.5f, 300.0f},
+    };
+
+    const NonLinearGaugeReader reader(airspeedCalibration, BoundaryMode::Clamp);
+    const float thetaFromYoloDeg = 35.99f;
+    const float thetaUnwrappedDeg =
+        thetaFromYoloDeg < airspeedCalibration.front().angle ? thetaFromYoloDeg + 360.0f : thetaFromYoloDeg;
+    const float airspeedKmh = reader.calculate_value(thetaUnwrappedDeg);
+
+    const std::filesystem::path outputDir = nextOutputDir("output");
+    const std::filesystem::path outputPath = outputDir / "non_linear_gauge_demo_result.json";
+    const std::filesystem::path imagePath = outputDir / "non_linear_gauge_demo_result.png";
+
+    if (!writeResultImage(imagePath, airspeedCalibration, thetaUnwrappedDeg, airspeedKmh)) {
+        std::cerr << "failed_to_write_image=" << std::filesystem::absolute(imagePath).string() << "\n";
+        return 1;
+    }
+
+    std::ofstream out(outputPath, std::ios::binary);
+    if (!out) {
+        std::cerr << "failed_to_open_output=" << std::filesystem::absolute(outputPath).string() << "\n";
+        return 1;
+    }
+
+    out << std::fixed << std::setprecision(3) << "{\n"
+        << "  \"valid\": true,\n"
+        << "  \"theta_deg\": " << thetaFromYoloDeg << ",\n"
+        << "  \"theta_unwrapped_deg\": " << thetaUnwrappedDeg << ",\n"
+        << "  \"airspeed_kmh\": " << airspeedKmh << ",\n"
+        << "  \"boundary_mode\": \"clamp\",\n"
+        << "  \"image\": \"" << std::filesystem::absolute(imagePath).generic_string() << "\"\n"
+        << "}\n";
+
+    std::cout << std::fixed << std::setprecision(3) << "theta_deg=" << thetaFromYoloDeg << "\n"
+              << "theta_unwrapped_deg=" << thetaUnwrappedDeg << "\n"
+              << "airspeed_kmh=" << airspeedKmh << "\n"
+              << "output=" << std::filesystem::absolute(outputPath).string() << "\n"
+              << "image=" << std::filesystem::absolute(imagePath).string() << "\n";
+
+    return 0;
+}

configs/airspeed_lut_example.json

 {
   "type": "instrument_lut_v1",
-  "start_angle_deg": 270.0000000000,
+  "start_angle_deg": 334.1000000000,
   "angle_unit": "degrees_image_coords_0_right_90_down",
-  "boundary_policy": "clamp",
+  "boundary_policy": "reject",
   "points": [
-    {"value": 0.0000000000, "theta_abs_deg": 270.0000000000, "theta_rel_deg": 0.0000000000},
-    {"value": 40.0000000000, "theta_abs_deg": 312.0000000000, "theta_rel_deg": 42.0000000000},
-    {"value": 80.0000000000, "theta_abs_deg": 354.0000000000, "theta_rel_deg": 84.0000000000},
-    {"value": 120.0000000000, "theta_abs_deg": 36.0000000000, "theta_rel_deg": 126.0000000000},
-    {"value": 160.0000000000, "theta_abs_deg": 78.0000000000, "theta_rel_deg": 168.0000000000},
-    {"value": 200.0000000000, "theta_abs_deg": 120.0000000000, "theta_rel_deg": 210.0000000000},
-    {"value": 240.0000000000, "theta_abs_deg": 162.0000000000, "theta_rel_deg": 252.0000000000},
-    {"value": 280.0000000000, "theta_abs_deg": 204.0000000000, "theta_rel_deg": 294.0000000000},
-    {"value": 300.0000000000, "theta_abs_deg": 225.0000000000, "theta_rel_deg": 315.0000000000}
+    {"value": 80.0000000000, "theta_abs_deg": 334.1000000000, "theta_rel_deg": 0.0000000000},
+    {"value": 140.0000000000, "theta_abs_deg": 46.1000000000, "theta_rel_deg": 72.0000000000},
+    {"value": 200.0000000000, "theta_abs_deg": 129.9000000000, "theta_rel_deg": 155.8000000000},
+    {"value": 300.0000000000, "theta_abs_deg": 226.5000000000, "theta_rel_deg": 252.4000000000}
   ]
 }

docs/ubuntu22.md

@@ -45,6 +45,9 @@ If the dataset is elsewhere:
 ./scripts/run_demo_ubuntu22.sh ./cmake-build-ubuntu22-release /path/to/data_root
 ```
 
+Processed overlays and `dispatch_results.json` are written under the next
+numbered run directory in `output`, for example `output/output9`.
+
 The data root must contain:
 
 ```text

include/instrument_reader/non_linear_gauge_reader.hpp

+#pragma once
+
+#include <cstddef>
+#include <vector>
+
+namespace instrument_reader::nonlinear {
+
+struct CalibrationPoint {
+    float angle = 0.0f;
+    float value = 0.0f;
+};
+
+enum class BoundaryMode {
+    // Saturate to the nearest calibrated endpoint. This is the safest default
+    // for real-time control loops when the detector briefly leaves the dial.
+    Clamp,
+
+    // Extend the first or last calibrated segment beyond the table range.
+    Extrapolate,
+};
+
+class NonLinearGaugeReader {
+  public:
+    // The input table is copied, sorted by angle once, and validated for
+    // finite values plus strictly increasing angles.
+    explicit NonLinearGaugeReader(std::vector<CalibrationPoint> calibration, BoundaryMode boundaryMode = BoundaryMode::Clamp);
+
+    // Hot path: no allocation, no exception, O(log N) segment lookup.
+    // Returns NaN when the upstream angle itself is NaN or infinity.
+    [[nodiscard]] float calculate_value(float current_angle) const noexcept;
+    [[nodiscard]] const std::vector<CalibrationPoint>& calibration() const noexcept { return calibration_; }
+    [[nodiscard]] BoundaryMode boundaryMode() const noexcept { return boundaryMode_; }
+
+  private:
+    [[nodiscard]] float interpolateSegment(std::size_t leftIndex, float angle) const noexcept;
+
+    std::vector<CalibrationPoint> calibration_;
+    std::vector<float> slopes_;
+    BoundaryMode boundaryMode_ = BoundaryMode::Clamp;
+};
+
+} // namespace instrument_reader::nonlinear

include/instrument_reader/types.hpp

@@ -62,6 +62,11 @@ struct ProcessResult {
     double airspeedValue = 0.0;
     bool airspeedThetaRelValid = false;
     double airspeedThetaRelDeg = 0.0;
+    bool airspeedSegmentValid = false;
+    double airspeedSegmentStartValue = 0.0;
+    double airspeedSegmentEndValue = 0.0;
+    double airspeedSegmentStartThetaRelDeg = 0.0;
+    double airspeedSegmentEndThetaRelDeg = 0.0;
     std::string airspeedValueCode;
     std::string airspeedValueMessage;
     double elapsedMs = 0.0;

scripts/run_demo_ubuntu22.sh

@@ -5,7 +5,7 @@ SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 PROJECT_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)"
 BUILD_DIR="${1:-${PROJECT_ROOT}/cmake-build-ubuntu22-release}"
 DATA_ROOT="${2:-${PROJECT_ROOT}/../2}"
-OUT_DIR="${PROJECT_ROOT}/results/ubuntu22_dispatch_results"
+OUT_DIR="${PROJECT_ROOT}/output"
 EXE="${BUILD_DIR}/instrument_reader_cli"
 
 if [[ ! -x "${EXE}" ]]; then
@@ -19,4 +19,5 @@ fi
   --input "${DATA_ROOT}/generated_scales/01_airspeed_indicator" \
   --airspeed-template "${DATA_ROOT}/generated_scales/01_airspeed_indicator/_restored_base_no_pointer.png" \
   --airspeed-base "${DATA_ROOT}/generated_scales/01_airspeed_indicator/_restored_base_no_pointer.png" \
+  --airspeed-lut "${PROJECT_ROOT}/configs/airspeed_lut_example.json" \
   --out-dir "${OUT_DIR}"

src/dispatcher.cpp

@@ -8,9 +8,11 @@
 #include <cctype>
 #include <fstream>
 #include <iomanip>
+#include <limits>
 #include <set>
 #include <sstream>
 #include <stdexcept>
+#include <string>
 #include <utility>
 
 namespace instrument_reader {
@@ -48,6 +50,25 @@ cv::Mat drawSkippedOverlay(const cv::Mat& bgr, const ClassificationResult& cls)
     return out;
 }
 
+bool parseOutputRunNumber(const fs::path& path, int& runNumber) {
+    constexpr const char* prefix = "output";
+    const std::string name = path.filename().string();
+    if (name.rfind(prefix, 0) != 0 || name.size() == std::char_traits<char>::length(prefix)) return false;
+
+    int value = 0;
+    for (size_t i = std::char_traits<char>::length(prefix); i < name.size(); ++i) {
+        const unsigned char ch = static_cast<unsigned char>(name[i]);
+        if (!std::isdigit(ch)) return false;
+        const int digit = name[i] - '0';
+        if (value > (std::numeric_limits<int>::max() - digit) / 10) return false;
+        value = value * 10 + digit;
+    }
+    if (value <= 0) return false;
+
+    runNumber = value;
+    return true;
+}
+
 void drawAirspeedValueLabel(cv::Mat& out, const ProcessResult& result) {
     int labelHeight = std::min(58, out.rows);
     cv::rectangle(out, cv::Rect(0, 0, out.cols, labelHeight), cv::Scalar(0, 0, 0), -1);
@@ -64,6 +85,10 @@ void drawAirspeedValueLabel(cv::Mat& out, const ProcessResult& result) {
     if (result.angle.valid) {
         line2 << "theta=" << std::fixed << std::setprecision(2) << result.angle.angleDegMod360;
         if (result.airspeedThetaRelValid) line2 << " rel=" << result.airspeedThetaRelDeg;
+        if (result.airspeedSegmentValid) {
+            line2 << " seg=" << std::setprecision(0) << result.airspeedSegmentStartValue << "-"
+                  << result.airspeedSegmentEndValue;
+        }
     } else {
         line2 << result.angle.errorCode;
     }
@@ -177,6 +202,11 @@ ProcessResult InstrumentDispatcher::processOne(const fs::path& path, int index)
             result.airspeedValue = mapped.value;
             result.airspeedThetaRelValid = true;
             result.airspeedThetaRelDeg = mapped.thetaRelDeg;
+            result.airspeedSegmentValid = mapped.valid;
+            result.airspeedSegmentStartValue = mapped.left.value;
+            result.airspeedSegmentEndValue = mapped.right.value;
+            result.airspeedSegmentStartThetaRelDeg = mapped.left.thetaRelDeg;
+            result.airspeedSegmentEndThetaRelDeg = mapped.right.thetaRelDeg;
             result.airspeedValueCode = mapped.code;
             result.airspeedValueMessage = mapped.message;
         } else if (result.angle.valid) {
@@ -243,6 +273,21 @@ void InstrumentDispatcher::writeResultsJson(const DispatchSummary& summary) cons
         } else {
             out << "      \"airspeed_value\": null,\n";
         }
+        if (r.airspeedSegmentValid) {
+            out << "      \"airspeed_segment_value_min\": " << std::fixed << std::setprecision(6)
+                << r.airspeedSegmentStartValue << ",\n";
+            out << "      \"airspeed_segment_value_max\": " << std::fixed << std::setprecision(6)
+                << r.airspeedSegmentEndValue << ",\n";
+            out << "      \"airspeed_segment_theta_rel_min\": " << std::fixed << std::setprecision(6)
+                << r.airspeedSegmentStartThetaRelDeg << ",\n";
+            out << "      \"airspeed_segment_theta_rel_max\": " << std::fixed << std::setprecision(6)
+                << r.airspeedSegmentEndThetaRelDeg << ",\n";
+        } else {
+            out << "      \"airspeed_segment_value_min\": null,\n";
+            out << "      \"airspeed_segment_value_max\": null,\n";
+            out << "      \"airspeed_segment_theta_rel_min\": null,\n";
+            out << "      \"airspeed_segment_theta_rel_max\": null,\n";
+        }
         out << "      \"airspeed_value_code\": "
             << (r.airspeedValueCode.empty() ? "null" : "\"" + jsonEscape(r.airspeedValueCode) + "\"") << ",\n";
         out << "      \"airspeed_value_message\": "
@@ -268,7 +313,13 @@ bool InstrumentDispatcher::isHelperImage(const fs::path& path) {
 fs::path InstrumentDispatcher::nextIncrementalOutputDir(const fs::path& outputRoot) {
     fs::create_directories(outputRoot);
 
-    for (int i = 1;; ++i) {
+    int maxRunNumber = 0;
+    for (const auto& entry : fs::directory_iterator(outputRoot)) {
+        int runNumber = 0;
+        if (parseOutputRunNumber(entry.path(), runNumber)) maxRunNumber = std::max(maxRunNumber, runNumber);
+    }
+
+    for (int i = maxRunNumber + 1;; ++i) {
         fs::path candidate = outputRoot / ("output" + std::to_string(i));
         std::error_code ec;
         if (fs::create_directory(candidate, ec)) return candidate;

src/lut_mapper.cpp

@@ -5,6 +5,7 @@
 #include <algorithm>
 #include <fstream>
 #include <iomanip>
+#include <iterator>
 #include <regex>
 #include <sstream>
 #include <utility>
@@ -177,23 +178,35 @@ MapResult LutMapper::map(double thetaAbsDeg) const {
         return result;
     }
 
-    for (size_t i = 1; i < points_.size(); ++i) {
-        const CalibrationPoint& left = points_[i - 1];
-        const CalibrationPoint& right = points_[i];
-        if (result.thetaRelDeg <= right.thetaRelDeg) {
-            double denom = right.thetaRelDeg - left.thetaRelDeg;
-            double t = denom > 0.0 ? (result.thetaRelDeg - left.thetaRelDeg) / denom : 0.0;
-            result.valid = true;
-            result.value = left.value + t * (right.value - left.value);
-            result.left = left;
-            result.right = right;
-            result.code = "OK_INTERPOLATED";
-            return result;
-        }
+    auto rightIt = std::lower_bound(points_.begin(), points_.end(), result.thetaRelDeg,
+                                    [](const CalibrationPoint& point, double thetaRelDeg) {
+                                        return point.thetaRelDeg < thetaRelDeg;
+                                    });
+
+    if (rightIt == points_.begin()) {
+        result.valid = true;
+        result.value = rightIt->value;
+        result.left = *rightIt;
+        result.right = *rightIt;
+        result.code = "OK_INTERPOLATED";
+        return result;
+    }
+
+    if (rightIt == points_.end()) {
+        result.code = "ERR_NO_INTERVAL";
+        result.message = "no interpolation interval found";
+        return result;
     }
 
-    result.code = "ERR_NO_INTERVAL";
-    result.message = "no interpolation interval found";
+    const CalibrationPoint& left = *std::prev(rightIt);
+    const CalibrationPoint& right = *rightIt;
+    const double denom = right.thetaRelDeg - left.thetaRelDeg;
+    const double t = denom > 0.0 ? (result.thetaRelDeg - left.thetaRelDeg) / denom : 0.0;
+    result.valid = true;
+    result.value = left.value + t * (right.value - left.value);
+    result.left = left;
+    result.right = right;
+    result.code = "OK_INTERPOLATED";
     return result;
 }
 

src/non_linear_gauge_reader.cpp

+#include "instrument_reader/non_linear_gauge_reader.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <stdexcept>
+#include <utility>
+
+namespace instrument_reader::nonlinear {
+
+namespace {
+
+bool isFinite(CalibrationPoint point) noexcept { return std::isfinite(point.angle) && std::isfinite(point.value); }
+
+} // namespace
+
+NonLinearGaugeReader::NonLinearGaugeReader(std::vector<CalibrationPoint> calibration, BoundaryMode boundaryMode)
+    : calibration_(std::move(calibration)), boundaryMode_(boundaryMode) {
+    if (calibration_.size() < 2) {
+        throw std::invalid_argument("NonLinearGaugeReader requires at least two calibration points");
+    }
+
+    if (!std::all_of(calibration_.begin(), calibration_.end(), isFinite)) {
+        throw std::invalid_argument("NonLinearGaugeReader calibration contains non-finite data");
+    }
+
+    // Sorting once at construction keeps the high-frequency read path branch-light.
+    std::sort(calibration_.begin(), calibration_.end(),
+              [](const CalibrationPoint& a, const CalibrationPoint& b) { return a.angle < b.angle; });
+
+    slopes_.reserve(calibration_.size() - 1);
+    for (std::size_t i = 1; i < calibration_.size(); ++i) {
+        const CalibrationPoint& left = calibration_[i - 1];
+        const CalibrationPoint& right = calibration_[i];
+        const float deltaAngle = right.angle - left.angle;
+        if (deltaAngle <= 0.0f) {
+            throw std::invalid_argument("NonLinearGaugeReader calibration angles must be strictly increasing");
+        }
+        slopes_.push_back((right.value - left.value) / deltaAngle);
+    }
+}
+
+float NonLinearGaugeReader::calculate_value(float current_angle) const noexcept {
+    if (!std::isfinite(current_angle)) {
+        return std::numeric_limits<float>::quiet_NaN();
+    }
+
+    const CalibrationPoint& first = calibration_.front();
+    const CalibrationPoint& last = calibration_.back();
+
+    if (current_angle <= first.angle) {
+        return boundaryMode_ == BoundaryMode::Clamp ? first.value : interpolateSegment(0, current_angle);
+    }
+
+    if (current_angle >= last.angle) {
+        return boundaryMode_ == BoundaryMode::Clamp ? last.value : interpolateSegment(slopes_.size() - 1, current_angle);
+    }
+
+    const auto upper = std::lower_bound(calibration_.begin(), calibration_.end(), current_angle,
+                                        [](const CalibrationPoint& point, float angle) { return point.angle < angle; });
+    const std::size_t rightIndex = static_cast<std::size_t>(upper - calibration_.begin());
+
+    if (upper->angle == current_angle) {
+        return upper->value;
+    }
+
+    return interpolateSegment(rightIndex - 1, current_angle);
+}
+
+float NonLinearGaugeReader::interpolateSegment(std::size_t leftIndex, float angle) const noexcept {
+    const CalibrationPoint& left = calibration_[leftIndex];
+    return left.value + (angle - left.angle) * slopes_[leftIndex];
+}
+
+} // namespace instrument_reader::nonlinear

src/types.cpp

@@ -13,7 +13,7 @@ int DispatchSummary::airspeedCount() const {
 int DispatchSummary::validAirspeedCount() const {
     int count = 0;
     for (const ProcessResult& r : results) {
-        if (r.classification.type == InstrumentType::AirspeedIndicator && r.angle.valid) ++count;
+        if (r.classification.type == InstrumentType::AirspeedIndicator && r.airspeedValueValid) ++count;
     }
     return count;
 }