IRLock: many changes to integrate with PrecLand library

irlock_center_x_pos calculates the lateral x position of the marker in cm. relative to the quad
irlock_x_pos_to_latlon rotates the frame based x position to latitude/longtitude based coordinates
same case for the y position
add get_angle_to_target method
get_angle_to_target replaces pixel to position calculations
Also removed ahrs reference (now in AC_PrecLand lib), unused references
to orb and commented out parameter declaration
reduce max objects to 5
remove ahrs reference
add timeout
return true if new sample found
bug fix get_angle_to_target
remove unused get_frame

libraries/AP_IRLock/AP_IRLock_PX4.cpp

@@ -29,55 +29,57 @@
 
 #include "AP_HAL.h"
 #include "drivers/drv_irlock.h"
-#include "uORB/topics/irlock.h"
 
 extern const AP_HAL::HAL& hal;
 
-AP_IRLock_PX4::AP_IRLock_PX4(const AP_AHRS &ahrs) :
+AP_IRLock_PX4::AP_IRLock_PX4() :
-		IRLock(ahrs),
 		_fd(0),
 		_last_timestamp(0)
 {}
 
 void AP_IRLock_PX4::init()
 {
-	_fd = open(IRLOCK_DEVICE_PATH, O_RDONLY);
+	_fd = open(IRLOCK0_DEVICE_PATH, O_RDONLY);
 	if (_fd < 0) {
-		hal.console->printf("Unable to open " IRLOCK_DEVICE_PATH "\n");
+		hal.console->printf("Unable to open " IRLOCK0_DEVICE_PATH "\n");
 		return;
 	}
 
 	_flags.healthy = true;
 }
 
-void AP_IRLock_PX4::update()
+// retrieve latest sensor data - returns true if new data is available
+bool AP_IRLock_PX4::update()
 {
-	if (!_flags.healthy)
+    // return immediately if not healthy
-		return;
+	if (!_flags.healthy) {
+		return false;
+	}
 
-//	struct irlock_s {
+	// read position of all objects
-//		uint64_t timestamp; // microseconds since system start
-//
-//		uint16_t signature;
-//		uint16_t center_x;
-//		uint16_t center_y;
-//		uint16_t width;
-//		uint16_t height;
-//		uint16_t angle;
-//	};
 	struct irlock_s report;
-	_num_blocks = 0;
+	uint16_t count = 0;
 	while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s) && report.timestamp >_last_timestamp) {
-		_current_frame[_num_blocks].signature = report.signature;
+		_current_frame[count].signature = report.signature;
-		_current_frame[_num_blocks].center_x = report.center_x;
+		_current_frame[count].center_x = report.center_x;
-		_current_frame[_num_blocks].center_y = report.center_y;
+		_current_frame[count].center_y = report.center_y;
-		_current_frame[_num_blocks].width = report.width;
+		_current_frame[count].width = report.width;
-		_current_frame[_num_blocks].height = report.height;
+		_current_frame[count].height = report.height;
 
-		++_num_blocks;
+		count++;
 		_last_timestamp = report.timestamp;
 		_last_update = hal.scheduler->millis();
 	}
+
+	// update num_blocks and implement timeout
+	if (count > 0) {
+	    _num_blocks = count;
+	} else if ((hal.scheduler->millis() - _last_update) > IRLOCK_TIMEOUT_MS) {
+	    _num_blocks = 0;
+	}
+
+	// return true if new data found
+	return (_num_blocks > 0);
 }
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

libraries/AP_IRLock/AP_IRLock_PX4.h

@@ -13,11 +13,13 @@
 class AP_IRLock_PX4 : public IRLock
 {
 public:
-	AP_IRLock_PX4(const AP_AHRS &ahrs);
+	AP_IRLock_PX4();
 
+	// init - initialize sensor library
 	virtual void init();
 
-	virtual void update();
+	// retrieve latest sensor data - returns true if new data is available
+	virtual bool update();
 
 private:
 	int _fd;

libraries/AP_IRLock/IRLock.cpp

@@ -7,32 +7,13 @@
 
 #include "IRLock.h"
 
-// not sure what is going on here...
-//const AP_Param::GroupInfo IRLock::var_info[] PROGMEM = {
-//		// @Param: ENABLE
-//		// @DisplayName: Optical flow enable/disable
-//		// @Description: Setting thisto Enabled(1) will enable irlock. Setting this to Disabled(0) will disable irlock.
-//		// @Values: 0:Disabled, 1:Enabled
-//		// @user: Standard
-//		AP_GROUPINFO("_ENABLE", 0, IRLock, _enabled, 0),
-//
-//		AP_GROUPEND
-//};
-
 // default constructor
-IRLock::IRLock(const AP_AHRS &ahrs) :
+IRLock::IRLock() :
-		_ahrs(ahrs),
 		_last_update(0),
 		_num_blocks(0)
 {
-	// can't figure out how to get this to set to enabled, so doing it manually
-//	AP_Param::setup_object_defaults(this, var_info);
-	_enabled = true;
-
 	// clear the frame buffer
-	for(int i = 0; i < IRLOCK_MAX_BLOCKS_PER_FRAME; ++i) {
+    memset(_current_frame, 0, sizeof(_current_frame));
-		memset(&(_current_frame[i]), 0, sizeof(irlock_block));
-	}
 
 	// will be adjusted when init is called
 	_flags.healthy = false;
@@ -40,22 +21,17 @@ IRLock::IRLock(const AP_AHRS &ahrs) :
 
 IRLock::~IRLock() {}
 
-void IRLock::get_current_frame(irlock_block data[IRLOCK_MAX_BLOCKS_PER_FRAME]) const
+// 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
 {
-	int index = 0;
+    // return false if we have no target
-	for (; index < _num_blocks; ++index) {
+    if (_num_blocks == 0) {
-		data[index].signature = _current_frame[index].signature;
+        return false;
-		data[index].center_x = _current_frame[index].center_x;
-		data[index].center_y = _current_frame[index].center_y;
-		data[index].width = _current_frame[index].width;
-		data[index].height = _current_frame[index].height;
     }
 
-	for (; index < IRLOCK_MAX_BLOCKS_PER_FRAME; ++index) {
+    // use data from first object
-		data[index].signature = 0;
+    x_angle_rad = (((float)(_current_frame[0].center_x-IRLOCK_CENTER_X))/IRLOCK_X_PIXEL_PER_DEGREE) * DEG_TO_RAD;
-		data[index].center_x = 0;
+    y_angle_rad = (((float)(_current_frame[0].center_y-IRLOCK_CENTER_Y))/IRLOCK_Y_PIXEL_PER_DEGREE) * DEG_TO_RAD;
-		data[index].center_y = 0;
+    return true;
-		data[index].width = 0;
-		data[index].height = 0;
-	}
 }

libraries/AP_IRLock/IRLock.h

@@ -25,31 +25,32 @@
 
 #include "AP_AHRS.h"
 
-#define IRLOCK_MAX_BLOCKS_PER_FRAME 135
+#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
 
-struct _irlock_block {
+typedef struct {
 	uint16_t signature;
 	uint16_t center_x;
 	uint16_t center_y;
 	uint16_t width;
 	uint16_t height;
-};
+} irlock_block;
-
-typedef struct _irlock_block irlock_block;
 
 class IRLock
 {
 public:
-	IRLock(const AP_AHRS &ahrs);
+	IRLock();
 	virtual ~IRLock();
 
 	// init - initialize sensor library
 	// library won't be useable unless this is first called
 	virtual void init() = 0;
 
-	// true if irlock is enabled
-	bool enabled() const { return _enabled; }
-
 	// true if irlock sensor is online and healthy
 	bool healthy() const { return _flags.healthy; }
 
@@ -59,29 +60,21 @@ public:
 	// returns the number of blocks in the current frame
 	size_t num_blocks() const { return _num_blocks; }
 
-	// retrieve latest sensor data
+	// retrieve latest sensor data - returns true if new data is available
-	virtual void update() = 0;
+	virtual bool update() = 0;
 
-	// copies the current data frame
+	// get_angle_to_target - retrieve body frame x and y angles (in radians) to target
-	void get_current_frame(irlock_block data[IRLOCK_MAX_BLOCKS_PER_FRAME]) const;
+	//  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;
-	// parameter var info table
-	static const struct AP_Param::GroupInfo var_info[];
 
 protected:
 	struct AP_IRLock_Flags {
 		uint8_t healthy : 1; // true if sensor is healthy
 	} _flags;
 
-	// external references
-	const AP_AHRS &_ahrs;
-
-	// parameters
-	AP_Int8 _enabled;
-
 	// internals
 	uint32_t _last_update;
-	size_t _num_blocks;
+	uint16_t _num_blocks;
 	irlock_block _current_frame[IRLOCK_MAX_BLOCKS_PER_FRAME];
 };