From 7dcef40516bc173fe54e85517551c6d9de1fa9b9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 May 2015 14:52:11 +0900 Subject: [PATCH] irlock: report target in radians --- src/drivers/drv_irlock.h | 11 +++++------ src/drivers/irlock/irlock.cpp | 36 +++++++++++++++++++++++------------ 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 06cf5c8645..fa6b3016cd 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -52,10 +52,9 @@ /** irlock_s structure returned from read calls **/ struct irlock_s { uint64_t timestamp; /** microseconds since system start **/ - - uint16_t signature; - uint16_t center_x; - uint16_t center_y; - uint16_t width; - uint16_t height; + uint16_t target_num; /** target number prioritised by size (largest is 0) **/ + float angle_x; /** x-axis angle in radians from center of image to center of target **/ + float angle_y; /** y-axis angle in radians from center of image to center of target **/ + float size_x; /** size in radians of target along x-axis **/ + float size_y; /** size in radians of target along y-axis **/ }; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index d22d9a38e4..4aeb8c7497 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -67,6 +67,11 @@ #define IRLOCK_RESYNC 0x5500 #define IRLOCK_ADJUST 0xAA +#define IRLOCK_CENTER_X 159 // the x-axis center pixel position +#define IRLOCK_CENTER_Y 99 // the y-axis center pixel position +#define IRLOCK_PIXELS_PER_RADIAN_X 307.9075f // x-axis pixel to radian scaler assuming 60deg FOV on x-axis +#define IRLOCK_PIXELS_PER_RADIAN_Y 326.4713f // y-axis pixel to radian scaler assuming 35deg FOV on y-axis + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -227,12 +232,12 @@ int IRLOCK::test() /** output all objects found **/ while (_reports->count() > 0) { _reports->get(&obj_report); - warnx("sig:%d x:%d y:%d width:%d height:%d", - (int)obj_report.signature, - (int)obj_report.center_x, - (int)obj_report.center_y, - (int)obj_report.width, - (int)obj_report.height); + warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", + (int)obj_report.target_num, + (double)obj_report.angle_x, + (double)obj_report.angle_y, + (double)obj_report.size_x, + (double)obj_report.size_y); } /** sleep for 0.05 seconds **/ @@ -369,18 +374,25 @@ int IRLOCK::read_device_block(struct irlock_s *block) int status = transfer(nullptr, 0, &bytes[0], 12); uint16_t checksum = bytes[1] << 8 | bytes[0]; - block->signature = bytes[3] << 8 | bytes[2]; - block->center_x = bytes[5] << 8 | bytes[4]; - block->center_y = bytes[7] << 8 | bytes[6]; - block->width = bytes[9] << 8 | bytes[8]; - block->height = bytes[11] << 8 | bytes[10]; + uint16_t target_num = bytes[3] << 8 | bytes[2]; + uint16_t pixel_x = bytes[5] << 8 | bytes[4]; + uint16_t pixel_y = bytes[7] << 8 | bytes[6]; + uint16_t pixel_size_x = bytes[9] << 8 | bytes[8]; + uint16_t pixel_size_y = bytes[11] << 8 | bytes[10]; /** crc check **/ - if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { + if (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { _read_failures++; return -EIO; } + /** convert to angles **/ + block->target_num = target_num; + block->angle_x = (((float)(pixel_x-IRLOCK_CENTER_X))/IRLOCK_PIXELS_PER_RADIAN_X); + block->angle_y = (((float)(pixel_y-IRLOCK_CENTER_Y))/IRLOCK_PIXELS_PER_RADIAN_Y); + block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X; + block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y; + block->timestamp = hrt_absolute_time(); return status; }