From 6f3ff0866f7f497f19f12a0d870b3c002f5832ab Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 30 Aug 2015 12:36:55 +0900 Subject: [PATCH] IRLock: adjust to px4 lib calculating bf angles --- libraries/AP_IRLock/AP_IRLock_PX4.cpp | 20 ++++++++-------- libraries/AP_IRLock/IRLock.cpp | 12 +++++----- libraries/AP_IRLock/IRLock.h | 34 +++++++++++++-------------- 3 files changed, 32 insertions(+), 34 deletions(-) diff --git a/libraries/AP_IRLock/AP_IRLock_PX4.cpp b/libraries/AP_IRLock/AP_IRLock_PX4.cpp index 267b623d99..9b823350b9 100644 --- a/libraries/AP_IRLock/AP_IRLock_PX4.cpp +++ b/libraries/AP_IRLock/AP_IRLock_PX4.cpp @@ -31,7 +31,7 @@ extern const AP_HAL::HAL& hal; AP_IRLock_PX4::AP_IRLock_PX4() : - _fd(0), + _fd(0), _last_timestamp(0) {} @@ -58,12 +58,12 @@ bool AP_IRLock_PX4::update() struct irlock_s report; uint16_t count = 0; while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s) && report.timestamp >_last_timestamp) { - _current_frame[count].signature = report.signature; - _current_frame[count].center_x = report.center_x; - _current_frame[count].center_y = report.center_y; - _current_frame[count].width = report.width; - _current_frame[count].height = report.height; - + _target_info[count].timestamp = report.timestamp; + _target_info[count].target_num = report.target_num; + _target_info[count].angle_x = report.angle_x; + _target_info[count].angle_y = report.angle_y; + _target_info[count].size_x = report.size_x; + _target_info[count].size_y = report.size_y; count++; _last_timestamp = report.timestamp; _last_update = hal.scheduler->millis(); @@ -71,13 +71,13 @@ bool AP_IRLock_PX4::update() // update num_blocks and implement timeout if (count > 0) { - _num_blocks = count; + _num_targets = count; } else if ((hal.scheduler->millis() - _last_update) > IRLOCK_TIMEOUT_MS) { - _num_blocks = 0; + _num_targets = 0; } // return true if new data found - return (_num_blocks > 0); + return (_num_targets > 0); } #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp index ed70f88f0f..64916f28ba 100644 --- a/libraries/AP_IRLock/IRLock.cpp +++ b/libraries/AP_IRLock/IRLock.cpp @@ -10,10 +10,10 @@ // default constructor IRLock::IRLock() : _last_update(0), - _num_blocks(0) + _num_targets(0) { - // clear the frame buffer - memset(_current_frame, 0, sizeof(_current_frame)); + // clear target info + memset(_target_info, 0, sizeof(_target_info)); // will be adjusted when init is called _flags.healthy = false; @@ -26,12 +26,12 @@ IRLock::~IRLock() {} bool IRLock::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const { // return false if we have no target - if (_num_blocks == 0) { + if (_num_targets == 0) { return false; } // use data from first object - x_angle_rad = (((float)(_current_frame[0].center_x-IRLOCK_CENTER_X))/IRLOCK_X_PIXEL_PER_DEGREE) * DEG_TO_RAD; - y_angle_rad = (((float)(_current_frame[0].center_y-IRLOCK_CENTER_Y))/IRLOCK_Y_PIXEL_PER_DEGREE) * DEG_TO_RAD; + x_angle_rad = _target_info[0].angle_x; + y_angle_rad = _target_info[0].angle_y; return true; } diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h index 9cd1d113f5..2ea58cc8fb 100644 --- a/libraries/AP_IRLock/IRLock.h +++ b/libraries/AP_IRLock/IRLock.h @@ -25,21 +25,8 @@ #include -#define IRLOCK_MAX_BLOCKS_PER_FRAME 5 // max number of blobs that can be detected by IR-LOCK sensor (should match PX4Firmware's irlock driver's IRLOCK_OBJECTS_MAX) -#define IRLOCK_CENTER_X 159 // the center x pixel value -#define IRLOCK_CENTER_Y 99 // the center y pixel value -#define IRLOCK_NOBLOB_FRAME 10 // the number of consecutive similar frames that will cause the sensor validity variable to turn false -#define IRLOCK_X_PIXEL_PER_DEGREE 5.374f // the x pixel to angle calibration variable -#define IRLOCK_Y_PIXEL_PER_DEGREE 5.698f // the y pixel to angle calibration variable -#define IRLOCK_TIMEOUT_MS 100 // remove all blocks if no data received within 0.1 seconds - -typedef struct { - uint16_t signature; - uint16_t center_x; - uint16_t center_y; - uint16_t width; - uint16_t height; -} irlock_block; +#define IRLOCK_MAX_TARGETS 5 // max number of targets that can be detected by IR-LOCK sensor (should match PX4Firmware's irlock driver's IRLOCK_OBJECTS_MAX) +#define IRLOCK_TIMEOUT_MS 100 // target info times out after 0.1 seconds class IRLock { @@ -58,7 +45,7 @@ public: uint32_t last_update() const { return _last_update; } // returns the number of blocks in the current frame - size_t num_blocks() const { return _num_blocks; } + size_t num_targets() const { return _num_targets; } // retrieve latest sensor data - returns true if new data is available virtual bool update() = 0; @@ -74,8 +61,19 @@ protected: // internals uint32_t _last_update; - uint16_t _num_blocks; - irlock_block _current_frame[IRLOCK_MAX_BLOCKS_PER_FRAME]; + uint16_t _num_targets; + + // irlock_target_info is a duplicate of the PX4Firmware irlock_s structure + typedef struct { + uint64_t timestamp; // time target was seen in 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 + } irlock_target_info; + + irlock_target_info _target_info[IRLOCK_MAX_TARGETS]; };