Server side changes for using saved fix coordinates with for the RTK base

src/flockwave/server/command_handlers/color.py

@@ -65,12 +65,10 @@ async def _color_command_handler(
         return "Color override turned off"
 
 
-def create_color_command_handler() -> (
-    Callable[
-        [UAVDriver, UAV, Optional[Union[str, int]], Optional[int], Optional[int]],
-        Awaitable[str],
-    ]
-):
+def create_color_command_handler() -> Callable[
+    [UAVDriver, UAV, Optional[Union[str, int]], Optional[int], Optional[int]],
+    Awaitable[str],
+]:
     """Creates a generic async command handler function that allows the user to
     set the color of the LED lights on the UAV, assuming that the UAV
     has an async or sync method named `set_led_color()`.

src/flockwave/server/ext/rtk/extension.py

@@ -5,21 +5,21 @@
 from __future__ import annotations
 
 import json
-
 from collections.abc import Sequence
 from contextlib import ExitStack
 from dataclasses import dataclass, field
 from fnmatch import fnmatch
 from functools import partial
 from pathlib import Path
 from time import monotonic
+from typing import Any, Callable, ClassVar, Iterator, Optional, Union, cast
+
 from trio import CancelScope, open_memory_channel, open_nursery, sleep
 from trio.abc import SendChannel
 from trio_util import AsyncBool, periodic
-from typing import Callable, cast, Any, ClassVar, Iterator, Optional, Union
 
 from flockwave.channels import ParserChannel
-from flockwave.connections import create_connection, RWConnection
+from flockwave.connections import RWConnection, create_connection
 from flockwave.gps.enums import GNSSType
 from flockwave.gps.formatting import format_gps_coordinate_as_nmea_gga_message
 from flockwave.gps.rtk import RTKMessageSet, RTKSurveySettings
@@ -49,8 +49,8 @@
 from .clock_sync import GPSClockSynchronizationValidator
 from .enums import MessageSet, RTKConfigurationPresetType
 from .preset import (
-    RTKConfigurationPreset,
     ALLOWED_FORMATS,
+    RTKConfigurationPreset,
     describe_format,
 )
 from .registry import RTKPresetRegistry
@@ -99,6 +99,7 @@ class RTKExtension(Extension):
     _statistics: RTKStatistics
     _survey_settings: RTKSurveySettings
     _tx_queue: Optional[SendChannel] = None
+    _config_fixed_position: Optional[Any] = None
 
     def __init__(self):
         """Constructor."""
@@ -223,6 +224,8 @@ def configure(self, configuration: dict[str, Any]) -> None:
 
             position = self._survey_settings.position
             if position is not None:
+                # Store the fixed position from config so we can restore it when switching RTK sources.
+                self._config_fixed_position = position
                 coord = ECEFToGPSCoordinateTransformation().to_gps(position).format()
                 self.log.info(
                     f"Base station is fixed at {coord} (accuracy: {accuracy}m)"
@@ -369,6 +372,9 @@ def handle_RTK_SOURCE(
             else:
                 preset_id, desired_preset = None, None
 
+            # Reset fixed position when switching RTK source, but restore from config if available
+            self._survey_settings.position = self._config_fixed_position
+
             self._request_preset_switch_later(desired_preset)
             self._last_preset_request_from_user = (
                 RTKPresetRequest(preset_id=preset_id)
@@ -410,6 +416,10 @@ def handle_RTK_SURVEY(self, message: FlockwaveMessage, sender, hub: MessageHub):
                 error = "Settings object missing or invalid"
 
             if error is None:
+                position = self._survey_settings.position
+                if position is not None:
+                    # Populate antenna.position immediately so RTK-STAT reflects it
+                    self._statistics.set_antenna_position_from_ecef(position)
                 self._request_survey()
 
             return hub.acknowledge(message, outcome=error is None, reason=error)
@@ -807,6 +817,10 @@ async def _run_survey(self, preset, connection, *, task_status) -> None:
                             self._statistics.set_to_fixed_with_accuracy(
                                 accuracy_cm / 100.0
                             )
+                            if position is not None:
+                                self._statistics.set_antenna_position_from_ecef(
+                                    position
+                                )
                         else:
                             self.log.error(
                                 f"Failed to configure {preset.title!r}",

src/flockwave/server/ext/rtk/statistics.py

@@ -113,9 +113,17 @@ def notify(self, packet: RTCMV3Packet) -> None:
 
         position = getattr(packet, "position", None)
         if position is not None:
-            self.position = _ecef_to_gps.to_gps(position)
-            self.position_ecef = position
-            self._antenna_position_timestamp = monotonic()
+            self.set_from_ecef(position)
+
+    def set_from_ecef(self, position: ECEFCoordinate) -> None:
+        """Sets the antenna position from an ECEF coordinate.
+        Computes and stores the corresponding GPS coordinate and refreshes
+        the internal timestamp used to track the age of the last antenna
+        position observation.
+        """
+        self.position = _ecef_to_gps.to_gps(position)
+        self.position_ecef = position
+        self._antenna_position_timestamp = monotonic()
 
     def _forget_old_antenna_position_if_needed(self) -> None:
         """Clears the position of the antenna we have not received another
@@ -420,6 +428,10 @@ def set_to_fixed_with_accuracy(self, accuracy: float) -> None:
         """
         self._survey_status.set_to_fixed_with_accuracy(accuracy)
 
+    def set_antenna_position_from_ecef(self, position: ECEFCoordinate) -> None:
+        """Sets the antenna position directly from an ECEF coordinate."""
+        self._antenna_information.set_from_ecef(position)
+
     @contextmanager
     def use(self):
         """Context manager that clears the statistics object upon entering

src/flockwave/server/message_hub.py

@@ -242,9 +242,9 @@ async def broadcast_message(self, message: FlockwaveNotification) -> Request:
             the request object that identifies this message in the outbound
             message queue. It can be used to wait until the message is delivered
         """
-        assert isinstance(
-            message, FlockwaveNotification
-        ), "only notifications may be broadcast"
+        assert isinstance(message, FlockwaveNotification), (
+            "only notifications may be broadcast"
+        )
 
         request = Request(message)
         await self._queue_tx.send(request)
@@ -379,9 +379,9 @@ def enqueue_broadcast_message(self, message: FlockwaveNotification) -> None:
         Parameters:
             message: the notification to enqueue
         """
-        assert isinstance(
-            message, FlockwaveNotification
-        ), "only notifications may be broadcast"
+        assert isinstance(message, FlockwaveNotification), (
+            "only notifications may be broadcast"
+        )
 
         # Don't return the request here because it is not guaranteed that it
         # ends up in the queue; it may be dropped
@@ -423,9 +423,9 @@ def enqueue_message(
                 message, in_response_to=in_response_to
             )
         if to is None:
-            assert isinstance(
-                message, FlockwaveNotification
-            ), "broadcast messages cannot be sent in response to a particular message"
+            assert isinstance(message, FlockwaveNotification), (
+                "broadcast messages cannot be sent in response to a particular message"
+            )
             return self.enqueue_broadcast_message(message)
         else:
             # Don't return the request here because it is not guaranteed that it
@@ -549,12 +549,12 @@ def _commit_broadcast_methods(
         """Calculates the list of methods to call when the message hub
         wishes to broadcast a message to all the connected clients.
         """
-        assert (
-            self._client_registry is not None
-        ), "message hub does not have a client registry yet"
-        assert (
-            self._channel_type_registry is not None
-        ), "message hub does not have a channel type registry yet"
+        assert self._client_registry is not None, (
+            "message hub does not have a client registry yet"
+        )
+        assert self._channel_type_registry is not None, (
+            "message hub does not have a channel type registry yet"
+        )
 
         result = []
         clients_for = self._client_registry.client_ids_for_channel_type
@@ -832,9 +832,9 @@ async def send_message(
             )
 
         if to is None:
-            assert isinstance(
-                message, FlockwaveNotification
-            ), "broadcast messages cannot be sent in response to a particular message"
+            assert isinstance(message, FlockwaveNotification), (
+                "broadcast messages cannot be sent in response to a particular message"
+            )
             return await self.broadcast_message(message)
         else:
             request = Request(message, to=to, in_response_to=in_response_to)
@@ -1068,9 +1068,9 @@ async def _send_message(
         in_response_to: Optional[FlockwaveMessage] = None,
         done: Optional[Callable[[], None]] = None,
     ):
-        assert (
-            self._client_registry is not None
-        ), "message hub does not have a client registry yet"
+        assert self._client_registry is not None, (
+            "message hub does not have a client registry yet"
+        )
 
         if not isinstance(to, Client):
             try:

src/flockwave/server/model/devices.py

@@ -338,7 +338,7 @@ def _add_child(self, id: str, node: C) -> C:
             self.children = {}
         if id in self.children:
             raise ValueError(
-                "another child node already exists with " "ID={0!r}".format(id)
+                "another child node already exists with ID={0!r}".format(id)
             )
         self.children[id] = node
 

src/flockwave/server/registries/channels.py

@@ -140,9 +140,9 @@ def create_channel_for(self, channel_id):
                 type.
         """
         result = self._entries[channel_id].factory()
-        assert isinstance(
-            result, CommunicationChannel
-        ), "communication channel factory did not return a CommunicationChannel"
+        assert isinstance(result, CommunicationChannel), (
+            "communication channel factory did not return a CommunicationChannel"
+        )
         return result
 
     @property

src/flockwave/server/registries/connections.py

@@ -80,8 +80,9 @@ def add(
         """
         if name in self:
             raise KeyError(
-                "another connection is already registered "
-                "with this name: {0!r}".format(name)
+                "another connection is already registered with this name: {0!r}".format(
+                    name
+                )
             )
 
         purpose = purpose if purpose is not None else ConnectionPurpose.other