From 487756f0dc756708df30b22bd55066dfcbd3079b Mon Sep 17 00:00:00 2001 From: Marcell Zahoran Date: Mon, 6 Oct 2025 09:42:53 +0200 Subject: [PATCH 1/6] fix: when getting rtk position from live use it --- src/flockwave/server/ext/rtk/extension.py | 19 +++++++++++++++---- src/flockwave/server/ext/rtk/statistics.py | 9 +++++++++ 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/flockwave/server/ext/rtk/extension.py b/src/flockwave/server/ext/rtk/extension.py index d7ea7aa6..7f0e6763 100644 --- a/src/flockwave/server/ext/rtk/extension.py +++ b/src/flockwave/server/ext/rtk/extension.py @@ -5,7 +5,6 @@ from __future__ import annotations import json - from collections.abc import Sequence from contextlib import ExitStack from dataclasses import dataclass, field @@ -13,13 +12,14 @@ 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 @@ -369,6 +369,9 @@ def handle_RTK_SOURCE( else: preset_id, desired_preset = None, None + # Always reset fixed position when switching RTK source + self._survey_settings.position = None + self._request_preset_switch_later(desired_preset) self._last_preset_request_from_user = ( RTKPresetRequest(preset_id=preset_id) @@ -410,6 +413,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 +814,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}", diff --git a/src/flockwave/server/ext/rtk/statistics.py b/src/flockwave/server/ext/rtk/statistics.py index cddc167a..7fdefc6d 100644 --- a/src/flockwave/server/ext/rtk/statistics.py +++ b/src/flockwave/server/ext/rtk/statistics.py @@ -117,6 +117,11 @@ def notify(self, packet: RTCMV3Packet) -> None: self.position_ecef = position self._antenna_position_timestamp = monotonic() + def set_from_ecef(self, position: ECEFCoordinate) -> None: + 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 antenna position packet for the last 30 seconds. @@ -420,6 +425,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 From 4d8e6a76bf0f45a34073c1c935baa0e6ba34a7e4 Mon Sep 17 00:00:00 2001 From: zarcell Date: Tue, 28 Oct 2025 14:44:48 +0100 Subject: [PATCH 2/6] fix: use newly added function + docs --- src/flockwave/server/ext/rtk/statistics.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/flockwave/server/ext/rtk/statistics.py b/src/flockwave/server/ext/rtk/statistics.py index 7fdefc6d..07f3b3a8 100644 --- a/src/flockwave/server/ext/rtk/statistics.py +++ b/src/flockwave/server/ext/rtk/statistics.py @@ -113,11 +113,14 @@ 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() From e93fbd7c377e65df1f0462fca3de51e3239941a7 Mon Sep 17 00:00:00 2001 From: Marcell Zahoran Date: Tue, 7 Oct 2025 15:10:47 +0200 Subject: [PATCH 3/6] chore: ruff format --- .../server/command_handlers/color.py | 10 ++--- src/flockwave/server/message_hub.py | 42 +++++++++---------- src/flockwave/server/model/devices.py | 2 +- src/flockwave/server/registries/channels.py | 6 +-- .../server/registries/connections.py | 5 ++- 5 files changed, 32 insertions(+), 33 deletions(-) diff --git a/src/flockwave/server/command_handlers/color.py b/src/flockwave/server/command_handlers/color.py index fdc46168..2a2f078f 100644 --- a/src/flockwave/server/command_handlers/color.py +++ b/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()`. diff --git a/src/flockwave/server/message_hub.py b/src/flockwave/server/message_hub.py index 36f10ad8..94f1fe6e 100644 --- a/src/flockwave/server/message_hub.py +++ b/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: diff --git a/src/flockwave/server/model/devices.py b/src/flockwave/server/model/devices.py index c424d139..dcf9dd81 100644 --- a/src/flockwave/server/model/devices.py +++ b/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 diff --git a/src/flockwave/server/registries/channels.py b/src/flockwave/server/registries/channels.py index 28e83722..b7b4f93d 100644 --- a/src/flockwave/server/registries/channels.py +++ b/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 diff --git a/src/flockwave/server/registries/connections.py b/src/flockwave/server/registries/connections.py index 834c2731..93a66941 100644 --- a/src/flockwave/server/registries/connections.py +++ b/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 From 878c8f66e7b85aa28a24300f7098120eba68cc53 Mon Sep 17 00:00:00 2001 From: Marcell Zahoran Date: Mon, 6 Oct 2025 09:42:53 +0200 Subject: [PATCH 4/6] fix: when getting rtk position from live use it --- src/flockwave/server/ext/rtk/statistics.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/flockwave/server/ext/rtk/statistics.py b/src/flockwave/server/ext/rtk/statistics.py index 07f3b3a8..37f29d42 100644 --- a/src/flockwave/server/ext/rtk/statistics.py +++ b/src/flockwave/server/ext/rtk/statistics.py @@ -125,6 +125,11 @@ def set_from_ecef(self, position: ECEFCoordinate) -> None: self.position_ecef = position self._antenna_position_timestamp = monotonic() + def set_from_ecef(self, position: ECEFCoordinate) -> None: + 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 antenna position packet for the last 30 seconds. From 07ff95f2966643e6c4462c06bc6922fa5be0789d Mon Sep 17 00:00:00 2001 From: zarcell Date: Wed, 19 Nov 2025 17:08:05 +0100 Subject: [PATCH 5/6] fix: restore fixed position if there is one in the config --- src/flockwave/server/ext/rtk/extension.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/flockwave/server/ext/rtk/extension.py b/src/flockwave/server/ext/rtk/extension.py index 7f0e6763..e0db5eeb 100644 --- a/src/flockwave/server/ext/rtk/extension.py +++ b/src/flockwave/server/ext/rtk/extension.py @@ -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,8 +372,8 @@ def handle_RTK_SOURCE( else: preset_id, desired_preset = None, None - # Always reset fixed position when switching RTK source - self._survey_settings.position = 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 = ( From 4014dfabef289466e79123b3bdcf5e3163c91a84 Mon Sep 17 00:00:00 2001 From: zarcell Date: Fri, 21 Nov 2025 13:30:05 +0100 Subject: [PATCH 6/6] fix: somehow duplicated function --- src/flockwave/server/ext/rtk/statistics.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/flockwave/server/ext/rtk/statistics.py b/src/flockwave/server/ext/rtk/statistics.py index 37f29d42..07f3b3a8 100644 --- a/src/flockwave/server/ext/rtk/statistics.py +++ b/src/flockwave/server/ext/rtk/statistics.py @@ -125,11 +125,6 @@ def set_from_ecef(self, position: ECEFCoordinate) -> None: self.position_ecef = position self._antenna_position_timestamp = monotonic() - def set_from_ecef(self, position: ECEFCoordinate) -> None: - 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 antenna position packet for the last 30 seconds.