diff --git a/device/bootloader-single-core/Source/main.c b/device/bootloader-single-core/Source/main.c index 467d98a..9b02cf7 100644 --- a/device/bootloader-single-core/Source/main.c +++ b/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)) { diff --git a/device/bootloader/Source/lh2_calibration.h b/device/bootloader/Source/lh2_calibration.h index ae2a47f..301c686 100644 --- a/device/bootloader/Source/lh2_calibration.h +++ b/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 diff --git a/device/bootloader/Source/localization.c b/device/bootloader/Source/localization.c index 6f68eda..04ba722 100644 --- a/device/bootloader/Source/localization.c +++ b/device/bootloader/Source/localization.c @@ -1,18 +1,28 @@ #include #include +#include #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"); @@ -20,48 +30,73 @@ void localization_init(void) { 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; } diff --git a/device/bootloader/Source/localization.h b/device/bootloader/Source/localization.h index e097b1c..f61cc12 100644 --- a/device/bootloader/Source/localization.h +++ b/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); diff --git a/device/bootloader/Source/protocol.h b/device/bootloader/Source/protocol.h index e6da880..f1b3136 100644 --- a/device/bootloader/Source/protocol.h +++ b/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; /** diff --git a/device/network_core/Source/ipc.h b/device/network_core/Source/ipc.h index ea722cc..b126f49 100644 --- a/device/network_core/Source/ipc.h +++ b/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)) { diff --git a/dotbot-libs b/dotbot-libs index a06ad9f..3fce29b 160000 --- a/dotbot-libs +++ b/dotbot-libs @@ -1 +1 @@ -Subproject commit a06ad9fcddb37e8767422e5c282b29d30baf9b86 +Subproject commit 3fce29b3ff468d1a1abb329c4b0308170549f174 diff --git a/swarmit/cli/main.py b/swarmit/cli/main.py index de7e59d..273d113 100755 --- a/swarmit/cli/main.py +++ b/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("-") } diff --git a/swarmit/dashboard/frontend/src/App.tsx b/swarmit/dashboard/frontend/src/App.tsx index 70519fe..da089a5 100644 --- a/swarmit/dashboard/frontend/src/App.tsx +++ b/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,6 +84,7 @@ export default function MainDashboard() { const { token, setToken } = usePersistedToken(); const [tokenActiveness, setTokenActiveness] = useState("NoToken"); const [settings, setSettings] = useState(null); + const [areaSize, setAreaSize] = useState<{width: number; height: number}>({width: 2500, height: 2500}); useEffect(() => { const fetchSettings = async () => { @@ -91,11 +92,12 @@ export default function MainDashboard() { 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) - .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() {
{page === 1 && ( - < HomePage token={token} tokenActiveness={tokenActiveness} dotbots={dotbots} /> + < HomePage token={token} tokenActiveness={tokenActiveness} dotbots={dotbots} areaSize={areaSize} /> )} {page === 2 && ( diff --git a/swarmit/dashboard/frontend/src/BotMap.tsx b/swarmit/dashboard/frontend/src/BotMap.tsx index 7dc290a..4c09d82 100644 --- a/swarmit/dashboard/frontend/src/BotMap.tsx +++ b/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}`} interface DotBotsMapProps { dotbots: Record; + areaSize: { + width: number; + height: number; + }; } -export const DotBotsMap: React.FC = ({ dotbots }: DotBotsMapProps) => { - const mapSize = 700; - const gridSize = `${mapSize + 1}px`; +export const DotBotsMap: React.FC = ({ dotbots, areaSize }: DotBotsMapProps) => { + const mapSize = 1000; + const gridWidth = `${mapSize + 1}px`; + const gridHeight = `${mapSize * areaSize.height / areaSize.width + 1}px`; return (
0 ? "visible" : "invisible"}`}>
-
- +
+ - - - - - - + + + @@ -90,7 +97,7 @@ export const DotBotsMap: React.FC = ({ dotbots }: DotBotsMapPro {Object.entries(dotbots) .map(([address, dotbot]) => ( - + ))}
diff --git a/swarmit/dashboard/frontend/src/HomePage.tsx b/swarmit/dashboard/frontend/src/HomePage.tsx index 006aedf..d74b501 100644 --- a/swarmit/dashboard/frontend/src/HomePage.tsx +++ b/swarmit/dashboard/frontend/src/HomePage.tsx @@ -6,10 +6,14 @@ interface HomePageProps { token: Token | null; tokenActiveness: tokenActivenessType; dotbots: Record; + 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(null); const [loading, setLoading] = useState(false); const [message, setMessage] = useState(null); @@ -141,7 +145,7 @@ export default function HomePage({ token, tokenActiveness, dotbots }: HomePagePr return (
- +
{token?.payload &&

Token Info

diff --git a/swarmit/dashboard/main.py b/swarmit/dashboard/main.py index 56af69e..7d35843 100755 --- a/swarmit/dashboard/main.py +++ b/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"], ) diff --git a/swarmit/testbed/controller.py b/swarmit/testbed/controller.py index 17e1fc1..ffddad5 100644 --- a/swarmit/testbed/controller.py +++ b/swarmit/testbed/controller.py @@ -164,7 +164,7 @@ def generate_status(status_data, devices=[], status_message="found"): f"{device_addr}", f"{device_data.device.name}", f"[{battery_level_color(device_data.battery)}]{device_data.battery / 1000:.2f}V ({int(device_data.battery / 3000 * 100)}%)", - f"({(device_data.pos_x / 1e6):.2f}, {(device_data.pos_y / 1e6):.2f})", + f"({device_data.pos_x}, {device_data.pos_y})", f"{'[bold cyan]' if device_data.status == StatusType.Running else '[bold green]'}{device_data.status.name}", ) return Group(header, table) @@ -214,6 +214,7 @@ class ControllerSettings: network_id: int = 1 adapter: str = "serial" # or "mqtt", "marilib-edge", "marilib-cloud" devices: list[str] = dataclasses.field(default_factory=lambda: []) + map_size: str = "2500x2500" ota_max_retries: int = OTA_MAX_RETRIES_DEFAULT ota_timeout: float = OTA_ACK_TIMEOUT_DEFAULT adapter_wait_timeout: float = 3 diff --git a/swarmit/testbed/webserver.py b/swarmit/testbed/webserver.py index 6196ccb..89add4f 100644 --- a/swarmit/testbed/webserver.py +++ b/swarmit/testbed/webserver.py @@ -226,12 +226,20 @@ async def status(request: Request): class SettingsResponse(BaseModel): network_id: int + area_width: int + area_height: int @api.get("/settings", response_model=SettingsResponse) async def settings(request: Request): controller: Controller = request.app.state.controller - return SettingsResponse(network_id=controller.settings.network_id) + map_size = controller.settings.map_size + width_str, height_str = map_size.lower().split('x') + return SettingsResponse( + network_id=controller.settings.network_id, + area_width=int(width_str), + area_height=int(height_str), + ) @api.post("/start") diff --git a/swarmit/tests/test_controller.py b/swarmit/tests/test_controller.py index ba6aa01..be2b0c8 100644 --- a/swarmit/tests/test_controller.py +++ b/swarmit/tests/test_controller.py @@ -283,8 +283,8 @@ def test_controller_reset(): for node in nodes: test_adapter.add_node(node) locations = { - "00000001": ResetLocation(pos_x=1000000, pos_y=2000000), - "00000002": ResetLocation(pos_x=2000000, pos_y=1000000), + "00000001": ResetLocation(pos_x=1000000, pos_y=2000), + "00000002": ResetLocation(pos_x=2000000, pos_y=1000), } controller.reset(locations=locations) time.sleep(0.3) @@ -317,8 +317,8 @@ def test_controller_reset_not_ready(): for node in nodes: test_adapter.add_node(node) locations = { - "00000001": ResetLocation(pos_x=1000000, pos_y=2000000), - "00000002": ResetLocation(pos_x=2000000, pos_y=1000000), + "00000001": ResetLocation(pos_x=1000000, pos_y=2000), + "00000002": ResetLocation(pos_x=2000000, pos_y=1000), } controller.reset(locations=locations) time.sleep(0.3) diff --git a/swarmit/tests/test_dashboard_main.py b/swarmit/tests/test_dashboard_main.py index 9c3e661..e095c2e 100644 --- a/swarmit/tests/test_dashboard_main.py +++ b/swarmit/tests/test_dashboard_main.py @@ -27,6 +27,8 @@ gateway. Default: edge -d, --devices TEXT Subset list of device addresses to interact with, separated with , + -m, --map-size TEXT Size of the map on the ground in mm, in the format + WIDTHxHEIGHT. Default: 2500x2500. -v, --verbose Enable verbose mode. --open-browser Open the dashboard in a web browser automatically. --http-port INTEGER HTTP port. Default: edge diff --git a/swarmit/tests/test_webserver.py b/swarmit/tests/test_webserver.py index cf7d939..035dc16 100644 --- a/swarmit/tests/test_webserver.py +++ b/swarmit/tests/test_webserver.py @@ -84,7 +84,11 @@ def test_status_endpoint(client): def test_settings_endpoint(client): res = client.get("/settings") assert res.status_code == 200 - assert res.json() == {"network_id": 999} + assert res.json() == { + "network_id": 999, + "area_height": 2500, + "area_width": 2500, + } def test_start_endpoint(client): diff --git a/swarmit/tests/utils.py b/swarmit/tests/utils.py index c337e6f..b378ea4 100644 --- a/swarmit/tests/utils.py +++ b/swarmit/tests/utils.py @@ -105,8 +105,8 @@ def run(self): device=self.device_type.value, status=self.status.value, battery=self.battery, - pos_x=0.5 * 1e6, - pos_y=0.5 * 1e6, + pos_x=2500, + pos_y=2500, ), ) self.send_packet(packet)