IRLock: adjust to px4 lib calculating bf angles

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

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

libraries/AP_IRLock/IRLock.h

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