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

AP_IRLock: refactor interface

This commit is contained in:
Jonathan Challinger 2016-07-05 12:20:35 -07:00 committed by Randy Mackay
parent d16e641709
commit 6254608c45
4 changed files with 53 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_IRLock/AP_IRLock_PX4.cpp
View File

@ -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<report.num_targets; i++) {
_target_info[i].timestamp = report.timestamp;
_target_info[i].pos_x = report.targets[i].pos_x;
_target_info[i].pos_y = report.targets[i].pos_y;
_target_info[i].size_x = report.targets[i].size_x;
_target_info[i].size_y = report.targets[i].size_y;
}
_last_update_ms = AP_HAL::millis();
}
// return true if new data found
return (_num_targets > 0);
return new_data;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

1
libraries/AP_IRLock/AP_IRLock_PX4.h
View File

@ -21,5 +21,4 @@ public:
private:
int _fd;
uint64_t _last_timestamp;
};

31
libraries/AP_IRLock/IRLock.cpp
View File

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

27
libraries/AP_IRLock/IRLock.h
View File

@ -24,7 +24,6 @@
#include <AP_AHRS/AP_AHRS.h>
#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];