Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

IRLock: adjust to px4 lib calculating bf angles

This commit is contained in:
Randy Mackay 2015-08-30 12:36:55 +09:00
parent 04c682542c
commit 6f3ff0866f
3 changed files with 32 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_IRLock/AP_IRLock_PX4.cpp
View File

@ -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

12
libraries/AP_IRLock/IRLock.cpp
View File

@ -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;
}

34
libraries/AP_IRLock/IRLock.h
View File

@ -25,21 +25,8 @@
#include <AP_AHRS/AP_AHRS.h>
#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];
};