From 0bda5c68405e5ef9e53f0496e3c6fb05e154c1d7 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jul 2016 12:04:27 -0700 Subject: [PATCH 1/3] irlock: change output format to tangent of angles --- src/drivers/drv_irlock.h | 8 ++++---- src/drivers/irlock/irlock.cpp | 27 +++++++++++++++++---------- 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index fa6b3016cd1a..36fc635dea04 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -53,8 +53,8 @@ struct irlock_s { uint64_t timestamp; /** microseconds since system start **/ 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 **/ + float pos_x; /** x-axis distance from center of image to center of target in units of tan(theta) **/ + float pos_y; /** y-axis distance from center of image to center of target in units of tan(theta) **/ + float size_x; /** size of target along x-axis in units of tan(theta) **/ + float size_y; /** size of target along y-axis in units of tan(theta) **/ }; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 620cbf08243d..18ac04a73f22 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -67,10 +67,17 @@ #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 +#define IRLOCK_RES_X 320 +#define IRLOCK_RES_Y 200 + +#define IRLOCK_CENTER_X (IRLOCK_RES_X/2) // the x-axis center pixel position +#define IRLOCK_CENTER_Y (IRLOCK_RES_Y/2) // the y-axis center pixel position + +#define IRLOCK_FOV_X (60.0f*M_PI_F/180.0f) +#define IRLOCK_FOV_Y (35.0f*M_PI_F/180.0f) + +#define IRLOCK_TAN_ANG_PER_PIXEL_X (2*tanf(IRLOCK_FOV_X/2)/IRLOCK_RES_X) +#define IRLOCK_TAN_ANG_PER_PIXEL_Y (2*tanf(IRLOCK_FOV_Y/2)/IRLOCK_RES_Y) #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -234,8 +241,8 @@ int IRLOCK::test() _reports->get(&obj_report); 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.pos_x, + (double)obj_report.pos_y, (double)obj_report.size_x, (double)obj_report.size_y); } @@ -388,10 +395,10 @@ int IRLOCK::read_device_block(struct irlock_s *block) /** 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->pos_x = (pixel_x - IRLOCK_CENTER_X) * IRLOCK_TAN_ANG_PER_PIXEL_X; + block->pos_y = (pixel_y - IRLOCK_CENTER_Y) * IRLOCK_TAN_ANG_PER_PIXEL_Y; + block->size_x = pixel_size_x * IRLOCK_TAN_ANG_PER_PIXEL_X; + block->size_y = pixel_size_y * IRLOCK_TAN_ANG_PER_PIXEL_Y; block->timestamp = hrt_absolute_time(); return status; From bd97824230e9df115b256ce378671a1c88084919 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jul 2016 18:31:44 -0700 Subject: [PATCH 2/3] irlock: put all targets in single struct --- src/drivers/drv_irlock.h | 13 ++++++--- src/drivers/irlock/irlock.cpp | 52 +++++++++++++++++------------------ 2 files changed, 35 insertions(+), 30 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 36fc635dea04..ea0eac8d7567 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -49,12 +49,17 @@ #define IRLOCK_OBJECTS_MAX 5 /** up to 5 objects can be detected/reported **/ -/** irlock_s structure returned from read calls **/ -struct irlock_s { - uint64_t timestamp; /** microseconds since system start **/ - uint16_t target_num; /** target number prioritised by size (largest is 0) **/ +struct irlock_target_s { + uint16_t signature; /** target signature **/ float pos_x; /** x-axis distance from center of image to center of target in units of tan(theta) **/ float pos_y; /** y-axis distance from center of image to center of target in units of tan(theta) **/ float size_x; /** size of target along x-axis in units of tan(theta) **/ float size_y; /** size of target along y-axis in units of tan(theta) **/ }; + +/** irlock_s structure returned from read calls **/ +struct irlock_s { + uint64_t timestamp; /** microseconds since system start **/ + uint8_t num_targets; + struct irlock_target_s targets[IRLOCK_OBJECTS_MAX]; +}; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 18ac04a73f22..721fc92ddfd4 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -114,7 +114,7 @@ class IRLOCK : public device::I2C int read_device(); bool sync_device(); int read_device_word(uint16_t *word); - int read_device_block(struct irlock_s *block); + int read_device_block(struct irlock_target_s *block); /** internal variables **/ ringbuffer::RingBuffer *_reports; @@ -165,7 +165,7 @@ int IRLOCK::init() } /** allocate buffer storing values read from sensor **/ - _reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct irlock_s)); if (_reports == nullptr) { return ENOTTY; @@ -231,20 +231,20 @@ int IRLOCK::test() warnx("searching for object for 10 seconds"); /** read from sensor for 10 seconds **/ - struct irlock_s obj_report; + struct irlock_s report; uint64_t start_time = hrt_absolute_time(); while ((hrt_absolute_time() - start_time) < 10000000) { - - /** output all objects found **/ - while (_reports->count() > 0) { - _reports->get(&obj_report); - warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", - (int)obj_report.target_num, - (double)obj_report.pos_x, - (double)obj_report.pos_y, - (double)obj_report.size_x, - (double)obj_report.size_y); + if (_reports->get(&report)) { + /** output all objects found **/ + for (uint8_t i = 0; i < report.num_targets; i++) { + warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", + (int)report.targets[i].signature, + (double)report.targets[i].pos_x, + (double)report.targets[i].pos_y, + (double)report.targets[i].size_x, + (double)report.targets[i].size_y); + } } /** sleep for 0.05 seconds **/ @@ -344,20 +344,22 @@ int IRLOCK::read_device() return -ENOTTY; } - /** now read blocks until sync stops, first flush stale queue data **/ - _reports->flush(); - int num_objects = 0; + struct irlock_s report; + + report.timestamp = hrt_absolute_time(); - while (sync_device() && (num_objects < IRLOCK_OBJECTS_MAX)) { - struct irlock_s block; + report.num_targets = 0; - if (read_device_block(&block) != OK) { + while (report.num_targets < IRLOCK_OBJECTS_MAX) { + if (!sync_device() || read_device_block(&report.targets[report.num_targets]) != OK) { break; } - _reports->force(&block); + report.num_targets++; } + _reports->force(&report); + return OK; } @@ -374,33 +376,31 @@ int IRLOCK::read_device_word(uint16_t *word) } /** read a single block (a full frame) from sensor **/ -int IRLOCK::read_device_block(struct irlock_s *block) +int IRLOCK::read_device_block(struct irlock_target_s *block) { uint8_t bytes[12]; memset(bytes, 0, sizeof bytes); int status = transfer(nullptr, 0, &bytes[0], 12); uint16_t checksum = bytes[1] << 8 | bytes[0]; - uint16_t target_num = bytes[3] << 8 | bytes[2]; + uint16_t signature = 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 (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { + if (signature + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { _read_failures++; return -EIO; } /** convert to angles **/ - block->target_num = target_num; + block->signature = signature; block->pos_x = (pixel_x - IRLOCK_CENTER_X) * IRLOCK_TAN_ANG_PER_PIXEL_X; block->pos_y = (pixel_y - IRLOCK_CENTER_Y) * IRLOCK_TAN_ANG_PER_PIXEL_Y; block->size_x = pixel_size_x * IRLOCK_TAN_ANG_PER_PIXEL_X; block->size_y = pixel_size_y * IRLOCK_TAN_ANG_PER_PIXEL_Y; - - block->timestamp = hrt_absolute_time(); return status; } From c2d66c8a1cd27fb91ee263fe59c764c635531ae7 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 7 Jul 2016 10:42:56 -0700 Subject: [PATCH 3/3] irlock: eliminate tanf function in constant --- src/drivers/irlock/irlock.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 721fc92ddfd4..e412bb57cc40 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -76,8 +76,11 @@ #define IRLOCK_FOV_X (60.0f*M_PI_F/180.0f) #define IRLOCK_FOV_Y (35.0f*M_PI_F/180.0f) -#define IRLOCK_TAN_ANG_PER_PIXEL_X (2*tanf(IRLOCK_FOV_X/2)/IRLOCK_RES_X) -#define IRLOCK_TAN_ANG_PER_PIXEL_Y (2*tanf(IRLOCK_FOV_Y/2)/IRLOCK_RES_Y) +#define IRLOCK_TAN_HALF_FOV_X 0.57735026919f // tan(0.5 * 60 * pi/180) +#define IRLOCK_TAN_HALF_FOV_Y 0.31529878887f // tan(0.5 * 35 * pi/180) + +#define IRLOCK_TAN_ANG_PER_PIXEL_X (2*IRLOCK_TAN_HALF_FOV_X/IRLOCK_RES_X) +#define IRLOCK_TAN_ANG_PER_PIXEL_Y (2*IRLOCK_TAN_HALF_FOV_Y/IRLOCK_RES_Y) #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE.