Skip to content
This repository has been archived by the owner on Oct 1, 2021. It is now read-only.

IR-LOCK: rework driver #59

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

IR-LOCK: rework driver #59

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0bda5c6
irlock: change output format to tangent of angles
jschall Jul 5, 2016
bd97824
irlock: put all targets in single struct
jschall Jul 6, 2016
c2d66c8
irlock: eliminate tanf function in constant
jschall Jul 7, 2016
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
15 changes: 10 additions & 5 deletions src/drivers/drv_irlock.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,17 @@

#define IRLOCK_OBJECTS_MAX 5 /** up to 5 objects can be detected/reported **/

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 **/
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 **/
uint8_t num_targets;
struct irlock_target_s targets[IRLOCK_OBJECTS_MAX];
};
75 changes: 41 additions & 34 deletions src/drivers/irlock/irlock.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

have you checked that the compiler does the tanf() call at build time?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have not and I suspect that it probably does not.

#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.
Expand Down Expand Up @@ -107,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;
Expand Down Expand Up @@ -158,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;
Expand Down Expand Up @@ -224,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.angle_x,
(double)obj_report.angle_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 **/
Expand Down Expand Up @@ -337,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;
}

Expand All @@ -367,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->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();
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;
return status;
}

Expand Down