#251 small fixes after ORP

lemon_pi/car/geometry.py

@@ -96,28 +96,14 @@ def get_point_on_heading(point, heading:float, d=0.05):
 
 
 def calc_intersect_heading(lat_long1, lat_long2, direction):
-    a = lat_long1
-    b = lat_long2
-    lat = 0
-    lon = 1
 
-    dL = b[lon] - a[lon]
-    X = cos(b[lat]) * sin(dL)
-    Y = cos(a[lat]) * sin(b[lat]) - sin(a[lat]) * cos(b[lat]) * cos(dL)
-    line_heading = degrees(arctan2(X, Y))
-
-    # print("got line_heading = {} want {}".format(line_heading, direction))
+    line_heading = heading_between_lat_long(lat_long2, lat_long1)
 
     line_heading += 360
     choice1 = (line_heading + 90)
     choice2 = (line_heading - 90)
     suggestion = DirectionMap[direction]
 
-    # print("suggested angle is close to {}".format(suggestion))
-    # print("choices are {} or {}".format(choice1 % 360, choice2 % 360))
-
-    # print("diff 1 / 2 = {} {}".format(abs(suggestion - choice1), abs(suggestion - choice2)))
-
     if abs(suggestion - choice1 % 360)  < abs(suggestion - choice2 % 360):
         # print("choosing 1")
         return choice1 % 360

lemon_pi/car/gps_reader.py

@@ -1,7 +1,6 @@
 from gps import *
 
 from dateutil import parser
-from datetime import datetime, timezone, timedelta
 from lemon_pi.car.display_providers import SpeedProvider, PositionProvider
 from lemon_pi.car.updaters import PositionUpdater
 from threading import Thread
@@ -39,9 +38,7 @@ def __init__(self, log_to_file=False):
         self.position_listener = None
         self.log = log_to_file
         self.finished = False
-        self.part_synced = False
         self.time_synced = False
-        self.sequential_timesync_errors = 0
         EnterTrackEvent.register_handler(self)
         LeaveTrackEvent.register_handler(self)
         ExitApplicationEvent.register_handler(self)
@@ -77,47 +74,17 @@ def run(self) -> None:
                             if gps_datetime.year < 2021:
                                 logger.debug("time wonky, ignoring")
                                 continue
-                            lag: timedelta = abs(datetime.now(tz=timezone.utc) - gps_datetime)
-
-                            if self.part_synced:
-                                # we have set the clock to the GPS a moment ago...whatever
-                                # the drift is due to the time taken to set the system clock
-                                logger.info("fine tuning clock from GPS by {}...".format(lag.seconds))
-                                epoch = int(gps_datetime.timestamp()) + lag.seconds
-                                subprocess.run(['sudo', 'date', '-u', '-s' '@{}'.format(epoch)])
-                                logger.info("time adjusted to {}".format(datetime.now()))
-                                self.part_synced = False
-                                self.time_synced = True
-                                self.sequential_timesync_errors = 0
-                                continue
-
-                            # we find there's a 1s difference between SKY and TPV messages that
-                            # come in.  We could ignore SKY, but for now we allow a 4s drift
-                            if lag.total_seconds() > 4:
-                                self.sequential_timesync_errors += 1
-                                if (lag.total_seconds() > 30 or self.sequential_timesync_errors > 30) \
-                                        and not self.time_synced:
-                                    logger.info("setting clock from GPS...")
-                                    epoch = int(gps_datetime.timestamp())
-                                    subprocess.run(['sudo', 'date', '-u', '-s' '@{}'.format(epoch)])
-                                    self.part_synced = True
-                                    logger.info("time corrected to {}".format(datetime.now()))
-                                else:
-                                    logger.warning("GPS Data time lag = {}  (skipping {}/30)".
-                                                   format(lag.total_seconds(), self.sequential_timesync_errors))
-                                    if lag.total_seconds() > 30 or self.sequential_timesync_errors > 30:
-                                        self.time_synced = False
-                                continue
-                            self.sequential_timesync_errors = 0
-                            self.time_synced = True
 
                         if session.fix.status == STATUS_NO_FIX:
                             # losing a gps fix doesn't emit a GPSDisconnected event ..
                             # we can look into whether it should or not once we have empirical information
                             logger.warning("no fix...awaiting")
+                            self.time_synced = False
                             time.sleep(0.5)
                             continue
 
+                        self.time_synced = True
+
                         if data['class'] == 'TPV':
                             # assuming its coming in m/s
                             if not math.isnan(session.fix.speed):
@@ -140,7 +107,8 @@ def run(self) -> None:
                                     start_time = time.time()
                                     try:
                                         self.position_listener.update_position(self.lat, self.long,
-                                                                            self.heading, time.time(), self.speed_mph)
+                                                                               self.heading, time.time(),
+                                                                               self.speed_mph)
                                     except Exception:
                                         logger.exception("issue with GPS listener.")
                                     finally:
@@ -189,7 +157,6 @@ def get_gps_position(self) -> GpsPosition:
         else:
             return None
 
-
     def is_working(self) -> bool:
         return self.working
 
@@ -210,14 +177,14 @@ def init_gps_connection(session: gps):
             raise Exception("no gps device")
         session.send('?WATCH={"enable":true,"json":true}')
 
-    def set_cycle(self, delay:float):
+    def set_cycle(self, delay: float):
         if UsbDetector.detected(UsbDevice.GPS):
             gps_device = UsbDetector.get(UsbDevice.GPS)
             result = subprocess.run(["gpsctl", "-c", str(delay), gps_device],
                                     stdout=subprocess.PIPE, stderr=subprocess.PIPE)
             logger.info(f"result of setting GPS cycle time = {result.returncode}")
 
-    def set_baud(self, baud:int):
+    def set_baud(self, baud: int):
         if UsbDetector.detected(UsbDevice.GPS):
             gps_device = UsbDetector.get(UsbDevice.GPS)
             result = subprocess.run(["gpsctl", "-s", str(baud), gps_device],
@@ -234,20 +201,19 @@ def set_baud(self, baud:int):
                         datefmt='%Y-%m-%d %H:%M:%S',
                         level=logging.DEBUG)
 
+
     class FileLogger(PositionUpdater):
 
         def __init__(self):
             self.file = open("traces/trace-{}.csv".format(int(time.time())), mode="w")
 
-        def update_position(self, lat:float, long:float, heading:float, time:float, speed:int) -> None:
+        def update_position(self, lat: float, long: float, heading: float, time: float, speed: int) -> None:
             self.file.write("{},{},{},{},{}\n".format(time, lat, long, heading, speed))
             self.file.flush()
 
+
     UsbDetector.init()
     tracker = GpsReader()
     tracker.set_cycle(1.0)
     tracker.register_position_listener(FileLogger())
     tracker.run()
-
-
-

lemon_pi/car/gui.py

@@ -257,9 +257,10 @@ def handle_event(self, event, **kwargs):
             return
 
         # go back to the fuel display if we complete a lap and it is not showing.
-        if event == CompleteLapEvent and not self.col3.visible and not settings.OBD_DISABLED:
-            self._col_display(3)
-            return
+        # PN 7/23/22 disabling as fuel display isn't useful right now
+        # if event == CompleteLapEvent and not self.col3.visible and not settings.OBD_DISABLED:
+        #     self._col_display(3)
+        #     return
 
         if event == OBDConnectedEvent:
             self.obd_image.on()

lemon_pi/car/main.py

@@ -43,6 +43,7 @@
     os.mkdir("logs")
 
 lap_file_handler = logging.FileHandler("logs/lap-logger-{}.csv".format(today))
+lap_file_handler.setFormatter(logging.Formatter('%(asctime)s.%(msecs)03d %(message)s'))
 lap_logger = logging.getLogger("lap-logger")
 lap_logger.addHandler(lap_file_handler)
 

lemon_pi/car/predictor.py

@@ -18,6 +18,7 @@
 ONE_DAY_IN_SECONDS = 24 * 3600
 TWO_DAYS_IN_SECONDS = ONE_DAY_IN_SECONDS * 2
 
+
 class PredictorState(Enum):
     # we are awaiting crossing the start finish line
     INIT = 1,
@@ -158,8 +159,9 @@ def _process_and_predict(self, lat, long, heading, time):
                 # all previous laps)
                 try:
                     self.current_predicted_time = self.gates[self.gate_index].predict_lap(elapsed_time)
-                    logger.info(
-                        f"predicted lap time = {self.current_predicted_time:.02f}")
+                    if self.gate_index % 10 == 0:
+                        logger.info(
+                            f"predicted lap time [gate = {self.gate_index}] = {self.current_predicted_time:.02f}")
                 except IndexError:
                     pass
                 self.gate_index += 1

lemon_pi/car/tests/lap_tracker_test.py

@@ -58,6 +58,29 @@ def test_pit_in_detection(self, leave_track_event):
         lt.update_position(35.4889, -119.5458, 90, now + 61, 50)
         leave_track_event.assert_called_once()
 
+    @patch("lemon_pi.car.event_defs.LeaveTrackEvent.emit")
+    def test_pit_in_detection2(self, leave_track_event):
+        bw = TrackLocation("ORP", "orp")
+        sf = Target("start-finish", (45.363790,-120.744707),(45.363816,-120.744277), "S")
+        bw.set_start_finish_target(sf)
+        pi = Target("pit-in", (45.365624, -120.744398), (45.365573, -120.744325), "SW")
+        bw.set_pit_in_target(pi)
+        lt = LapTracker(bw)
+        lt.on_track = True
+        now = time.time()
+        lt.update_position(45.365835,-120.744046667, 221, now + 60, 34)
+        lt.update_position(45.365785,-120.74411, 221, now + 61, 32)
+        lt.update_position(45.365736667,-120.744171667, 221, now + 62, 31)
+        lt.update_position(45.365691667,-120.74423, 221, now + 63, 30)
+        lt.update_position(45.365646667,-120.744285, 221, now + 64, 28)
+        lt.update_position(45.365605,-120.744338333, 221, now + 65, 27)
+        lt.update_position(45.365563333,-120.74439, 221, now + 66, 26)
+        lt.update_position(45.365523333,-120.744438333, 219, now + 67, 25)
+        lt.update_position(45.365483333,-120.744481667, 214, now + 68, 24)
+        lt.update_position(45.36544,-120.744518333, 208, now + 69, 24)
+        lt.update_position(45.365398333,-120.744546667, 203, now + 70, 23)
+        leave_track_event.assert_called_once()
+
     @patch("lemon_pi.car.event_defs.RadioSyncEvent.emit")
     def test_radio_sync_detection(self, radio_sync_event):
         bw = TrackLocation("bw", "bar")

lemon_pi/shared/meringue_comms.py

@@ -88,7 +88,6 @@ def __send_outbound_messages__(self):
                     stub.sendMessageFromPits(request=msg, timeout=10,
                                              metadata=build_auth_header(self.track_id, self.sender, self.key))
                 if isinstance(msg, ToPitMessage):
-                    logger.info("sending message to pit")
                     stub.sendMessageFromCar(request=msg, timeout=10,
                                             metadata=build_auth_header(self.track_id, self.sender, self.key))
                 logger.debug("sent message")