IRLock: Initial implementation

Define the IRLock sensor object using PX4 level driver

libraries/AP_IRLock/AP_IRLock.h

@@ -0,0 +1,11 @@
+/*
+ * AP_IRLock.h
+ *
+ *  Created on: Nov 10, 2014
+ *      Author: MLandes
+ */
+
+// @file AP_IRLock.h
+// @brief Catch-all headerthat defines all supported irlock classes.
+
+#include "AP_IRLock_PX4.h"

libraries/AP_IRLock/AP_IRLock_PX4.cpp

@@ -0,0 +1,83 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * AP_IRLock_PX4.cpp
+ *
+ *  Created on: Nov 16, 2014
+ *      Author: MLandes
+ */
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include "AP_IRLock_PX4.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#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) :
+		IRLock(ahrs),
+		_fd(0),
+		_last_timestamp(0)
+{}
+
+void AP_IRLock_PX4::init()
+{
+	_fd = open(IRLOCK_DEVICE_PATH, O_RDONLY);
+	if (_fd < 0) {
+		hal.console->printf("Unable to open " IRLOCK_DEVICE_PATH "\n");
+		return;
+	}
+
+	_flags.healthy = true;
+}
+
+void AP_IRLock_PX4::update()
+{
+	if (!_flags.healthy)
+		return;
+
+//	struct irlock_s {
+//		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;
+	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[_num_blocks].center_x = report.center_x;
+		_current_frame[_num_blocks].center_y = report.center_y;
+		_current_frame[_num_blocks].width = report.width;
+		_current_frame[_num_blocks].height = report.height;
+
+		++_num_blocks;
+		_last_timestamp = report.timestamp;
+		_last_update = hal.scheduler->millis();
+	}
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

libraries/AP_IRLock/AP_IRLock_PX4.h

@@ -0,0 +1,27 @@
+/*
+ * AP_IRLock_PX4.h
+ *
+ *  Created on: Nov 10, 2014
+ *      Author: MLandes
+ */
+
+#ifndef AP_IRLOCK_PX4_H_
+#define AP_IRLOCK_PX4_H_
+
+#include "IRLock.h"
+
+class AP_IRLock_PX4 : public IRLock
+{
+public:
+	AP_IRLock_PX4(const AP_AHRS &ahrs);
+
+	virtual void init();
+
+	virtual void update();
+
+private:
+	int _fd;
+	uint64_t _last_timestamp;
+};
+
+#endif /* AP_IRLOCK_PX4_H_ */

libraries/AP_IRLock/IRLock.cpp

@@ -0,0 +1,61 @@
+/*
+ * IRLock.cpp
+ *
+ *  Created on: Nov 12, 2014
+ *      Author: MLandes
+ */
+
+#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) :
+		_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[i]), 0, sizeof(irlock_block));
+	}
+
+	// will be adjusted when init is called
+	_flags.healthy = false;
+}
+
+IRLock::~IRLock() {}
+
+void IRLock::get_current_frame(irlock_block data[IRLOCK_MAX_BLOCKS_PER_FRAME]) const
+{
+	int index = 0;
+	for (; index < _num_blocks; ++index) {
+		data[index].signature = _current_frame[index].signature;
+		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) {
+		data[index].signature = 0;
+		data[index].center_x = 0;
+		data[index].center_y = 0;
+		data[index].width = 0;
+		data[index].height = 0;
+	}
+}

libraries/AP_IRLock/IRLock.h

@@ -0,0 +1,89 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * IRLock.h - IRLock Base Class for Ardupilot
+ *
+ *  Created on: Nov 10, 2014
+ *      Author: MLandes
+ */
+
+#ifndef __IRLOCK_H__
+#define __IRLOCK_H__
+
+#include "AP_AHRS.h"
+
+#define IRLOCK_MAX_BLOCKS_PER_FRAME 135
+
+struct _irlock_block {
+	uint16_t signature;
+	uint16_t center_x;
+	uint16_t center_y;
+	uint16_t width;
+	uint16_t height;
+};
+
+typedef struct _irlock_block irlock_block;
+
+class IRLock
+{
+public:
+	IRLock(const AP_AHRS &ahrs);
+	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; }
+
+	// timestamp of most recent data read from the sensor
+	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; }
+
+	// retrieve latest sensor data
+	virtual void update() = 0;
+
+	// copies the current data frame
+	void get_current_frame(irlock_block data[IRLOCK_MAX_BLOCKS_PER_FRAME]) 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;
+	irlock_block _current_frame[IRLOCK_MAX_BLOCKS_PER_FRAME];
+};
+
+
+#endif /* __IRLOCK_H__ */