diff --git a/libraries/AP_IRLock/AP_IRLock.h b/libraries/AP_IRLock/AP_IRLock.h
index dad8fb7f02..ecfcc5ae8b 100644
--- a/libraries/AP_IRLock/AP_IRLock.h
+++ b/libraries/AP_IRLock/AP_IRLock.h
@@ -9,7 +9,7 @@
// @brief Catch-all headerthat defines all supported irlock classes.
#include "IRLock.h"
-#include "AP_IRLock_PX4.h"
+#include "AP_IRLock_I2C.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "AP_IRLock_SITL.h"
diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.cpp b/libraries/AP_IRLock/AP_IRLock_I2C.cpp
new file mode 100644
index 0000000000..7dbfb4cd96
--- /dev/null
+++ b/libraries/AP_IRLock/AP_IRLock_I2C.cpp
@@ -0,0 +1,152 @@
+/*
+ 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 .
+ */
+
+/*
+ * AP_IRLock_I2C.cpp
+ *
+ * Based on AP_IRLock_PX4 by MLandes
+ */
+#include
+#include "AP_IRLock_I2C.h"
+#include
+
+extern const AP_HAL::HAL& hal;
+
+#define IRLOCK_I2C_ADDRESS 0x54
+
+#define IRLOCK_SYNC 0xAA55
+#define IRLOCK_RESYNC 0x5500
+#define IRLOCK_ADJUST 0xAA
+
+void AP_IRLock_I2C::init()
+{
+ AP_HAL::OwnPtr tdev = hal.i2c_mgr->get_device(1, IRLOCK_I2C_ADDRESS);
+ if (!tdev || !tdev->get_semaphore()->take(0)) {
+ return;
+ }
+
+ // get initial frame
+ read_frame();
+
+ tdev->get_semaphore()->give();
+
+ if (_flags.healthy) {
+ // read at 50Hz
+ printf("Found IRLock on I2C\n");
+ dev = std::move(tdev);
+
+ sem = hal.util->new_semaphore();
+
+ dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_IRLock_I2C::read_frame, bool));
+ }
+}
+
+/*
+ synchronise with frame start
+*/
+bool AP_IRLock_I2C::sync_frame_start(void)
+{
+ uint16_t sync_word;
+ if (!dev->transfer(nullptr, 0, (uint8_t *)&sync_word, 2)) {
+ return false;
+ }
+ if (sync_word == IRLOCK_SYNC) {
+ return true;
+ }
+ if (sync_word != IRLOCK_RESYNC) {
+ return false;
+ }
+ uint8_t sync_byte;
+ if (!dev->transfer(nullptr, 0, &sync_byte, 1)) {
+ return false;
+ }
+ if (sync_byte == IRLOCK_ADJUST) {
+ return true;
+ }
+ return false;
+}
+
+/*
+ converts IRLOCK pixels to a position on a normal plane 1m in front of the lens
+ based on a characterization of IR-LOCK with the standard lens, focused such that 2.38mm of threads are exposed
+ */
+void AP_IRLock_I2C::pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y)
+{
+ ret_x = (-0.00293875727162397f*pix_x + 0.470201163459835f)/(4.43013552642296e-6f*((pix_x - 160.0f)*(pix_x - 160.0f)) +
+ 4.79331390531725e-6f*((pix_y - 100.0f)*(pix_y - 100.0f)) - 1.0f);
+ ret_y = (-0.003056843086277f*pix_y + 0.3056843086277f)/(4.43013552642296e-6f*((pix_x - 160.0f)*(pix_x - 160.0f)) +
+ 4.79331390531725e-6f*((pix_y - 100.0f)*(pix_y - 100.0f)) - 1.0f);
+}
+
+/*
+ read a frame from sensor
+*/
+bool AP_IRLock_I2C::read_frame(void)
+{
+ if (!sync_frame_start()) {
+ return false;
+ }
+
+ struct frame irframe;
+ if (!dev->transfer(nullptr, 0, (uint8_t*)&irframe, sizeof(irframe))) {
+ return false;
+ }
+
+ /* check crc */
+ if (irframe.signature + irframe.pixel_x + irframe.pixel_y + irframe.pixel_size_x + irframe.pixel_size_y !=
+ irframe.checksum) {
+ return false;
+ }
+
+ int16_t corner1_pix_x = irframe.pixel_x - irframe.pixel_size_x/2;
+ int16_t corner1_pix_y = irframe.pixel_y - irframe.pixel_size_y/2;
+ int16_t corner2_pix_x = irframe.pixel_x + irframe.pixel_size_x/2;
+ int16_t corner2_pix_y = irframe.pixel_y + irframe.pixel_size_y/2;
+
+ float corner1_pos_x, corner1_pos_y, corner2_pos_x, corner2_pos_y;
+ pixel_to_1M_plane(corner1_pix_x, corner1_pix_y, corner1_pos_x, corner1_pos_y);
+ pixel_to_1M_plane(corner2_pix_x, corner2_pix_y, corner2_pos_x, corner2_pos_y);
+
+ if (sem->take(0)) {
+ /* convert to angles */
+ _target_info.timestamp = AP_HAL::millis();
+ _target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x);
+ _target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y);
+ _target_info.size_x = corner2_pos_x-corner1_pos_x;
+ _target_info.size_y = corner2_pos_y-corner1_pos_y;
+ sem->give();
+ }
+
+ return true;
+}
+
+// retrieve latest sensor data - returns true if new data is available
+bool AP_IRLock_I2C::update()
+{
+ bool new_data = false;
+ if (!sem) {
+ return false;
+ }
+ if (sem->take(0)) {
+ if (_last_update_ms != _target_info.timestamp) {
+ new_data = true;
+ }
+ _last_update_ms = _target_info.timestamp;
+ _flags.healthy = (AP_HAL::millis() - _last_update_ms < 100);
+ sem->give();
+ }
+ // return true if new data found
+ return new_data;
+}
diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.h b/libraries/AP_IRLock/AP_IRLock_I2C.h
new file mode 100644
index 0000000000..5a3f52dd75
--- /dev/null
+++ b/libraries/AP_IRLock/AP_IRLock_I2C.h
@@ -0,0 +1,38 @@
+/*
+ * AP_IRLock_I2C.h
+ *
+ */
+#pragma once
+
+#include "IRLock.h"
+
+class AP_IRLock_I2C : public IRLock
+{
+public:
+ // init - initialize sensor library
+ void init();
+
+ // retrieve latest sensor data - returns true if new data is available
+ bool update();
+
+private:
+ AP_HAL::OwnPtr dev;
+
+ struct PACKED frame {
+ uint16_t checksum;
+ uint16_t signature;
+ uint16_t pixel_x;
+ uint16_t pixel_y;
+ uint16_t pixel_size_x;
+ uint16_t pixel_size_y;
+ };
+
+ bool timer(void);
+
+ bool sync_frame_start(void);
+ bool read_frame(void);
+
+ void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y);
+
+ AP_HAL::Semaphore *sem;
+};
diff --git a/libraries/AP_IRLock/AP_IRLock_PX4.cpp b/libraries/AP_IRLock/AP_IRLock_PX4.cpp
deleted file mode 100644
index fda41b6db0..0000000000
--- a/libraries/AP_IRLock/AP_IRLock_PX4.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- 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 .
- */
-
-/*
- * AP_IRLock_PX4.cpp
- *
- * Created on: Nov 16, 2014
- * Author: MLandes
- */
-#include
-#include
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-#include "AP_IRLock_PX4.h"
-
-#include
-#include
-#include "drivers/drv_irlock.h"
-
-extern const AP_HAL::HAL& hal;
-
-AP_IRLock_PX4::AP_IRLock_PX4() :
- _fd(0)
-{}
-
-extern "C" int irlock_main(int, char **);
-
-void AP_IRLock_PX4::init()
-{
- if (!AP_BoardConfig::px4_start_driver(irlock_main, "irlock", "start")) {
- hal.console->printf("irlock driver start failed\n");
-#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
- if (!AP_BoardConfig::px4_start_driver(irlock_main, "irlock", "start -b 2")) {
- hal.console->printf("irlock driver start failed (bus2)\n");
- } else {
- // give it time to initialise
- hal.scheduler->delay(500);
- }
-#endif
- } else {
- // give it time to initialise
- hal.scheduler->delay(500);
- }
-
- _fd = open(IRLOCK0_DEVICE_PATH, O_RDONLY);
- if (_fd < 0) {
- hal.console->printf("Unable to open " IRLOCK0_DEVICE_PATH "\n");
- return;
- }
-
- _flags.healthy = true;
-}
-
-// retrieve latest sensor data - returns true if new data is available
-bool AP_IRLock_PX4::update()
-{
- // return immediately if not healthy
- if (!_flags.healthy) {
- return false;
- }
-
- // read position of all objects
- bool new_data = false;
- struct irlock_s report;
- 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_last_timestamp) {
- // fprintf(stderr, " posx %f posy %f sizex %f sizey %f\n", pkt.pos_x, pkt.pos_y, pkt.size_x, pkt.size_y);
- _target_info[i].timestamp = pkt.timestamp;
- _target_info[i].pos_x = pkt.pos_x;
- _target_info[i].pos_y = pkt.pos_y;
- _target_info[i].size_x = pkt.size_x;
- _target_info[i].size_y = pkt.size_y;
- _last_timestamp = pkt.timestamp;
- _last_update_ms = AP_HAL::millis();
- }
+ bool new_data = false;
+
+ // fprintf(stderr, "sitl %d %d\n", i, _num_targets);
+ if (s == sizeof(irlock_packet) && pkt.timestamp/1000 > _last_timestamp) {
+ // fprintf(stderr, " posx %f posy %f sizex %f sizey %f\n", pkt.pos_x, pkt.pos_y, pkt.size_x, pkt.size_y);
+ _target_info.timestamp = pkt.timestamp / 1000;
+ _target_info.pos_x = pkt.pos_x;
+ _target_info.pos_y = pkt.pos_y;
+ _target_info.size_x = pkt.size_x;
+ _target_info.size_y = pkt.size_y;
+ _last_timestamp = pkt.timestamp/1000;
+ _last_update_ms = _last_timestamp;
+ new_data = true;
}
// return true if new data found
- return (_num_targets > 0);
+ return new_data;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.h b/libraries/AP_IRLock/AP_IRLock_SITL.h
index f7097d67ce..2d7a8aa00e 100644
--- a/libraries/AP_IRLock/AP_IRLock_SITL.h
+++ b/libraries/AP_IRLock/AP_IRLock_SITL.h
@@ -27,14 +27,14 @@ private:
reply packet sent from simulator to ArduPilot
*/
struct irlock_packet {
- uint64_t timestamp;
- uint16_t num_targets;
- float pos_x;
- float pos_y;
- float size_x;
- float size_y;
- };
+ uint64_t timestamp;
+ uint16_t num_targets;
+ float pos_x;
+ float pos_y;
+ float size_x;
+ float size_y;
+ };
- uint64_t _last_timestamp;
+ uint32_t _last_timestamp;
SocketAPM sock;
};
diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp
index 99b2d9c144..7bdddc15d3 100644
--- a/libraries/AP_IRLock/IRLock.cpp
+++ b/libraries/AP_IRLock/IRLock.cpp
@@ -7,32 +7,18 @@
#include "IRLock.h"
-// default constructor
-IRLock::IRLock() :
- _last_update_ms(0),
- _num_targets(0)
-{
- // clear target info
- memset(_target_info, 0, sizeof(_target_info));
-
- // will be adjusted when init is called
- _flags.healthy = false;
-}
-
-IRLock::~IRLock() {}
-
// 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) {
+ if (!_flags.healthy) {
return false;
}
// use data from first (largest) object
- x_angle_rad = atanf(_target_info[0].pos_x);
- y_angle_rad = atanf(_target_info[0].pos_y);
+ x_angle_rad = atanf(_target_info.pos_x);
+ y_angle_rad = atanf(_target_info.pos_y);
return true;
}
@@ -41,14 +27,14 @@ bool IRLock::get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) con
bool IRLock::get_unit_vector_body(Vector3f& ret) const
{
// return false if we have no target
- if (_num_targets == 0) {
+ if (!_flags.healthy) {
return false;
}
// use data from first (largest) object
- ret.x = -_target_info[0].pos_y;
- ret.y = _target_info[0].pos_x;
+ ret.x = -_target_info.pos_y;
+ ret.y = _target_info.pos_x;
ret.z = 1.0f;
ret /= ret.length();
return true;
-}
\ No newline at end of file
+}
diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h
index 4608cc0921..f8e802a8e1 100644
--- a/libraries/AP_IRLock/IRLock.h
+++ b/libraries/AP_IRLock/IRLock.h
@@ -23,14 +23,9 @@
#include
-#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)
-
class IRLock
{
public:
- IRLock();
- virtual ~IRLock();
-
// init - initialize sensor library
// library won't be useable unless this is first called
virtual void init() = 0;
@@ -42,7 +37,7 @@ public:
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; }
+ size_t num_targets() const { return _flags.healthy?1:0; }
// retrieve latest sensor data - returns true if new data is available
virtual bool update() = 0;
@@ -63,16 +58,15 @@ protected:
// internals
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; // microseconds since system start
+ uint32_t timestamp; // milliseconds 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];
+ irlock_target_info _target_info;
};