device: swarmit: multi-lighthouse support + use position coordinates in mm

device/bootloader-single-core/Source/main.c

@@ -69,8 +69,8 @@ typedef struct {
 
 /// DotBot protocol LH2 computed location
 typedef struct __attribute__((packed)) {
-    uint32_t x;  ///< X coordinate, multiplied by 1e6
-    uint32_t y;  ///< Y coordinate, multiplied by 1e6
+    uint32_t x;  ///< X coordinate in mm
+    uint32_t y;  ///< Y coordinate in mm
 } position_2d_t;
 
 typedef struct __attribute__((packed)) {

device/bootloader/Source/lh2_calibration.h

@@ -4,12 +4,10 @@
 
 #include "localization.h"
 
-#define LH2_CALIBRATION_IS_VALID    (1)
+#define LH2_CALIBRATION_IS_VALID    (0)
+#define LH2_CALIBRATION_COUNT       (0)
 
-static int32_t swrmt_homography[3][3] = {
-    {-910156, 4037136, -16647},
-    {1519258, 3291055, -139111},
-    {264976, 3267254, 1000000},
+static int32_t swrmt_homography[LH2_CALIBRATION_COUNT][3][3] = {
 };
 
 #endif // __LH2_CALIBRATION_H

device/bootloader/Source/localization.c

@@ -1,67 +1,102 @@
 #include <stdio.h>
 #include <string.h>
+#include <math.h>
 
 #include "board_config.h"
 #include "lh2.h"
 #include "localization.h"
 #include "lh2_calibration.h"
 
+#define VALID_POSITION_DISTANCE_THRESHOLD_MM 500.0f  ///< Maximum distance in mm between two consecutive position measurements for the position to be considered valid
+
 typedef struct {
     db_lh2_t                lh2;
     double                  coordinates[2];
+    position_2d_t           position;
+    position_2d_t           previous_position;
 } localization_data_t;
 
 static __attribute__((aligned(4))) localization_data_t _localization_data = { 0 };
 
+float _distance(position_2d_t *reference, position_2d_t *current) {
+    float dx = ((float)current->x - (float)reference->x);
+    float dy = ((float)current->y - (float)reference->y);
+    return sqrtf(powf(dx, 2) + powf(dy, 2));
+}
 
 void localization_init(void) {
     puts("Initialize localization");
     db_lh2_init(&_localization_data.lh2, &db_lh2_d, &db_lh2_e);
     db_lh2_start();
 
 #if LH2_CALIBRATION_IS_VALID
-    puts("Store homography matrix");
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
-            printf("%i ", swrmt_homography[i][j]);
+    // Only store the homography if a valid one is set in lh2_calibration.h
+    for (uint8_t lh_index = 0; lh_index < LH2_CALIBRATION_COUNT; lh_index++) {
+        printf("Store homography matrix for LH%u:\n", lh_index);
+        for (int i = 0; i < 3; i++) {
+            for (int j = 0; j < 3; j++) {
+                printf("%i ", swrmt_homographies[lh_index][i][j]);
+            }
+            printf("\n");
         }
-        printf("\n");
+        db_lh2_store_homography(&_localization_data.lh2, lh_index, swrmt_homographies[lh_index]);
     }
-    // Only store the homography if a valid one is set in lh2_calibration.h
-    db_lh2_store_homography(&_localization_data.lh2, 0, swrmt_homography);
 #endif
 
 }
 
 bool localization_process_data(void) {
     db_lh2_process_location(&_localization_data.lh2);
-    return (_localization_data.lh2.data_ready[0][0] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][0] == DB_LH2_PROCESSED_DATA_AVAILABLE);
+    for (uint8_t lh_index = 0; lh_index < LH2_BASESTATION_COUNT; lh_index++) {
+        if (_localization_data.lh2.data_ready[0][lh_index] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][lh_index] == DB_LH2_PROCESSED_DATA_AVAILABLE) {
+            return true;
+        }
+    }
+    return false;
 }
 
 bool localization_get_position(position_2d_t *position) {
-    if ((LH2_CALIBRATION_IS_VALID) && (_localization_data.lh2.data_ready[0][0] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][0] == DB_LH2_PROCESSED_DATA_AVAILABLE)) {
+    if (LH2_CALIBRATION_IS_VALID) {
         db_lh2_stop();
-        db_lh2_calculate_position(_localization_data.lh2.locations[0][0].lfsr_counts, _localization_data.lh2.locations[1][0].lfsr_counts, _localization_data.lh2.locations[0][0].selected_polynomial, _localization_data.coordinates);
-        _localization_data.lh2.data_ready[0][0] = DB_LH2_NO_NEW_DATA;
-        _localization_data.lh2.data_ready[1][0] = DB_LH2_NO_NEW_DATA;
-        if (_localization_data.coordinates[0] < 0 || _localization_data.coordinates[0] > 1.0 || _localization_data.coordinates[1] < 0 || _localization_data.coordinates[1] > 1) {
-            printf("Invalid coordinates (%f,%f)\n", _localization_data.coordinates[0], _localization_data.coordinates[1]);
-            db_lh2_start();
+        for (uint8_t lh_index = 0; lh_index < LH2_BASESTATION_COUNT; lh_index++) {
+            if (_localization_data.lh2.data_ready[0][lh_index] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][lh_index] == DB_LH2_PROCESSED_DATA_AVAILABLE) {
+                db_lh2_calculate_position(_localization_data.lh2.locations[0][lh_index].lfsr_counts, _localization_data.lh2.locations[1][lh_index].lfsr_counts, lh_index, _localization_data.coordinates);
+                _localization_data.lh2.data_ready[0][lh_index] = DB_LH2_NO_NEW_DATA;
+                _localization_data.lh2.data_ready[1][lh_index] = DB_LH2_NO_NEW_DATA;
+                break;
+            }
+        }
+        db_lh2_start();
+
+        if (_localization_data.coordinates[0] < 0 || _localization_data.coordinates[0] > 100000 || _localization_data.coordinates[1] < 0 || _localization_data.coordinates[1] > 100000) {
+            printf("Invalid position (%u,%u)\n", _localization_data.position.x, _localization_data.position.y);
             return false;
         }
 
-        uint32_t position_x = (uint32_t)(_localization_data.coordinates[0] * 1e6);
-        uint32_t position_y = (uint32_t)(_localization_data.coordinates[1] * 1e6);
+        _localization_data.position.x = (uint32_t)_localization_data.coordinates[0];
+        _localization_data.position.y = (uint32_t)_localization_data.coordinates[1];
 
-        if (position_x == UINT32_MAX || position_y == UINT32_MAX) {
-            db_lh2_start();
+        if (_localization_data.previous_position.x == 0 && _localization_data.previous_position.y == 0) {
+            _localization_data.previous_position.x = _localization_data.position.x;
+            _localization_data.previous_position.y = _localization_data.position.y;
+        }
+
+        float distance = _distance((position_2d_t *)&_localization_data.previous_position, (position_2d_t *)&_localization_data.position);
+        if (distance > VALID_POSITION_DISTANCE_THRESHOLD_MM) {
+            printf("Distance (%f) from (%u,%u) to (%u,%u) is too high\n",
+                    distance,
+                   _localization_data.previous_position.x,
+                   _localization_data.previous_position.y,
+                   _localization_data.position.x,
+                   _localization_data.position.y);
             return false;
         }
 
-        position->x = (uint32_t)(_localization_data.coordinates[0] * 1e6);
-        position->y = (uint32_t)(_localization_data.coordinates[1] * 1e6);
+        _localization_data.previous_position.x = _localization_data.position.x;
+        _localization_data.previous_position.y = _localization_data.position.y;
+        position->x = _localization_data.position.x;
+        position->y = _localization_data.position.y;
         printf("Position (%u,%u)\n", position->x, position->y);
-        db_lh2_start();
         return true;
     }
 

device/bootloader/Source/localization.h

@@ -18,13 +18,13 @@
 
 /// DotBot protocol LH2 computed location
 typedef struct __attribute__((packed)) {
-    uint32_t x;  ///< X coordinate, multiplied by 1e6
-    uint32_t y;  ///< Y coordinate, multiplied by 1e6
+    uint32_t x;  ///< X coordinate in mm
+    uint32_t y;  ///< Y coordinate in mm
 } position_2d_t;
 
 typedef struct __attribute__((packed)) {
     uint8_t basestation_index;        ///< which LH basestation is this homography for?
-    int32_t homography_matrix[3][3];  ///< homography matrix, each element multiplied by 1e6
+    int32_t homography_matrix[3][3];  ///< homography matrix, each element multiplied by 1e3
 } localization_homography_t;
 
 void localization_init(void);

device/bootloader/Source/protocol.h

@@ -113,8 +113,8 @@ typedef struct __attribute__((packed)) {
 
 /// DotBot protocol LH2 computed location
 typedef struct __attribute__((packed)) {
-    uint32_t x;  ///< X coordinate, multiplied by 1e6
-    uint32_t y;  ///< Y coordinate, multiplied by 1e6
+    uint32_t x;  ///< X coordinate in mm
+    uint32_t y;  ///< Y coordinate in mm
 } protocol_lh2_location_t;
 
 /**

device/network_core/Source/ipc.h

@@ -66,8 +66,8 @@ typedef struct __attribute__((packed)) {
 
 /// DotBot protocol LH2 computed location
 typedef struct __attribute__((packed)) {
-    uint32_t x;  ///< X coordinate, multiplied by 1e6
-    uint32_t y;  ///< Y coordinate, multiplied by 1e6
+    uint32_t x;  ///< X coordinate in mm
+    uint32_t y;  ///< Y coordinate in mm
 } position_2d_t;
 
 typedef struct __attribute__((packed)) {

swarmit/cli/main.py

@@ -193,8 +193,8 @@ def reset(ctx, locations):
         return
     locations = {
         int(location.split(":")[0], 16): ResetLocation(
-            pos_x=int(float(location.split(":")[1].split(",")[0]) * 1e6),
-            pos_y=int(float(location.split(":")[1].split(",")[1]) * 1e6),
+            pos_x=int(float(location.split(":")[1].split(",")[0])),
+            pos_y=int(float(location.split(":")[1].split(",")[1])),
         )
         for location in locations.split("-")
     }

swarmit/dashboard/frontend/src/App.tsx

@@ -71,9 +71,9 @@ export function usePersistedToken() {
 }
 
 export interface SettingsResponse {
-  response: {
-    network_id: number;
-  };
+  network_id: number;
+  area_width: number;
+  area_height: number;
 }
 
 
@@ -84,18 +84,20 @@ export default function MainDashboard() {
   const { token, setToken } = usePersistedToken();
   const [tokenActiveness, setTokenActiveness] = useState<tokenActivenessType>("NoToken");
   const [settings, setSettings] = useState<SettingsType | null>(null);
+  const [areaSize, setAreaSize] = useState<{width: number; height: number}>({width: 2500, height: 2500});
 
   useEffect(() => {
     const fetchSettings = async () => {
       try {
         const res = await fetch(`${API_URL}/settings`);
         if (!res.ok) throw new Error("Network response was not ok");
 
-        const json: SettingsResponse = await res.json();
+        const json = await res.json();
         const settings: SettingsType = {
-          network_id: json.response.network_id.toString(16),
+          network_id: json.network_id.toString(16),
         };
         setSettings(settings);
+        setAreaSize({width: json.area_width, height: json.area_height});
       } catch (err) {
         console.error("Error fetching settings:", err);
       }
@@ -141,7 +143,7 @@ export default function MainDashboard() {
         .then((json) => {
           const dotbots = Object.fromEntries(
             Object.entries(json.response as Record<string, DotBotData>)
-              .map(([k, v]) => [k, { ...v, battery: v.battery / 1000, pos_x: v.pos_x / 1000000, pos_y: v.pos_y / 1000000 }]));
+              .map(([k, v]) => [k, { ...v, battery: v.battery / 1000, pos_x: v.pos_x, pos_y: v.pos_y }]));
           setDotBots(dotbots);
         })
         .catch((_err) => {
@@ -188,7 +190,7 @@ export default function MainDashboard() {
 
         <main className="flex-1 p-8">
           {page === 1 && (
-            < HomePage token={token} tokenActiveness={tokenActiveness} dotbots={dotbots} />
+            < HomePage token={token} tokenActiveness={tokenActiveness} dotbots={dotbots} areaSize={areaSize} />
           )}
 
           {page === 2 && (

swarmit/dashboard/frontend/src/BotMap.tsx

@@ -5,15 +5,20 @@ interface DotBotsMapPointProps {
   dotbot: DotBotData;
   address: string;
   mapSize: number;
+  areaSize: {
+    width: number;
+    height: number;
+  };
 }
 
 function DotBotsMapPoint({
   dotbot,
   address,
   mapSize,
+  areaSize,
 }: DotBotsMapPointProps) {
-  const posX = mapSize * dotbot.pos_x;
-  const posY = mapSize * dotbot.pos_y;
+  const posX = mapSize * dotbot.pos_x / areaSize!.width;
+  const posY = mapSize * dotbot.pos_y / areaSize!.width;
 
   const getStatusColor = (status: StatusType) => {
     switch (status) {
@@ -59,24 +64,26 @@ Position: ${posX}x${posY}`}</title>
 
 interface DotBotsMapProps {
   dotbots: Record<string, DotBotData>;
+  areaSize: {
+    width: number;
+    height: number;
+  };
 }
 
-export const DotBotsMap: React.FC<DotBotsMapProps> = ({ dotbots }: DotBotsMapProps) => {
-  const mapSize = 700;
-  const gridSize = `${mapSize + 1}px`;
+export const DotBotsMap: React.FC<DotBotsMapProps> = ({ dotbots, areaSize }: DotBotsMapProps) => {
+  const mapSize = 1000;
+  const gridWidth = `${mapSize + 1}px`;
+  const gridHeight = `${mapSize * areaSize.height / areaSize.width + 1}px`;
 
   return (
     <div className={`${Object.keys(dotbots).length > 0 ? "visible" : "invisible"}`}>
       <div className="flex justify-center">
-        <div style={{ height: gridSize, width: gridSize }}>
-          <svg style={{ height: gridSize, width: gridSize }}>
+        <div style={{ height: gridHeight, width: gridWidth }}>
+          <svg style={{ height: gridHeight, width: gridWidth }}>
             <defs>
-              <pattern id={`smallGrid${mapSize}`} width={mapSize / 50} height={mapSize / 50} patternUnits="userSpaceOnUse">
-                <path d={`M ${mapSize / 50} 0 L 0 0 0 ${mapSize / 50}`} fill="none" stroke="gray" strokeWidth={0.5} />
-              </pattern>
-              <pattern id={`grid${mapSize}`} width={mapSize / 5} height={mapSize / 5} patternUnits="userSpaceOnUse">
-                <rect width={mapSize / 5} height={mapSize / 5} fill={`url(#smallGrid${mapSize})`} />
-                <path d={`M ${mapSize / 5} 0 L 0 0 0 ${mapSize / 5}`} fill="none" stroke="gray" strokeWidth={1} />
+              <pattern id={`grid${mapSize}`} width={`${500 * mapSize / areaSize!.width}`} height={`${500 * mapSize / areaSize!.width}`} patternUnits="userSpaceOnUse">
+                <rect width={`${500 * mapSize / areaSize.width}`} height={`${500 * mapSize / areaSize!.width}`} fill={`url(#smallGrid${mapSize})`}/>
+                <path d={`M ${500 * mapSize / areaSize!.width} 0 L 0 0 0 ${500 * mapSize / areaSize!.width}`} fill="none" stroke="gray" strokeWidth="1"/>
               </pattern>
             </defs>
 
@@ -90,7 +97,7 @@ export const DotBotsMap: React.FC<DotBotsMapProps> = ({ dotbots }: DotBotsMapPro
 
             {Object.entries(dotbots)
               .map(([address, dotbot]) => (
-                <DotBotsMapPoint key={address} dotbot={dotbot} address={address} mapSize={mapSize} />
+                <DotBotsMapPoint key={address} dotbot={dotbot} address={address} mapSize={mapSize} areaSize={areaSize} />
               ))}
           </svg>
         </div>

swarmit/dashboard/frontend/src/HomePage.tsx

@@ -6,10 +6,14 @@ interface HomePageProps {
   token: Token | null;
   tokenActiveness: tokenActivenessType;
   dotbots: Record<string, DotBotData>;
+  areaSize: {
+    width: number;
+    height: number;
+  };
 }
 
 
-export default function HomePage({ token, tokenActiveness, dotbots }: HomePageProps) {
+export default function HomePage({ token, tokenActiveness, dotbots, areaSize }: HomePageProps) {
   const [file, setFile] = useState<File | null>(null);
   const [loading, setLoading] = useState(false);
   const [message, setMessage] = useState<string | null>(null);
@@ -141,7 +145,7 @@ export default function HomePage({ token, tokenActiveness, dotbots }: HomePagePr
 
   return (
     <div>
-      <DotBotsMap dotbots={dotbots} />
+      <DotBotsMap dotbots={dotbots} areaSize={areaSize} />
       <div className="max-w-md mx-auto p-6 space-y-6 bg-white rounded-2xl shadow mt-10 animate-fadeIn">
         {token?.payload && <div className="border p-4 rounded-lg bg-gray-50">
           <h3 className="font-semibold mb-2">Token Info</h3>

swarmit/dashboard/main.py

@@ -14,6 +14,7 @@
 DEFAULTS_DASHBOARD = {
     **DEFAULTS,
     "http_port": 8001,
+    "map_size": "2500x2500",
 }
 
 
@@ -73,6 +74,13 @@
     default="",
     help="Subset list of device addresses to interact with, separated with ,",
 )
+@click.option(
+    "-m",
+    "--map-size",
+    type=str,
+    default=DEFAULTS_DASHBOARD['map_size'],
+    help="Size of the map on the ground in mm, in the format WIDTHxHEIGHT. Default: 2500x2500.",
+)
 @click.option(
     "-v",
     "--verbose",
@@ -101,6 +109,7 @@ def main(
     network_id,
     adapter,
     devices,
+    map_size,
     verbose,
     open_browser,
 ):
@@ -114,6 +123,7 @@ def main(
         "mqtt_use_tls": mqtt_use_tls,
         "swarmit_network_id": network_id,
         "devices": devices,
+        "map_size": map_size,
         "verbose": verbose,
         "http_port": http_port,
     }
@@ -134,6 +144,7 @@ def main(
         network_id=int(final_config["swarmit_network_id"], 16),
         adapter=final_config["adapter"],
         devices=[d for d in final_config["devices"].split(",") if d],
+        map_size=final_config["map_size"],
         verbose=final_config["verbose"],
     )