Skip to content

bsp/lh2: drv/protocol: handle multi-lighthouse calibration and coordinates in mm #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
aabadie merged 1 commit into from
Jan 22, 2026
Merged

bsp/lh2: drv/protocol: handle multi-lighthouse calibration and coordinates in mm #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion bsp/lh2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 ===========================================
Expand Down
6 changes: 3 additions & 3 deletions bsp/nrf/lh2_default.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 4 additions & 4 deletions drv/protocol.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down