From afbf8ed95dc63dd8ffaed1a591ea1fc2ffd44c4f Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 22 Jan 2026 13:46:05 +0100 Subject: [PATCH] bsp/lh2: drv/protocol: handle multi-lighthouse calibration and coordinates in mm --- bsp/lh2.h | 2 +- bsp/nrf/lh2_default.c | 6 +++--- drv/protocol.h | 8 ++++---- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/bsp/lh2.h b/bsp/lh2.h index c480824..a2095c7 100644 --- a/bsp/lh2.h +++ b/bsp/lh2.h @@ -54,7 +54,7 @@ typedef struct { uint32_t timestamps[LH2_SWEEP_COUNT][LH2_BASESTATION_COUNT]; ///< timestamp of when the raw data was received db_lh2_data_ready_state_t data_ready[LH2_SWEEP_COUNT][LH2_BASESTATION_COUNT]; ///< Is the data in the buffer ready to send over radio, or has it already been sent ? uint8_t *spi_ring_buffer_count_ptr; ///< pointer to the SPI rung buffer packet count, so the user application can read how many spi captures are waiting to be processed. - bool lh2_calibration_complete; ///< Indicator of LH system calibration status (False = uncalibrated) + bool lh2_calibration_complete[LH2_BASESTATION_COUNT]; ///< Indicator of LH system calibration status (False = uncalibrated) } db_lh2_t; //=========================== public =========================================== diff --git a/bsp/nrf/lh2_default.c b/bsp/nrf/lh2_default.c index 07fafd9..2fe8075 100644 --- a/bsp/nrf/lh2_default.c +++ b/bsp/nrf/lh2_default.c @@ -988,19 +988,19 @@ void db_lh2_calculate_position(uint32_t count1, uint32_t count2, uint32_t basest double scale = homography_matrix[basestation_index][2][0] * cam_x + homography_matrix[basestation_index][2][1] * cam_y + homography_matrix[basestation_index][2][2]; coordinates[0] = x_position / scale; - coordinates[1] = (double)(1.0) - (y_position / scale); + coordinates[1] = y_position / scale; } void db_lh2_store_homography(db_lh2_t *lh2, uint8_t basestation_index, int32_t homography_matrix_from_packet[3][3]) { double homography_matrix_temp_storage[3][3] = { 0 }; for (uint8_t i = 0; i < 3; i++) { for (uint8_t j = 0; j < 3; j++) { - homography_matrix_temp_storage[i][j] = (double)(homography_matrix_from_packet[i][j]) / 1e6; + homography_matrix_temp_storage[i][j] = (double)(homography_matrix_from_packet[i][j] / 1e3); } } memcpy(homography_matrix[basestation_index], homography_matrix_temp_storage, sizeof(double) * 3 * 3); - lh2->lh2_calibration_complete = true; + lh2->lh2_calibration_complete[basestation_index] = true; } #define TS4231_INIT_SAMPLES_LEN 14 diff --git a/drv/protocol.h b/drv/protocol.h index 46b046d..833c156 100644 --- a/drv/protocol.h +++ b/drv/protocol.h @@ -94,9 +94,9 @@ 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 z; ///< Z coordinate, multiplied by 1e6 + uint32_t x; ///< X coordinate in mm + uint32_t y; ///< Y coordinate in mm + uint32_t z; ///< Z coordinate in mm } protocol_lh2_location_t; /// DotBot protocol LH2 waypoints @@ -107,7 +107,7 @@ typedef struct __attribute__((packed)) { 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 } protocol_lh2_homography_t; /// DotBot protocol GPS coordinates