DovesLapTimer.h

Go to the documentation of this file.

 
#ifndef _DOVES_LAP_TIMER_H
#define _DOVES_LAP_TIMER_H
 
#include "ArxTypeTraits.h"
using TRITYPE = double;
 
#define CROSSING_LINE_SIDE_A -1
#define CROSSING_LINE_SIDE_EXACT 0
#define CROSSING_LINE_SIDE_B 1
 
// Course detection constants
#define COURSE_DETECT_SPEED_THRESHOLD_MPH  20
#define COURSE_DETECT_WAYPOINT_PROXIMITY_METERS 10.0
#define COURSE_DETECT_MIN_DISTANCE_METERS  200.0
#define COURSE_DETECT_DISTANCE_TOLERANCE_PCT  0.25
#define COURSE_DETECT_MAX_REJECTIONS  3
#define METERS_TO_FEET  3.28084
 
// Waypoint lap timer constants
#define WAYPOINT_LAP_MIN_DISTANCE_METERS  100.0
#define WAYPOINT_LAP_PROXIMITY_METERS  30.0
#define WAYPOINT_LAP_BUFFER_SIZE  50
 
// Direction detection
#define DIR_UNKNOWN  0
#define DIR_FORWARD  1
#define DIR_REVERSE  2
 
// Course detection states
#define DETECT_STATE_IDLE  0
#define DETECT_STATE_WAITING_FOR_SPEED  1
#define DETECT_STATE_WAYPOINT_SET  2
#define DETECT_STATE_CANDIDATES_READY  3
#define DETECT_STATE_DETECTED  4
 
// Maximum courses supported
#define MAX_COURSES  8
 
// Outcome of a single _detectLineCrossing pass.
enum LineDetectResult {
  LINE_DETECT_NONE,         // not in / near the crossing zone
  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
};
 
struct DirectionDetector {
  int direction;    // DIR_UNKNOWN, DIR_FORWARD, DIR_REVERSE
  bool raceSeen;
  // Per-lap window: timestamps of physical S2/S3 crossings since the last
  // start/finish. Direction is decided only when BOTH are present, by their
  // temporal order — independent of the lap calculation's sector output.
  unsigned long lapS2CrossingTime;
  unsigned long lapS3CrossingTime;
 
  DirectionDetector()
    : direction(DIR_UNKNOWN), raceSeen(false),
      lapS2CrossingTime(0), lapS3CrossingTime(0) {}
  void reset() {
    direction = DIR_UNKNOWN;
    raceSeen = false;
    lapS2CrossingTime = 0;
    lapS3CrossingTime = 0;
  }
  void onLineCrossing(int sectorNumber, unsigned long crossingTime);
  bool isReverse() const { return direction == DIR_REVERSE; }
  bool isResolved() const { return direction != DIR_UNKNOWN; }
};
 
class DovesLapTimer {
public:
  DovesLapTimer(double crossingThresholdMeters = 7, Stream *debugSerial = NULL);
 
  int loop(double currentLat, double currentLng, float currentAltitudeMeters, float currentSpeedKnots);
 
 
  bool isObtuseTriangle(double lat1, double lon1, double lat2, double lon2, double lat3, double lon3);
 
  bool insideLineThreshold(double driverLat, double driverLon, double crossingPointALat, double crossingPointALon, double crossingPointBLat, double crossingPointBLon);
 
  int pointOnSideOfLine(double driverLat, double driverLng, double pointALat, double pointALng, double pointBLat, double pointBLng);
  double pointLineSegmentDistance(double pointX, double pointY, double startX, double startY, double endX, double endY);
  double haversine(double lat1, double lon1, double lat2, double lon2);
  double haversine3D(double prevLat, double prevLng, double prevAlt, double currentLat, double currentLng, double currentAlt);
 
 
  void reset();
  void setStartFinishLine(double pointALat, double pointALng, double pointBLat, double pointBLng);
  void setSector2Line(double pointALat, double pointALng, double pointBLat, double pointBLng);
  void setSector3Line(double pointALat, double pointALng, double pointBLat, double pointBLng);
  void updateCurrentTime(unsigned long currentTimeMilliseconds);
  void forceLinearInterpolation();
  void forceCatmullRomInterpolation();
  bool getRaceStarted() const;
  bool getCrossing() const;
  unsigned long getCurrentLapStartTime() const;
  unsigned long getCurrentLapTime() const;
  unsigned long getLastLapTime() const;
  unsigned long getBestLapTime() const;
  float getCurrentLapOdometerStart() const;
  float getCurrentLapDistance() const;
  float getLastLapDistance() const;
  float getBestLapDistance() const;
  float getTotalDistanceTraveled() const;
  int getBestLapNumber() const;
  int getLaps() const;
  float getPaceDifference() const;
  float getCurrentSpeedKmh() const;
  float getCurrentSpeedMph() const;
 
 
  // Sector timing methods
 
  unsigned long getBestSector1Time() const;
  unsigned long getBestSector2Time() const;
  unsigned long getBestSector3Time() const;
  unsigned long getCurrentLapSector1Time() const;
  unsigned long getCurrentLapSector2Time() const;
  unsigned long getCurrentLapSector3Time() const;
  unsigned long getOptimalLapTime() const;
  int getBestSector1LapNumber() const;
  int getBestSector2LapNumber() const;
  int getBestSector3LapNumber() const;
  int getCurrentSector() const;
  bool areSectorLinesConfigured() const;
 
  // Direction detection methods
 
  int getDirection() const;
  bool isDirectionResolved() const;
 
private:
  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)...);
    }
  }
 
  bool checkStartFinish(double currentLat, double currentLng);
  bool checkSectorLine(double currentLat, double currentLng, double pointALat, double pointALng, double pointBLat, double pointBLng, bool& crossingFlag, int sectorNumber);
  LineDetectResult _detectLineCrossing(
      double currentLat, double currentLng,
      double pointALat, double pointALng,
      double pointBLat, double pointBLng,
      bool& crossingFlag,
      int lineLabel,
      double& outLat, double& outLng,
      unsigned long& outTime,
      double& outOdometer);
  void handleLineCrossing(unsigned long crossingTime, int sectorNumber);
  void updateBestSectors();
  double catmullRom(double p0, double p1, double p2, double p3, double t);
  double interpolateWeight(double distA, double distB, float speedA, float speedB);
  void interpolateCrossingPoint(double& crossingLat, double& crossingLng, unsigned long& crossingTime, double& crossingOdometer, double pointALat, double pointALng, double pointBLat, double pointBLng);
 
  Stream *_serial;
  DirectionDetector _directionDetector;
 
  unsigned long millisecondsSinceMidnight = 0;
  // Timing variables
  double crossingThresholdMeters;
  bool raceStarted = false;
  bool crossing = false;
  bool forceLinear = true;
  unsigned long currentLapStartTime = 0;
  unsigned long lastLapTime = 0;
  unsigned long bestLapTime = 0;
  float currentLapOdometerStart = 0.0;
  float lastLapDistance = 0.0;
  float bestLapDistance = 0.0;
  float currentSpeedkmh = 0.0;
  int bestLapNumber = 0;
  int laps = 0;
 
  // Sector timing state
  int currentSector = 0;  // 0=not started, 1/2/3=in sector
  unsigned long currentSectorStartTime = 0;
  bool crossingSector2 = false;
  bool crossingSector3 = false;
 
  // Current lap sector times (reset each lap)
  unsigned long currentLapSector1Time = 0;
  unsigned long currentLapSector2Time = 0;
  unsigned long currentLapSector3Time = 0;
 
  // Best sector times (persistent across laps)
  unsigned long bestSector1Time = 0;
  unsigned long bestSector2Time = 0;
  unsigned long bestSector3Time = 0;
 
  // Lap numbers that achieved best sectors
  int bestSector1LapNumber = 0;
  int bestSector2LapNumber = 0;
  int bestSector3LapNumber = 0;
 
  float totalDistanceTraveled = 0;
  float positionPrevAlt = 0.00;
  double positionPrevLat = 0.00;
  double positionPrevLng = 0.00;
  bool firstPositionReceived = false;  // Explicit flag for first GPS fix detection
 
  // Previous GPS fix snapshot (used as Catmull-Rom pre-crossing control point)
  double prevFixLat = 0;
  double prevFixLng = 0;
  unsigned long prevFixTime = 0;
  float prevFixOdometer = 0;
  float prevFixSpeedKmh = 0;
  bool hasPrevFix = false;
 
  double startFinishPointALat;
  double startFinishPointALng;
  double startFinishPointBLat;
  double startFinishPointBLng;
 
  // Sector 2 line coordinates
  double sector2PointALat;
  double sector2PointALng;
  double sector2PointBLat;
  double sector2PointBLng;
 
  // Sector 3 line coordinates
  double sector3PointALat;
  double sector3PointALng;
  double sector3PointBLat;
  double sector3PointBLng;
 
  // Sector line configuration flags
  bool sector2LineConfigured = false;
  bool sector3LineConfigured = false;
 
  // Earth's radius in meters
  static constexpr double radiusEarth = 6371.0 * 1000;
 
  // 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