From 6254608c451ae14a93ef63058bcb133294ae48d2 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jul 2016 12:20:35 -0700 Subject: [PATCH] AP_IRLock: refactor interface --- libraries/AP_IRLock/AP_IRLock_PX4.cpp | 35 +++++++++++---------------- libraries/AP_IRLock/AP_IRLock_PX4.h | 1 - libraries/AP_IRLock/IRLock.cpp | 31 ++++++++++++++++++------ libraries/AP_IRLock/IRLock.h | 27 ++++++++++++--------- 4 files changed, 53 insertions(+), 41 deletions(-) diff --git a/libraries/AP_IRLock/AP_IRLock_PX4.cpp b/libraries/AP_IRLock/AP_IRLock_PX4.cpp index 24bc95d2b9..63454ef56b 100644 --- a/libraries/AP_IRLock/AP_IRLock_PX4.cpp +++ b/libraries/AP_IRLock/AP_IRLock_PX4.cpp @@ -31,8 +31,7 @@ extern const AP_HAL::HAL& hal; AP_IRLock_PX4::AP_IRLock_PX4() : - _fd(0), - _last_timestamp(0) + _fd(0) {} void AP_IRLock_PX4::init() @@ -55,29 +54,23 @@ bool AP_IRLock_PX4::update() } // read position of all objects + bool new_data = false; struct irlock_s report; - uint16_t count = 0; - while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s) && report.timestamp >_last_timestamp) { - _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 = AP_HAL::millis(); - } - - // update num_blocks and implement timeout - if (count > 0) { - _num_targets = count; - } else if ((AP_HAL::millis() - _last_update) > IRLOCK_TIMEOUT_MS) { - _num_targets = 0; + while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s)) { + new_data = true; + _num_targets = report.num_targets; + for (uint8_t i=0; i 0); + return new_data; } #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_IRLock/AP_IRLock_PX4.h b/libraries/AP_IRLock/AP_IRLock_PX4.h index f9168dd53e..e78c1dac4b 100644 --- a/libraries/AP_IRLock/AP_IRLock_PX4.h +++ b/libraries/AP_IRLock/AP_IRLock_PX4.h @@ -21,5 +21,4 @@ public: private: int _fd; - uint64_t _last_timestamp; }; diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp index be90300479..99b2d9c144 100644 --- a/libraries/AP_IRLock/IRLock.cpp +++ b/libraries/AP_IRLock/IRLock.cpp @@ -9,7 +9,7 @@ // default constructor IRLock::IRLock() : - _last_update(0), + _last_update_ms(0), _num_targets(0) { // clear target info @@ -21,17 +21,34 @@ IRLock::IRLock() : IRLock::~IRLock() {} -// get_angle_to_target - retrieve body frame x and y angles (in radians) to target -// returns true if angles are available, false if not (i.e. no target) -bool IRLock::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const +// retrieve body frame x and y angles (in radians) to target +// returns true if data is available +bool IRLock::get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const { // return false if we have no target if (_num_targets == 0) { return false; } - // use data from first object - x_angle_rad = _target_info[0].angle_x; - y_angle_rad = _target_info[0].angle_y; + // use data from first (largest) object + x_angle_rad = atanf(_target_info[0].pos_x); + y_angle_rad = atanf(_target_info[0].pos_y); return true; } + +// retrieve body frame unit vector in direction of target +// returns true if data is available +bool IRLock::get_unit_vector_body(Vector3f& ret) const +{ + // return false if we have no target + if (_num_targets == 0) { + return false; + } + + // use data from first (largest) object + ret.x = -_target_info[0].pos_y; + ret.y = _target_info[0].pos_x; + ret.z = 1.0f; + ret /= ret.length(); + return true; +} \ No newline at end of file diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h index d134ebf184..4608cc0921 100644 --- a/libraries/AP_IRLock/IRLock.h +++ b/libraries/AP_IRLock/IRLock.h @@ -24,7 +24,6 @@ #include #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 { @@ -40,7 +39,7 @@ public: bool healthy() const { return _flags.healthy; } // timestamp of most recent data read from the sensor - uint32_t last_update() const { return _last_update; } + uint32_t last_update_ms() const { return _last_update_ms; } // returns the number of blocks in the current frame size_t num_targets() const { return _num_targets; } @@ -48,9 +47,14 @@ public: // retrieve latest sensor data - returns true if new data is available virtual bool update() = 0; - // get_angle_to_target - retrieve body frame x and y angles (in radians) to target - // returns true if angles are available, false if not (i.e. no target) - bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const; + // retrieve body frame x and y angles (in radians) to target + // returns true if data is available + bool get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) const; + + // retrieve body frame unit vector in direction of target + // returns true if data is available + bool get_unit_vector_body(Vector3f& ret) const; + protected: struct AP_IRLock_Flags { @@ -58,17 +62,16 @@ protected: } _flags; // internals - uint32_t _last_update; + uint32_t _last_update_ms; 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 + uint64_t timestamp; // microseconds since system start + 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_target_info; irlock_target_info _target_info[IRLOCK_MAX_TARGETS];