Demo mode is activated even with GPS module. add COURSE_DIVIDE_FACTOR…

… to dummy.py
hishizuka · Dec 18, 2023 · f6f7371 · f6f7371
1 parent 5d478b5
commit f6f7371
Show file tree

Hide file tree

Showing 6 changed files with 77 additions and 70 deletions.
diff --git a/modules/sensor/gps/__init__.py b/modules/sensor/gps/__init__.py
@@ -17,4 +17,4 @@
 else:
     SensorGPS = Dummy_GPS
 
-app_logger.info(f"{SensorGPS.__name__} chosen")
+app_logger.info(f"GPS ({SensorGPS.__name__})")
diff --git a/modules/sensor/gps/adafruit_uart.py b/modules/sensor/gps/adafruit_uart.py
@@ -18,9 +18,6 @@
     except:
         pass
 
-if _SENSOR_GPS_ADAFRUIT_UART:
-    app_logger.info("GPS_ADAFRUIT_UART")
-
 
 class Adafruit_GPS(AbstractSensorGPS):
     NULL_VALUE = None
@@ -41,6 +38,10 @@ async def quit(self):
 
     # experimental code
     async def update(self):
+        if self.config.G_DUMMY_OUTPUT:
+            await self.output_dummy()
+            return
+
         g = self.adafruit_gps
 
         try:

diff --git a/modules/sensor/gps/base.py b/modules/sensor/gps/base.py
@@ -311,3 +311,7 @@ def get_utc_time(self, gps_time):
 
         if not self.is_time_modified:
             self.is_time_modified = set_time(gps_time)
+
+    async def output_dummy(self):
+        from .dummy import Dummy_GPS
+        await Dummy_GPS(self.config, self.values).update()
diff --git a/modules/sensor/gps/dummy.py b/modules/sensor/gps/dummy.py
@@ -9,6 +9,7 @@
 
 class Dummy_GPS(AbstractSensorGPS):
     LOG_SPEED = 5
+    COURSE_DIVIDE_FACTOR = 500
     COURSE_RAND_FACTOR = 0.5  # np.random.randint(0,10)/10
 
     @property
@@ -54,70 +55,70 @@ def set_position_from_course(self, course, idx):
             self.values["track_str"] = get_track_str(self.values["track"])
 
     async def update(self):
-        if self.config.G_DUMMY_OUTPUT:
-            course_i = pre_course_i = 0
-
-            try:
-                while True:
-                    await self.sleep()
+        if not self.config.G_DUMMY_OUTPUT:
+            return
+        course_i = pre_course_i = 0
+
+        try:
+            while True:
+                await self.sleep()
+
+                if self.config.logger is None or (
+                    not self.config.logger.course.is_set
+                    and self.config.logger.position_log.shape[0] == 0
+                ):
+                    continue
+
+                # unit: m/s
+                # self.values['speed'] = random.randint(1,6) * 3.6
+                self.values["speed"] = np.random.randint(13, 83) / 10  # 5 - 30km/h
+
+                self.values["pre_lat"] = self.values["lat"]
+                self.values["pre_lon"] = self.values["lon"]
+                self.values["pre_track"] = self.values["track"]
+
+                # generate dummy position from log
+                if self.config.logger.position_log.shape[0] > 0:
+                    self.set_position_from_log(
+                        self.config.logger.position_log[course_i]
+                    )
 
-                    if self.config.logger is None or (
-                        not self.config.logger.course.is_set
-                        and self.config.logger.position_log.shape[0] == 0
-                    ):
+                    if course_i == pre_course_i:
+                        course_i += 1 * self.LOG_SPEED
                         continue
-
-                    # self.values['track_str'] = "-"
-                    # unit: m/s
-                    # self.values['speed'] = random.randint(1,6) * 3.6
-                    self.values["speed"] = np.random.randint(13, 83) / 10  # 5 - 30km/h
-
-                    self.values["pre_lat"] = self.values["lat"]
-                    self.values["pre_lon"] = self.values["lon"]
-                    self.values["pre_track"] = self.values["track"]
-
-                    # generate dummy position from log
-                    if self.config.logger.position_log.shape[0] > 0:
-                        self.set_position_from_log(
-                            self.config.logger.position_log[course_i]
-                        )
-
-                        if course_i == pre_course_i:
-                            course_i += 1 * self.LOG_SPEED
-                            continue
-                        else:
-                            pre_course_i = course_i
-                            course_i += 1 * self.LOG_SPEED
-                            if course_i >= len(self.config.logger.position_log):
-                                course_i = pre_course_i = 0
-                                continue
-
-                    # from course
                     else:
-                        # TODO No need to do this for each loop, unless the course can be changed in between  ?
-                        course_n = len(self.config.logger.course.latitude)
+                        pre_course_i = course_i
+                        course_i += 1 * self.LOG_SPEED
+                        if course_i >= len(self.config.logger.position_log):
+                            course_i = pre_course_i = 0
+                            continue
 
-                        self.set_position_from_course(
-                            self.config.logger.course, course_i
-                        )
+                # from course
+                else:
+                    # TODO No need to do this for each loop, unless the course can be changed in between  ?
+                    course_n = len(self.config.logger.course.latitude)
 
-                        pre_course_i = course_i
-                        course_i += int(course_n / 200) + 1
-                        if course_i >= course_n:
-                            pre_course_i = 0
-                            course_i = course_i % course_n
-
-                    self.config.logger.course.get_index(
-                        self.values["lat"],
-                        self.values["lon"],
-                        self.values["track"],
-                        self.config.G_GPS_SEARCH_RANGE,
-                        self.config.G_GPS_ON_ROUTE_CUTOFF,
-                        self.azimuth_cutoff,
+                    self.set_position_from_course(
+                        self.config.logger.course, course_i
                     )
 
-                    self.values["timestamp"] = datetime.now()
-                    self.get_sleep_time(self.config.G_GPS_INTERVAL)
+                    pre_course_i = course_i
+                    course_i += int(course_n / self.COURSE_DIVIDE_FACTOR) + 1
+                    if course_i >= course_n:
+                        pre_course_i = 0
+                        course_i = course_i % course_n
+
+                self.config.logger.course.get_index(
+                    self.values["lat"],
+                    self.values["lon"],
+                    self.values["track"],
+                    self.config.G_GPS_SEARCH_RANGE,
+                    self.config.G_GPS_ON_ROUTE_CUTOFF,
+                    self.azimuth_cutoff,
+                )
+
+                self.values["timestamp"] = datetime.now()
+                self.get_sleep_time(self.config.G_GPS_INTERVAL)
 
-            except asyncio.CancelledError:
-                pass
+        except asyncio.CancelledError:
+            pass
diff --git a/modules/sensor/gps/gpsd.py b/modules/sensor/gps/gpsd.py
@@ -22,9 +22,6 @@
     except:
         pass
 
-if _SENSOR_GPS_GPSD:
-    app_logger.info("GPS")
-
 
 class GPSD(AbstractSensorGPS):
     gps_thread = None
@@ -51,6 +48,10 @@ async def quit(self):
         self.gps_thread.stop()
 
     async def update(self):
+        if self.config.G_DUMMY_OUTPUT:
+            await self.output_dummy()
+            return
+
         g = self.gps_thread.data_stream
         try:
             while True:

diff --git a/modules/sensor/gps/i2c.py b/modules/sensor/gps/i2c.py
@@ -17,10 +17,6 @@
     app_logger.exception("Failed to init GPS_I2C")
 
 
-if _SENSOR_GPS_I2C:
-    app_logger.info("GPS_I2C")
-
-
 class GPS_I2C(AbstractSensorGPS):
     NULL_VALUE = None
 
@@ -32,6 +28,10 @@ def sensor_init(self):
         super().sensor_init()
 
     async def update(self):
+        if self.config.G_DUMMY_OUTPUT:
+            await self.output_dummy()
+            return
+
         g = self.i2c_gps
 
         try: