CrossingEngine.h

Go to the documentation of this file.

 
#ifndef _DOVES_CROSSING_ENGINE_H
#define _DOVES_CROSSING_ENGINE_H
 
#include <Arduino.h>
// Polyfills std::forward (used by the debug templates below) on cores with
// no C++ standard library — classic AVR. Must come before any std:: use so
// this header is safe to include in any order relative to DovesLapTimer.h.
#include "ArxTypeTraits.h"
#include "GeoMath.h"
 
#define CROSSING_LINE_SIDE_A -1
#define CROSSING_LINE_SIDE_EXACT 0
#define CROSSING_LINE_SIDE_B 1
 
// GPS time base: milliseconds since UTC midnight, wraps 86,399,999 -> 0
#define DOVES_MILLIS_PER_DAY  86400000UL
 
// Max believable gap between the two consecutive buffered fixes that straddle
// a crossing line. A larger gap means the GPS time base stepped (e.g. u-blox
// re-acquisition) and the pair cannot be interpolated coherently.
#define CROSSING_MAX_FIX_GAP_MS  10000UL
 
// The straddling pair's summed distance to the line is validated against
// max(crossingThresholdMeters, factor * pair spacing) so that low-rate GPS
// (widely spaced fixes) doesn't fail validation purely on sample density.
#define CROSSING_PAIR_SPACING_FACTOR  1.25
 
static inline unsigned long timeSinceMidnightDelta(unsigned long startMs, unsigned long endMs) {
  if (endMs < startMs) {
    endMs += DOVES_MILLIS_PER_DAY;
  }
  return endMs - startMs;
}
 
// Outcome of a single CrossingEngine::detect pass.
enum LineDetectResult {
  LINE_DETECT_NONE,         // not in / near the zone, or exited with an invalid interpolation
  LINE_DETECT_IN_ZONE,      // inside the zone (entered this fix or continuing)
  LINE_DETECT_COMPLETED,    // exited the zone this fix — interpolated outputs valid
};
 
struct crossingPointBufferEntry {
  double lat; // latitude
  double lng; // longitude
  unsigned long time; // current time in milliseconds
  float odometer; // time traveled since device start and this entry
  float speedKmh; // speed in kmph
};
 
class CrossingEngine {
public:
  CrossingEngine(double crossingThresholdMeters = 7, Stream *debugSerial = NULL);
 
  void setThreshold(double meters);
  void setDebugSerial(Stream *debugSerial);
  void setForceLinear(bool linear);
  bool getForceLinear() const;
  unsigned int getRejectedCrossingCount() const;
  void reset();
 
  LineDetectResult detect(
      double currentLat, double currentLng,
      unsigned long currentTimeMs,
      float currentOdometer,
      float currentSpeedKmh,
      const crossingPointBufferEntry *prevFix,
      double pointALat, double pointALng,
      double pointBLat, double pointBLng,
      bool& crossingFlag,
      int lineLabel,
      double& outLat, double& outLng,
      unsigned long& outTime,
      double& outOdometer);
 
private:
#ifdef DOVES_DISABLE_DEBUG
  // Compile-time debug kill switch: the runtime _serial null-check still
  // keeps every debug string and print call-site in flash on production
  // builds that never attach a debug Stream. Defining DOVES_DISABLE_DEBUG
  // (e.g. -DDOVES_DISABLE_DEBUG in build flags) replaces the debug
  // templates with empty inlines so the compiler drops it all — several
  // KB on a fully-featured firmware. Debug behavior is unchanged when
  // the macro is not defined.
  template<typename... Args> void debug_print(Args&&...) {}
  template<typename... Args> void debug_println(Args&&...) {}
#else
  template<typename... Args>
  void debug_print(Args&&... args) {
    if(_serial) { _serial->print(std::forward<Args>(args)...); }
  }
  template<typename... Args>
  void debug_println(Args&&... args) {
    if(_serial) { _serial->println(std::forward<Args>(args)...); }
  }
#endif
 
  bool interpolateCrossingPoint(double& crossingLat, double& crossingLng, unsigned long& crossingTime, double& crossingOdometer, double pointALat, double pointALng, double pointBLat, double pointBLng);
  double interpolateWeight(double distA, double distB, float speedA, float speedB);
  double catmullRom(double p0, double p1, double p2, double p3, double t);
 
  Stream *_serial;
  double crossingThresholdMeters;
  bool forceLinear = true;
  unsigned int rejectedCrossingCount = 0;
 
  // Buffer sizing: AVR exposes RAMEND/RAMSTART so we can tell a Mega (8KB) from a Uno (2KB).
  // On modern 32-bit cores (nRF52, ESP32, SAMD, RP2040, etc.) those macros are undefined
  // and would silently evaluate to 0 - defaulting such targets to the small buffer would
  // defeat the whole point of the hotfix. Assume anyone not on classic AVR has plenty of RAM.
  #if defined(RAMEND) && defined(RAMSTART)
    #if ((RAMEND - RAMSTART) > 3000)
      static const int crossingPointBufferSize = 100;
    #else
      static const int crossingPointBufferSize = 25;
    #endif
  #else
    static const int crossingPointBufferSize = 100;
  #endif
 
  crossingPointBufferEntry crossingPointBuffer[crossingPointBufferSize];
  int crossingPointBufferIndex = 0;
  bool crossingPointBufferFull = false;
};
 
#endif