AP_IRLock: refactor interface

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

libraries/AP_IRLock/AP_IRLock_PX4.h

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

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

libraries/AP_IRLock/IRLock.h

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