Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IRLock: new in-tree IRLock driver on I2C

This commit is contained in:
Andrew Tridgell 2016-11-26 16:22:18 +11:00
parent ca75f1fc12
commit c6ea451c56
9 changed files with 223 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_IRLock/AP_IRLock.h
View File

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

152
libraries/AP_IRLock/AP_IRLock_I2C.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* AP_IRLock_I2C.cpp
*
* Based on AP_IRLock_PX4 by MLandes
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_IRLock_I2C.h"
#include <stdio.h>
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<AP_HAL::Device> 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;
}

38
libraries/AP_IRLock/AP_IRLock_I2C.h Normal file
View File

@ -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<AP_HAL::Device> 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;
};

94
libraries/AP_IRLock/AP_IRLock_PX4.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* AP_IRLock_PX4.cpp
*
* Created on: Nov 16, 2014
* Author: MLandes
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_IRLock_PX4.h"
#include <fcntl.h>
#include <unistd.h>
#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<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 new_data;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

24
libraries/AP_IRLock/AP_IRLock_PX4.h
View File

@ -1,24 +0,0 @@
/*
* AP_IRLock_PX4.h
*
* Created on: Nov 10, 2014
* Author: MLandes
*/
#pragma once
#include "IRLock.h"
class AP_IRLock_PX4 : public IRLock
{
public:
AP_IRLock_PX4();
// init - initialize sensor library
virtual void init();
// retrieve latest sensor data - returns true if new data is available
virtual bool update();
private:
int _fd;
};

33
libraries/AP_IRLock/AP_IRLock_SITL.cpp
View File

@ -64,28 +64,23 @@ bool AP_IRLock_SITL::update()
const int wait_ms = 0;
size_t s = sock.recv(&pkt, sizeof(irlock_packet), wait_ms);
// TODO: need to fix this
// for some reason, pkt.num_targets occasionally gets a bad number.
// hardcode _num_targets to 1 for now.
// _num_targets = pkt.num_targets;
_num_targets = 1;
for (uint16_t i=0; i<_num_targets; ++i) {
// fprintf(stderr, "sitl %d %d\n", i, _num_targets);
if (s == sizeof(irlock_packet) && pkt.timestamp >_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

16
libraries/AP_IRLock/AP_IRLock_SITL.h
View File

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

28
libraries/AP_IRLock/IRLock.cpp
View File

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

12
libraries/AP_IRLock/IRLock.h
View File

@ -23,14 +23,9 @@
#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)
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;
};