dotbot/calibration: push and bake stations by mask, each with its rectangle

dotbot/calibration/lighthouse2.py

@@ -379,8 +379,10 @@ def reprojection_residual_mm(
 # --- Wire -------------------------------------------------------------------
 
 # One calibration message per station, `swrmt_lh2_calibration_data_t` in the
-# swarmit netcore: count, index, the matrix, then the site fields every
-# message of a push repeats. swarmit's `helpers.py` packs the same bytes; the
+# swarmit netcore: the station mask, the station's index, its matrix and its
+# rectangle, then the site name and calibration id every message of a push
+# repeats. The net core commits once every station of the mask has arrived,
+# keyed by mask and id. swarmit's `helpers.py` packs the same bytes; the
 # fixture test in each repo pins them.
 LH2_SITE_NAME_BYTES = 16
 LH2_CALIBRATION_ID_BYTES = 8
@@ -483,11 +485,10 @@ def station_label(index: int) -> str:
 
 
 def pushable_stations(calibration: Calibration) -> list[StationSolution]:
-    """The stations in index order, refused when a robot would mis-key them.
+    """The stations in index order, refused when a robot could not hold them.
 
-    The receiver stores a matrix at its index and takes the count as "slots
-    0 to count - 1 are valid", so a gap in the numbering would leave a slot
-    the count claims and nothing filled.
+    A robot stores each station in the slot of its index, 0 to 15, so any
+    set of indices in that range will do.
     """
     ordered = sorted(calibration.stations, key=lambda s: s.index)
     if not ordered:
@@ -497,30 +498,30 @@ def pushable_stations(calibration: Calibration) -> list[StationSolution]:
             f"{len(ordered)} stations exceeds the LH2 limit "
             f"({LH2_BASESTATION_COUNT_MAX})"
         )
-    if [s.index for s in ordered] != list(range(len(ordered))):
-        got = ", ".join(str(s.index) for s in ordered)
+    outside = [s.index for s in ordered if not 0 <= s.index < LH2_BASESTATION_COUNT_MAX]
+    if outside:
         raise ValueError(
-            f"stations must be numbered from zero without gaps to be pushed, got {got}"
+            f"station index {outside} is outside 0 to {LH2_BASESTATION_COUNT_MAX - 1}"
         )
     return ordered
 
 
 def site_fields_as_bytes(calibration: Calibration) -> bytes:
-    """valid_mm, site name and id: the 40 bytes after every matrix."""
-    return (
-        valid_mm_as_bytes(calibration.valid_mm)
-        + site_name_as_bytes(calibration.site.name)
-        + calibration_id_as_bytes(pushed_id(calibration))
+    """Site name and id: the 24 bytes that close every message and the page."""
+    return site_name_as_bytes(calibration.site.name) + calibration_id_as_bytes(
+        pushed_id(calibration)
     )
 
 
 def calibration_messages(calibration: Calibration) -> list[bytes]:
     """One 84-byte calibration message per station, in index order."""
     stations = pushable_stations(calibration)
+    mask = station_mask(calibration)
     site_fields = site_fields_as_bytes(calibration)
     return [
-        struct.pack("<II", len(stations), station.index)
+        struct.pack("<II", mask, station.index)
         + homography_as_float32(station.homography)
+        + valid_mm_as_bytes(station.valid_mm)
         + site_fields
         for station in stations
     ]

dotbot/calibration/push.py

@@ -25,7 +25,7 @@
 
 from dotbot.calibration.lighthouse2 import Calibration, pushed_id
 
-DEVICE_INFO_VERSION_MIN = 3
+DEVICE_INFO_VERSION_MIN = 4
 # Seconds a pushed robot gets to reset, rejoin and report the pushed id.
 PUSH_REJOIN_TIMEOUT = 30.0
 PUSH_POLL_INTERVAL = 1.0

dotbot/calibration/simulated.py

@@ -115,7 +115,7 @@ def status(self) -> dict[str, Any]:
                 self.pushed[-1][:LH2_CALIBRATION_MESSAGE_BYTES]
             )
         info = SimpleNamespace(
-            info_version=3, lh2_site_name=site, lh2_calibration_id=calibration_id
+            info_version=4, lh2_site_name=site, lh2_calibration_id=calibration_id
         )
         return {self.device: SimpleNamespace(info_gen=1, info=info)}
 

dotbot/cli/swarm_lh2.py

@@ -754,8 +754,16 @@ def _collect_spin(
         "report another one."
     ),
 )
+@click.option(
+    "--dry-run",
+    is_flag=True,
+    help=(
+        "Print the messages the push would send, one per station, and send "
+        "nothing; needs no swarm connection."
+    ),
+)
 @click.pass_context
-def _push(ctx, calibration, conn, swarm_id, site_name, site_changed):
+def _push(ctx, calibration, conn, swarm_id, site_name, site_changed, dry_run):
     from dotbot.calibration.lighthouse2 import (
         read_calibration_file,
         resolve_calibration_path,
@@ -767,17 +775,45 @@ def _push(ctx, calibration, conn, swarm_id, site_name, site_changed):
         loaded = read_calibration_file(path)
     except ValueError as exc:
         raise click.ClickException(str(exc)) from exc
+    if dry_run:
+        _print_messages(loaded)
+        return
     client = _swarmit_client(ctx, conn, swarm_id)
     with client:
         _gated_push(client, loaded, site_changed=site_changed, devices=_devices(ctx))
 
 
+def _print_messages(calibration):
+    """What a push of `calibration` sends, message by message."""
+    from dotbot.calibration.lighthouse2 import (
+        calibration_messages,
+        station_label,
+        station_mask,
+    )
+
+    try:
+        messages = calibration_messages(calibration)
+    except ValueError as exc:
+        raise click.ClickException(str(exc)) from exc
+    stations = sorted(calibration.stations, key=lambda s: s.index)
+    click.echo(
+        f"Would send {len(messages)} message(s), {sum(map(len, messages))} B: "
+        f"id {calibration.id8}, site {calibration.site.name}, station mask "
+        f"0x{station_mask(calibration):04X}"
+    )
+    for station, message in zip(stations, messages):
+        click.echo(
+            f"  {station_label(station.index)}, rectangle {list(station.valid_mm)}: "
+            f"{message.hex()}"
+        )
+
+
 def _gated_push(client, calibration, site_changed=False, devices=None, stop=False):
     """Check the robots' device info, push, and print the stale-id worklist.
 
     With `stop`, the named `devices` still in their app are stopped first.
     """
-    from dotbot.calibration.lighthouse2 import calibration_payload
+    from dotbot.calibration.lighthouse2 import calibration_payload, station_label
     from dotbot.calibration.push import (
         PushRefused,
         gate_push,
@@ -810,8 +846,12 @@ def _gated_push(client, calibration, site_changed=False, devices=None, stop=Fals
             f"{len(check.other_site)} robot(s) move to site "
             f"{calibration.site.name} (--site-changed)."
         )
+    stations = ", ".join(
+        station_label(s.index)
+        for s in sorted(calibration.stations, key=lambda s: s.index)
+    )
     click.echo(
-        f"Sending {len(calibration.stations)} calibration matrix/matrices "
+        f"Sending {stations} "
         f"({len(payload)} B, id {calibration.id8}, site {calibration.site.name}) "
         f"to the swarm; {len(check.stale)} robot(s) hold another id..."
     )

dotbot/console-web/dev/fake_swarmit_server.py

@@ -174,17 +174,19 @@ def device_info(addr: str) -> dict:
     seed = _seed(addr)
     name, digest, size = IMAGES[seed % len(IMAGES)]
     # A minority are uncalibrated, which is the state an operator acts on.
-    homographies = 0 if seed % 9 == 0 else (2 if seed % 5 == 0 else 1)
-    flags = LH2_FLAG_VALID | LH2_FLAG_FROM_FLASH if homographies else 0
-    noun = "basestation" if homographies == 1 else "basestations"
+    mask = 0 if seed % 9 == 0 else (0b101 if seed % 5 == 0 else 0b1)
+    flags = LH2_FLAG_VALID | LH2_FLAG_FROM_FLASH if mask else 0
+    stations = [i for i in range(16) if mask >> i & 1]
     summary = (
         "uncalibrated"
-        if not homographies
-        else f"{homographies} {noun} (valid, from flash)"
+        if not mask
+        else f"station{'s' if len(stations) > 1 else ''} "
+        f"{', '.join(map(str, stations))} (channel{'s' if len(stations) > 1 else ''} "
+        f"{', '.join(str(i + 1) for i in stations)}; valid, from flash)"
     )
     return {
         # A few run sandbox firmware too old for this host's calibrations.
-        "info_version": 2 if seed % 7 == 0 else 3,
+        "info_version": 3 if seed % 7 == 0 else 4,
         "info_gen": 4,
         "boot_count": 2 + seed % 30,
         "uptime_s": 60 + seed % 9000,
@@ -198,10 +200,10 @@ def device_info(addr: str) -> dict:
         "image_digest": digest,
         "image_name": name,
         "image_version": "",
-        "lh2_homography_count": homographies,
+        "lh2_station_mask": mask,
         "lh2_flags": flags,
-        "lh2_site_name": FAKE_SITE if homographies else "",
-        "lh2_calibration_id": FAKE_CALIBRATION_ID if homographies else "",
+        "lh2_site_name": FAKE_SITE if mask else "",
+        "lh2_calibration_id": FAKE_CALIBRATION_ID if mask else "",
         "lh2_summary": summary,
         "raw": "8f0104" + f"{seed:08x}" * 4,
     }

dotbot/console-web/src/fleetViews.test.tsx

@@ -30,7 +30,7 @@ const site: Site = {
 };
 
 const info = (over: Partial<SwarmitDeviceInfo> = {}): SwarmitDeviceInfo => ({
-  info_version: 3,
+  info_version: 4,
   bl_version: "1.25.0",
   net_version: "1.25.0",
   boot_count: 3,

dotbot/console-web/src/localization.test.ts

@@ -56,7 +56,7 @@ const bot = (id: string, calibrationId?: string): UnifiedBot =>
       calibrationId === undefined
         ? null
         : ({
-            info: { info_version: 3, lh2_calibration_id: calibrationId },
+            info: { info_version: 4, lh2_calibration_id: calibrationId },
           } as unknown as SwarmitNode),
   }) as UnifiedBot;
 

dotbot/console-web/src/localization.ts

@@ -39,7 +39,7 @@ export interface Coverage {
 }
 
 /** The device-info version of firmware that takes this host's calibrations; older needs a reflash. */
-export const DEVICE_INFO_VERSION_MIN = 3;
+export const DEVICE_INFO_VERSION_MIN = 4;
 
 /** Who carries `savedId`, from the calibration id each robot's device info reports. */
 export function calibrationCoverage(bots: UnifiedBot[], savedId: string): Coverage {

dotbot/console-web/src/pushTarget.test.tsx

@@ -76,7 +76,7 @@ const bot = (id: string, calibrationId: string): UnifiedBot =>
   ({
     id,
     swarmit: {
-      info: { info_version: 3, lh2_calibration_id: calibrationId },
+      info: { info_version: 4, lh2_calibration_id: calibrationId },
     } as unknown as SwarmitNode,
   }) as UnifiedBot;
 

dotbot/console-web/src/types.ts

@@ -266,7 +266,8 @@ export interface SwarmitDeviceInfo {
   image_name: string;
   image_version: string;
   image_digest: string;
-  lh2_homography_count?: number;
+  // Bit i set: the bot holds station i's homography (channel i + 1).
+  lh2_station_mask?: number;
   lh2_flags?: number;
   // Device info v2: the site and calibration id the bot holds, "" for none.
   lh2_site_name?: string;

dotbot/dotbot_simulator.py

@@ -61,8 +61,8 @@
 # How far apart `--robots N` puts a generated fleet
 FLEET_PITCH_MM = 200
 # The stations a simulated robot holds when neither its world file nor the
-# controller's calibration says: all eight
-CALIBRATED_ALL = 0xFF
+# controller's calibration says: all sixteen
+CALIBRATED_ALL = 0xFFFF
 # The heading of every robot of a generated fleet: up (-y), 0 facing +y
 FLEET_FACING = 180
 
@@ -113,7 +113,7 @@ class SimulatedDotBotSettings(BaseModel):
     estimator starts out tracking; without one the robot faces +y and starts
     with no heading, as a real robot does from boot. `calibrated` is the
     bitmask of stations the robot holds a homography for; without one it
-    holds the stations of the controller's calibration, all eight when none
+    holds the stations of the controller's calibration, all sixteen when none
     is loaded.
     """
 
@@ -522,7 +522,7 @@ def __init__(
         ]
         calibrated = CALIBRATED_ALL if calibrated is None else calibrated
         self._calibrated = [
-            (calibrated if s.calibrated is None else s.calibrated) & 0xFF
+            (calibrated if s.calibrated is None else s.calibrated) & 0xFFFF
             for s in settings
         ]
         self._dotbot_modes = [s.network_mode for s in settings]
@@ -725,7 +725,7 @@ def _gru_residual(self, index: int, model, report):
     def _advertise(self, index: int, packet: bytearray):
         """Send robot `index`'s advertisement, as its firmware encoded it."""
         # The calibration bitmask is the node's, not the control core's
-        packet[1] = self._calibrated[index]
+        packet[1:3] = self._calibrated[index].to_bytes(2, "little")
         self._counts[:, index] = 0
         frame = Frame(header=self._headers[index], packet=Packet.from_bytes(packet))
         if self._dotbot_modes[index] == SimulatedNetworkMode.MARI:

dotbot/firmware/flash.py

@@ -52,22 +52,27 @@
 # swarmit's `swarmit_config_t` (SWARMIT_CONFIG_MAGIC_VALUE). Each value must
 # match its firmware's.
 CONFIG_MAGIC_BY_ROLE = {
-    "dotbot-v3": 0x5753524F,
+    "dotbot-v3": 0x57535250,
     "gateway": 0x5753524D,
 }
 CONFIG_MANIFEST_NAME = "config-manifest.json"
 # swarmit_config_t, little-endian and packed: magic, has_net_id, net_id,
-# homography_count, sixteen 3x3 float32 matrices, valid_mm[4], site_name[16],
-# calibration_id[8]. Every byte the calibration does not set is 0xFF, the
-# erase state the firmware reads as "absent".
+# station_mask, sixteen 3x3 float32 matrices by slot, sixteen valid_mm[4] by
+# slot, site_name[16], calibration_id[8]; 872 bytes. Every byte the
+# calibration does not set is 0xFF, the erase state the firmware reads as
+# "absent"; slots outside the mask are ignored.
 LH2_MATRIX_BYTES = 3 * 3 * 4
+LH2_VALID_MM_BYTES = 4 * 4
 LH2_MAX_HOMOGRAPHIES = 16
-SWARMIT_CONFIG_COUNT_OFFSET = 12
-SWARMIT_CONFIG_MATRICES_OFFSET = SWARMIT_CONFIG_COUNT_OFFSET + 4
-SWARMIT_CONFIG_SITE_OFFSET = (
+SWARMIT_CONFIG_MASK_OFFSET = 12
+SWARMIT_CONFIG_MATRICES_OFFSET = SWARMIT_CONFIG_MASK_OFFSET + 4
+SWARMIT_CONFIG_VALID_MM_OFFSET = (
     SWARMIT_CONFIG_MATRICES_OFFSET + LH2_MAX_HOMOGRAPHIES * LH2_MATRIX_BYTES
 )
-SWARMIT_CONFIG_SITE_BYTES = 4 * 4 + 16 + 8
+SWARMIT_CONFIG_SITE_OFFSET = (
+    SWARMIT_CONFIG_VALID_MM_OFFSET + LH2_MAX_HOMOGRAPHIES * LH2_VALID_MM_BYTES
+)
+SWARMIT_CONFIG_SITE_BYTES = 16 + 8
 SWARMIT_CONFIG_BYTES = SWARMIT_CONFIG_SITE_OFFSET + SWARMIT_CONFIG_SITE_BYTES
 # Application images are linked after the bootloader.
 APP_FLASH_BASE_ADDR = 0x00010000
@@ -163,17 +168,15 @@ def make_config_hex_path(
 
 
 def load_calibration_file(path: Path):
-    """Read a schema 3 calibration file, refused when a robot could not take it."""
+    """Read a calibration file, refused when a robot could not take it."""
     from dotbot.calibration.lighthouse2 import (
-        pushable_stations,
+        calibration_messages,
         read_calibration_file,
-        site_fields_as_bytes,
     )
 
     try:
         calibration = read_calibration_file(Path(path))
-        pushable_stations(calibration)
-        site_fields_as_bytes(calibration)
+        calibration_messages(calibration)
     except (OSError, ValueError) as exc:
         raise click.ClickException(
             f"Cannot use calibration file {path}: {exc}"
@@ -187,22 +190,28 @@ def swarmit_config_page(net_id_value: int, calibration=None) -> bytes:
         homography_as_float32,
         pushable_stations,
         site_fields_as_bytes,
+        station_mask,
+        valid_mm_as_bytes,
     )
 
     page = bytearray(b"\xff" * SWARMIT_CONFIG_BYTES)
-    page[:SWARMIT_CONFIG_COUNT_OFFSET] = struct.pack(
+    page[:SWARMIT_CONFIG_MASK_OFFSET] = struct.pack(
         "<III", CONFIG_MAGIC_BY_ROLE["dotbot-v3"], 1, net_id_value
     )
     if calibration is not None:
         stations = pushable_stations(calibration)
-        page[SWARMIT_CONFIG_COUNT_OFFSET:SWARMIT_CONFIG_MATRICES_OFFSET] = struct.pack(
-            "<I", len(stations)
+        page[SWARMIT_CONFIG_MASK_OFFSET:SWARMIT_CONFIG_MATRICES_OFFSET] = struct.pack(
+            "<I", station_mask(calibration)
         )
         for station in stations:
             offset = SWARMIT_CONFIG_MATRICES_OFFSET + station.index * LH2_MATRIX_BYTES
             page[offset : offset + LH2_MATRIX_BYTES] = homography_as_float32(
                 station.homography
             )
+            offset = SWARMIT_CONFIG_VALID_MM_OFFSET + station.index * LH2_VALID_MM_BYTES
+            page[offset : offset + LH2_VALID_MM_BYTES] = valid_mm_as_bytes(
+                station.valid_mm
+            )
         page[SWARMIT_CONFIG_SITE_OFFSET:] = site_fields_as_bytes(calibration)
     return bytes(page)
 
@@ -410,9 +419,10 @@ def flash_role(
             )
         calibration = load_calibration_file(calibration_path)
         page = swarmit_config_page(net_id_val, calibration)
-        calibration_hex = page[SWARMIT_CONFIG_COUNT_OFFSET:].hex()
+        calibration_hex = page[SWARMIT_CONFIG_MASK_OFFSET:].hex()
         click.echo(
-            f"[INFO] calibration: {len(calibration.stations)} matrices, "
+            f"[INFO] calibration: stations "
+            f"{', '.join(str(s.index) for s in calibration.stations)}, "
             f"site {calibration.site.name}, id {calibration.id} "
             f"from {calibration_path}"
         )

dotbot/models.py

@@ -684,7 +684,7 @@ class DotBotModel(BaseModel):
     axle_position: Optional[DotBotLH2Position] = None  # the robot's own estimate
     # Where the robot has been, oldest first
     trail: List[Union[DotBotLH2Position, DotBotGPSPosition]] = []
-    calibrated: int = 0x00  # Bitmask: first lighthouse = 0x01, second lighthouse = 0x02
+    calibrated: int = 0x00  # bit i: the robot holds station i's homography
     battery: float = 3.0  # Voltage in Volts