From 210ad9e36cf813f1cf07563482b42b07a8b8980d Mon Sep 17 00:00:00 2001 From: Michael Landes Date: Mon, 17 Nov 2014 00:04:45 -0500 Subject: [PATCH 1/4] irlock: initial version of IR-LOCK sensor driver Also works with the Pixy Cam --- src/drivers/drv_irlock.h | 65 +++++ src/drivers/irlock/irlock.cpp | 499 ++++++++++++++++++++++++++++++++++ src/drivers/irlock/module.mk | 42 +++ 3 files changed, 606 insertions(+) create mode 100644 src/drivers/drv_irlock.h create mode 100644 src/drivers/irlock/irlock.cpp create mode 100644 src/drivers/irlock/module.mk diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h new file mode 100644 index 0000000000..612bf9464a --- /dev/null +++ b/src/drivers/drv_irlock.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * drv_irlock.h + * + * Created on: Nov 12, 2014 + * Author: MLandes + */ + +#ifndef _DRV_IRLOCK_H +#define _DRV_IRLOCK_H + +#include +#include + +#include "drv_sensor.h" // include sensor driver interfaces +#include "drv_orb_dev.h" + +#define IRLOCK_DEVICE_PATH "/dev/irlock" + +/* + * ObjDev tag for irlock data. + */ +ORB_DECLARE(irlock_data); + +/* + * ioctl() definitions + * + * custom irlock sensor driver interface (none currently) + */ +#define _IRLOCKIOCBASE (0x7800) +#define __IRLOCKIOC(_n) (_IOC(_IRLOCKIOCBASE, _n)) + +#endif /* _DRV_IRLOCK_H */ diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp new file mode 100644 index 0000000000..45aab2cdc1 --- /dev/null +++ b/src/drivers/irlock/irlock.cpp @@ -0,0 +1,499 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file irlock.cpp + * @author Michael Landes + * + * Driver for the IRLock sensor connected via I2C. + * + * Created on: Nov 12, 2014 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "drivers/boards/px4fmu-v2/board_config.h" +#include "drivers/device/i2c.h" +#include "drivers/device/ringbuffer.h" +#include "drivers/drv_irlock.h" +#include "drivers/drv_hrt.h" + +#include "nuttx/clock.h" +#include "nuttx/wqueue.h" +#include "systemlib/err.h" + +#include "uORB/topics/irlock.h" +#include "uORB/topics/subsystem_info.h" +#include "uORB/uORB.h" + +/* Configuration Constants */ +#define IRLOCK_BUS PX4_I2C_BUS_EXPANSION +#define I2C_IRLOCK_ADDRESS 0x65 //* 7-bit address (non shifted) +#define IRLOCK_CONVERSION_INTERVAL 20000 /* us = 20ms = 50Hz */ + +#define IRLOCK_SYNC 0xAA55 +#define IRLOCK_RESYNC 0x5500 +#define IRLOCK_ADJUST 0xAA + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class IRLOCK : public device::I2C +{ +public: + IRLOCK(int bus = IRLOCK_BUS, int address = I2C_IRLOCK_ADDRESS); + virtual ~IRLOCK(); + + virtual int init(); + + void print_diagnostics(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + +protected: + virtual int probe(); + +private: + // static callback for the work queue, arg will be pointer to instance of this class + static void cycle_trampoline(void *arg); + + RingBuffer *_reports; + orb_advert_t _irlock_topic; + bool _sensor_ok; + int _measure_ticks; + work_s _work; + + // irlock device io + int read_device(); + bool sync_device(); + int read_device_word(uint16_t *word); + int read_device_block(struct irlock_s *block); + + // driver control + void start(); + void stop(); + void cycle(); +}; + +extern "C" __EXPORT int irlock_main(int argc, char *argv[]); + +IRLOCK::IRLOCK(int bus, int address) : + I2C("IRLOCK", IRLOCK_DEVICE_PATH, bus, address, 400000), // 400 khz - supposedly pointless number... + _reports(nullptr), + _irlock_topic(-1), + _sensor_ok(false), + _measure_ticks(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +IRLOCK::~IRLOCK() +{ + stop(); + + if (_reports != nullptr) { + delete _reports; + } +} + +int IRLOCK::init() +{ + int status = ERROR; + + if (I2C::init() != OK) { + goto finish; + } + + _reports = new RingBuffer(135, sizeof(struct irlock_s)); + if (_reports == nullptr) + goto finish; + + status = OK; + _sensor_ok = true; + +finish: + return status; +} + +bool IRLOCK::sync_device() +{ + uint8_t sync_byte; + uint16_t sync_word; + + if (read_device_word(&sync_word) != OK) + return false; + + if (sync_word == IRLOCK_RESYNC) { + transfer(nullptr, 0, &sync_byte, 1); + if (sync_byte == IRLOCK_ADJUST) + return true; + } else if (sync_word == IRLOCK_SYNC) { + return true; + } + + return false; +} + +int IRLOCK::probe() +{ + /* + * IRLock defaults to sending 0x00 when there is no block + * data to return, so really all we can do is check to make + * sure a transfer completes successfully. + */ + uint8_t byte; + if (transfer(nullptr, 0, &byte, 1) != OK) { + return -EIO; + } + + return OK; +} + +int IRLOCK::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch(cmd) { + case SENSORIOCSPOLLRATE: // set poll rate (and start polling) + switch (arg) { + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + bool stopped = (_measure_ticks == 0); + + _measure_ticks = USEC2TICK(IRLOCK_CONVERSION_INTERVAL); + if (stopped) { + start(); + } + + return OK; + } + // not yet supporting manual, custom or external polling + case SENSOR_POLLRATE_MANUAL: + warnx("manual polling not currently supported"); + return -EINVAL; + case SENSOR_POLLRATE_EXTERNAL: + warnx("external polling not currently supported"); + return -EINVAL; + default: + warnx("custom poll-rates not currently supported"); + return -EINVAL; + } + warnx("set poll rate"); + return OK; + case SENSORIOCRESET: + warnx("reset not implemented, use stop and then start"); + return -EINVAL; + default: + warnx("default ioctl call"); + return OK; // I2C::ioctl(filp, cmd, arg); - causes errors for some reason + } +} + +ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct irlock_s); + struct irlock_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + if (count < 1) + return -ENOSPC; + + // only try to read if we are using automatic polling in the driver + if (_measure_ticks > 0) { + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + ++rbuf; + } + } + + return ret ? ret : -EAGAIN; + } + + return ret; +} + +int IRLOCK::read_device() +{ + // if we sync, then we are starting a new frame, else fail + if (!sync_device()) + return ERROR; + + // now read blocks until sync stops, first flush stale queue data + _reports->flush(); + while (sync_device()) { + struct irlock_s block; + if (read_device_block(&block) != OK) + break; + + _reports->force(&block); + } + + return OK; +} + +int IRLOCK::read_device_word(uint16_t *word) +{ + uint8_t bytes[2]; + memset(bytes, 0, sizeof bytes); + + int status = transfer(nullptr, 0, &bytes[0], 2); + *word = bytes[1] << 8 | bytes[0]; + + return status; +} + +int IRLOCK::read_device_block(struct irlock_s *block) +{ + uint8_t bytes[12]; + memset(bytes, 0, sizeof bytes); + + int status = transfer(nullptr, 0, &bytes[0], 12); + uint16_t checksum = bytes[1] << 8 | bytes[0]; + block->signature = bytes[3] << 8 | bytes[2]; + block->center_x = bytes[5] << 8 | bytes[4]; + block->center_y = bytes[7] << 8 | bytes[6]; + block->width = bytes[9] << 8 | bytes[8]; + block->height = bytes[11] << 8 | bytes[10]; + + if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) + return -EIO; + + block->timestamp = hrt_absolute_time(); + return status; +} + +void IRLOCK::start() +{ + // flush ring and reset state machine + _reports->flush(); + + // start work queue cycle + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); +} + +void IRLOCK::stop() +{ + warnx("stopping queue work"); + work_cancel(HPWORK, &_work); +} + +void IRLOCK::cycle_trampoline(void *arg) +{ + IRLOCK *device = (IRLOCK*)arg; + device->cycle(); +} + +void IRLOCK::cycle() +{ + // ignoring failure, if we do, we will be back again right away... + read_device(); + + // schedule the next cycle + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL)); +} + +void IRLOCK::print_diagnostics() +{ + if (!_sensor_ok) + warnx("sensor is not ok"); + + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue: "); +} + +namespace irlock +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +IRLOCK *g_dev = nullptr; + +void start(); +void stop(); +void test(); +void reset(); +void info(); +void usage(); + +void start() +{ + int fd; + + if (g_dev != nullptr) { + warnx("irlock device driver has already been started"); + exit(OK); + } + + g_dev = new IRLOCK(IRLOCK_BUS, I2C_IRLOCK_ADDRESS); + if (g_dev == nullptr) { + warnx("failed to instantiate device reference"); + goto fail; + } + + if (g_dev->init() != OK) { + warnx("failed to initialize device"); + goto fail; + } + + fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); + if (fd < 0) { + warnx("could not open device file"); + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + warnx("set poll rate on device failed"); + goto fail; + } + + exit(OK); + +fail: + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + exit(ERROR); +} + +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } else { + warnx("irlock device driver was not running"); + } + + exit(OK); +} + +void reset() +{ + int fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + errx(ERROR, "driver access failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + errx(ERROR, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + errx(ERROR, "driver poll restart failed"); + + exit(OK); +} + +void test() +{ + errx(1, "test - not implmented"); +} + +void info() +{ + if (g_dev == nullptr) { + errx(1, "irlock device driver is not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_diagnostics(); + + exit(0); +} + +void usage() +{ + warnx("missing command: try 'start', 'stop', 'reset', 'test', 'info'"); +} + +} // namespace irlock + +int irlock_main(int argc, char *argv[]) +{ + const char *command = argv[1]; + + /* + * Start/load the driver + */ + if (!strcmp(command, "start")) { + irlock::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(command, "stop")) { + irlock::stop(); + } + + /* + * Reset the driver + */ + if (!strcmp(command, "reset")) { + irlock::reset(); + } + + /* + * Test the driver/device + */ + if (!strcmp(command, "test")) { + irlock::test(); + } + + /* + * Print driver information + */ + if (!strcmp(command, "info") || !strcmp(command, "status")) { + irlock::info(); + } + + irlock::usage(); +} diff --git a/src/drivers/irlock/module.mk b/src/drivers/irlock/module.mk new file mode 100644 index 0000000000..da3e1e42a1 --- /dev/null +++ b/src/drivers/irlock/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the IRLock driver. +# + +MODULE_COMMAND = irlock + +SRCS = irlock.cpp + +MAXOPTIMIZATION = -Os From 10139400d501229bf62016b83574808e4aadff60 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 1 Mar 2015 16:32:58 +0900 Subject: [PATCH 2/4] irlock: simplify driver Remove ioctl calls by always starting cycling Remove unused orb variables and includes Remove unused angle from irlock_s structure Add test and set I2C address to pixy default Reduce max num objects to 5 Add read errors reporting via nsh --- src/drivers/drv_irlock.h | 34 +- src/drivers/irlock/irlock.cpp | 569 ++++++++++++++++------------------ 2 files changed, 283 insertions(+), 320 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 612bf9464a..9f1487a311 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -32,34 +32,30 @@ ****************************************************************************/ /* - * drv_irlock.h + * @file drv_irlock.h * - * Created on: Nov 12, 2014 - * Author: MLandes + * IR-Lock device API */ -#ifndef _DRV_IRLOCK_H -#define _DRV_IRLOCK_H +#pragma once #include #include #include "drv_sensor.h" // include sensor driver interfaces -#include "drv_orb_dev.h" -#define IRLOCK_DEVICE_PATH "/dev/irlock" +#define IRLOCK_BASE_DEVICE_PATH "/dev/irlock" +#define IRLOCK0_DEVICE_PATH "/dev/irlock0" -/* - * ObjDev tag for irlock data. - */ -ORB_DECLARE(irlock_data); +#define IRLOCK_OBJECTS_MAX 5 /* up to 5 objects can be detected/reported */ -/* - * ioctl() definitions - * - * custom irlock sensor driver interface (none currently) - */ -#define _IRLOCKIOCBASE (0x7800) -#define __IRLOCKIOC(_n) (_IOC(_IRLOCKIOCBASE, _n)) +/* irlock_s structure returned from read calls */ +struct irlock_s { + uint64_t timestamp; // microseconds since system start -#endif /* _DRV_IRLOCK_H */ + uint16_t signature; + uint16_t center_x; + uint16_t center_y; + uint16_t width; + uint16_t height; +}; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 45aab2cdc1..8ccffc426a 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -35,9 +35,9 @@ * @file irlock.cpp * @author Michael Landes * - * Driver for the IRLock sensor connected via I2C. + * Driver for an IR-Lock and Pixy vision sensor connected via I2C. * - * Created on: Nov 12, 2014 + * Created on: Nov 12, 2014 */ #include @@ -48,35 +48,25 @@ #include #include -#include "drivers/boards/px4fmu-v2/board_config.h" -#include "drivers/device/i2c.h" -#include "drivers/device/ringbuffer.h" -#include "drivers/drv_irlock.h" -#include "drivers/drv_hrt.h" +#include +#include +#include +#include +#include -#include "nuttx/clock.h" -#include "nuttx/wqueue.h" -#include "systemlib/err.h" - -#include "uORB/topics/irlock.h" -#include "uORB/topics/subsystem_info.h" -#include "uORB/uORB.h" +#include +#include +#include /* Configuration Constants */ -#define IRLOCK_BUS PX4_I2C_BUS_EXPANSION -#define I2C_IRLOCK_ADDRESS 0x65 //* 7-bit address (non shifted) -#define IRLOCK_CONVERSION_INTERVAL 20000 /* us = 20ms = 50Hz */ +#define IRLOCK_I2C_BUS PX4_I2C_BUS_EXPANSION +#define IRLOCK_I2C_ADDRESS 0x54 //* 7-bit address (non shifted) +#define IRLOCK_CONVERSION_INTERVAL_US 20000U /* us = 20ms = 50Hz */ #define IRLOCK_SYNC 0xAA55 #define IRLOCK_RESYNC 0x5500 #define IRLOCK_ADJUST 0xAA -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -84,100 +74,99 @@ static const int ERROR = -1; class IRLOCK : public device::I2C { public: - IRLOCK(int bus = IRLOCK_BUS, int address = I2C_IRLOCK_ADDRESS); + IRLOCK(int bus = IRLOCK_I2C_BUS, int address = IRLOCK_I2C_ADDRESS); virtual ~IRLOCK(); virtual int init(); + virtual int probe(); + virtual int info(); + virtual int test(); - void print_diagnostics(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); -protected: - virtual int probe(); - private: - // static callback for the work queue, arg will be pointer to instance of this class - static void cycle_trampoline(void *arg); - RingBuffer *_reports; - orb_advert_t _irlock_topic; - bool _sensor_ok; - int _measure_ticks; - work_s _work; + /* start periodic reads from sensor */ + void start(); - // irlock device io + /* stop periodic reads from sensor */ + void stop(); + + /* static function that is called by worker queue, arg will be pointer to instance of this class */ + static void cycle_trampoline(void *arg); + + /* read from device and schedule next read */ + void cycle(); \ + + /* low level communication with sensor */ int read_device(); bool sync_device(); int read_device_word(uint16_t *word); int read_device_block(struct irlock_s *block); - // driver control - void start(); - void stop(); - void cycle(); + /* internal variables */ + RingBuffer *_reports; + bool _sensor_ok; + work_s _work; + uint32_t _read_failures; }; +/* global pointer for single IRLOCK sensor */ +namespace +{ +IRLOCK *g_irlock = nullptr; +} + +void irlock_usage(); + extern "C" __EXPORT int irlock_main(int argc, char *argv[]); +/* constructor */ IRLOCK::IRLOCK(int bus, int address) : - I2C("IRLOCK", IRLOCK_DEVICE_PATH, bus, address, 400000), // 400 khz - supposedly pointless number... - _reports(nullptr), - _irlock_topic(-1), - _sensor_ok(false), - _measure_ticks(0) + I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), + _reports(nullptr), + _sensor_ok(false), + _read_failures(0) { memset(&_work, 0, sizeof(_work)); } +/* destructor */ IRLOCK::~IRLOCK() { stop(); + /* clear reports queue */ if (_reports != nullptr) { delete _reports; } } +/* initialise driver to communicate with sensor */ int IRLOCK::init() { - int status = ERROR; + /* initialise I2C bus */ + int ret = I2C::init(); - if (I2C::init() != OK) { - goto finish; + if (ret != OK) { + return ret; } - _reports = new RingBuffer(135, sizeof(struct irlock_s)); - if (_reports == nullptr) - goto finish; + /* allocate buffer storing values read from sensor */ + _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); - status = OK; - _sensor_ok = true; + if (_reports == nullptr) { + return ENOTTY; -finish: - return status; -} - -bool IRLOCK::sync_device() -{ - uint8_t sync_byte; - uint16_t sync_word; - - if (read_device_word(&sync_word) != OK) - return false; - - if (sync_word == IRLOCK_RESYNC) { - transfer(nullptr, 0, &sync_byte, 1); - if (sync_byte == IRLOCK_ADJUST) - return true; - } else if (sync_word == IRLOCK_SYNC) { - return true; + } else { + _sensor_ok = true; + /* start work queue */ + start(); + return OK; } - - return false; } +/* probe the device is on the I2C bus */ int IRLOCK::probe() { /* @@ -186,6 +175,7 @@ int IRLOCK::probe() * sure a transfer completes successfully. */ uint8_t byte; + if (transfer(nullptr, 0, &byte, 1) != OK) { return -EIO; } @@ -193,42 +183,98 @@ int IRLOCK::probe() return OK; } -int IRLOCK::ioctl(struct file *filp, int cmd, unsigned long arg) +/* display driver info */ +int IRLOCK::info() { - switch(cmd) { - case SENSORIOCSPOLLRATE: // set poll rate (and start polling) - switch (arg) { - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - bool stopped = (_measure_ticks == 0); - - _measure_ticks = USEC2TICK(IRLOCK_CONVERSION_INTERVAL); - if (stopped) { - start(); - } - - return OK; - } - // not yet supporting manual, custom or external polling - case SENSOR_POLLRATE_MANUAL: - warnx("manual polling not currently supported"); - return -EINVAL; - case SENSOR_POLLRATE_EXTERNAL: - warnx("external polling not currently supported"); - return -EINVAL; - default: - warnx("custom poll-rates not currently supported"); - return -EINVAL; - } - warnx("set poll rate"); - return OK; - case SENSORIOCRESET: - warnx("reset not implemented, use stop and then start"); - return -EINVAL; - default: - warnx("default ioctl call"); - return OK; // I2C::ioctl(filp, cmd, arg); - causes errors for some reason + if (g_irlock == nullptr) { + errx(1, "irlock device driver is not running"); } + + /* display reports in queue */ + if (_sensor_ok) { + _reports->print_info("report queue: "); + warnx("read errors:%lu", (unsigned long)_read_failures); + + } else { + warnx("sensor is not healthy"); + } + + return OK; +} + +/* test driver */ +int IRLOCK::test() +{ + /* exit immediately if driver not running */ + if (g_irlock == nullptr) { + errx(1, "irlock device driver is not running"); + } + + /* exit immediately if sensor is not healty */ + if (!_sensor_ok) { + errx(1, "sensor is not healthy"); + } + + /* instructions to user */ + warnx("searching for object for 10 seconds"); + + /* read from sensor for 10 seconds */ + struct irlock_s obj_report; + uint64_t start_time = hrt_absolute_time(); + + while ((hrt_absolute_time() - start_time) < 10000000) { + + /* output all objects found */ + while (_reports->count() > 0) { + _reports->get(&obj_report); + warnx("sig:%d x:%d y:%d width:%d height:%d", + (int)obj_report.signature, + (int)obj_report.center_x, + (int)obj_report.center_y, + (int)obj_report.width, + (int)obj_report.height); + } + + /* sleep for 0.05 seconds */ + usleep(50000); + } + + return OK; +} + +/* start periodic reads from sensor */ +void IRLOCK::start() +{ + // flush ring and reset state machine + _reports->flush(); + + // start work queue cycle + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); +} + +/* stop periodic reads from sensor */ +void IRLOCK::stop() +{ + work_cancel(HPWORK, &_work); +} + +void IRLOCK::cycle_trampoline(void *arg) +{ + IRLOCK *device = (IRLOCK *)arg; + + /* check global irlock reference and cycle */ + if (g_irlock != nullptr) { + device->cycle(); + } +} + +void IRLOCK::cycle() +{ + // ignoring failure, if we do, we will be back again right away... + read_device(); + + // schedule the next cycle + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL_US)); } ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) @@ -237,36 +283,65 @@ ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) struct irlock_s *rbuf = reinterpret_cast(buffer); int ret = 0; - if (count < 1) + if (count < 1) { return -ENOSPC; - - // only try to read if we are using automatic polling in the driver - if (_measure_ticks > 0) { - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - ++rbuf; - } - } - - return ret ? ret : -EAGAIN; } + // try to read + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + ++rbuf; + } + } + + return ret ? ret : -EAGAIN; + return ret; } +/* sync device to ensure reading starts at new frame*/ +bool IRLOCK::sync_device() +{ + uint8_t sync_byte; + uint16_t sync_word; + + if (read_device_word(&sync_word) != OK) { + return false; + } + + if (sync_word == IRLOCK_RESYNC) { + transfer(nullptr, 0, &sync_byte, 1); + + if (sync_byte == IRLOCK_ADJUST) { + return true; + } + + } else if (sync_word == IRLOCK_SYNC) { + return true; + } + + return false; +} + +/* read all available frames from sensor */ int IRLOCK::read_device() { // if we sync, then we are starting a new frame, else fail - if (!sync_device()) - return ERROR; + if (!sync_device()) { + return -ENOTTY; + } // now read blocks until sync stops, first flush stale queue data _reports->flush(); - while (sync_device()) { + int num_objects = 0; + + while (sync_device() && (num_objects < IRLOCK_OBJECTS_MAX)) { struct irlock_s block; - if (read_device_block(&block) != OK) + + if (read_device_block(&block) != OK) { break; + } _reports->force(&block); } @@ -274,6 +349,7 @@ int IRLOCK::read_device() return OK; } +/* read a word (two bytes) from sensor */ int IRLOCK::read_device_word(uint16_t *word) { uint8_t bytes[2]; @@ -285,6 +361,7 @@ int IRLOCK::read_device_word(uint16_t *word) return status; } +/* read a single block (a full frame) from sensor */ int IRLOCK::read_device_block(struct irlock_s *block) { uint8_t bytes[12]; @@ -298,202 +375,92 @@ int IRLOCK::read_device_block(struct irlock_s *block) block->width = bytes[9] << 8 | bytes[8]; block->height = bytes[11] << 8 | bytes[10]; - if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) + // crc check + if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { + _read_failures++; return -EIO; - - block->timestamp = hrt_absolute_time(); - return status; -} - -void IRLOCK::start() -{ - // flush ring and reset state machine - _reports->flush(); - - // start work queue cycle - work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); -} - -void IRLOCK::stop() -{ - warnx("stopping queue work"); - work_cancel(HPWORK, &_work); -} - -void IRLOCK::cycle_trampoline(void *arg) -{ - IRLOCK *device = (IRLOCK*)arg; - device->cycle(); -} - -void IRLOCK::cycle() -{ - // ignoring failure, if we do, we will be back again right away... - read_device(); - - // schedule the next cycle - work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL)); -} - -void IRLOCK::print_diagnostics() -{ - if (!_sensor_ok) - warnx("sensor is not ok"); - - printf("poll interval: %u ticks\n", _measure_ticks); - _reports->print_info("report queue: "); -} - -namespace irlock -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -IRLOCK *g_dev = nullptr; - -void start(); -void stop(); -void test(); -void reset(); -void info(); -void usage(); - -void start() -{ - int fd; - - if (g_dev != nullptr) { - warnx("irlock device driver has already been started"); - exit(OK); } - g_dev = new IRLOCK(IRLOCK_BUS, I2C_IRLOCK_ADDRESS); - if (g_dev == nullptr) { - warnx("failed to instantiate device reference"); - goto fail; - } - - if (g_dev->init() != OK) { - warnx("failed to initialize device"); - goto fail; - } - - fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); - if (fd < 0) { - warnx("could not open device file"); - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - warnx("set poll rate on device failed"); - goto fail; - } - - exit(OK); - -fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - exit(ERROR); + block->timestamp = hrt_absolute_time(); + return status; } -void stop() +void irlock_usage() { - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } else { - warnx("irlock device driver was not running"); - } - - exit(OK); + warnx("missing command: try 'start', 'stop', 'info', 'test'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", IRLOCK_I2C_BUS); } -void reset() -{ - int fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - errx(ERROR, "driver access failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - errx(ERROR, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - errx(ERROR, "driver poll restart failed"); - - exit(OK); -} - -void test() -{ - errx(1, "test - not implmented"); -} - -void info() -{ - if (g_dev == nullptr) { - errx(1, "irlock device driver is not running"); - } - - printf("state @ %p\n", g_dev); - g_dev->print_diagnostics(); - - exit(0); -} - -void usage() -{ - warnx("missing command: try 'start', 'stop', 'reset', 'test', 'info'"); -} - -} // namespace irlock - int irlock_main(int argc, char *argv[]) { - const char *command = argv[1]; + int i2cdevice = IRLOCK_I2C_BUS; - /* - * Start/load the driver - */ + /* jump over start/off/etc and look at options first */ + if (getopt(argc, argv, "b:") != EOF) { + i2cdevice = (int)strtol(optarg, NULL, 0); + } + + if (optind >= argc) { + irlock_usage(); + exit(1); + } + + const char *command = argv[optind]; + + /* start driver */ if (!strcmp(command, "start")) { - irlock::start(); + if (g_irlock != nullptr) { + errx(1, "driver has already been started"); + } + + /* instantiate global instance */ + g_irlock = new IRLOCK(i2cdevice, IRLOCK_I2C_ADDRESS); + + if (g_irlock == nullptr) { + errx(1, "failed to allocated memory for driver"); + } + + /* initialise global instance */ + if (g_irlock->init() != OK) { + IRLOCK *tmp_irlock = g_irlock; + g_irlock = nullptr; + delete tmp_irlock; + errx(1, "failed to initialize device, stopping driver"); + } + + exit(0); } - /* - * Stop the driver - */ + /* need the driver past this point */ + if (g_irlock == nullptr) { + warnx("not started"); + irlock_usage(); + exit(1); + } + + /* stop the driver */ if (!strcmp(command, "stop")) { - irlock::stop(); + IRLOCK *tmp_irlock = g_irlock; + g_irlock = nullptr; + delete tmp_irlock; + warnx("irlock stopped"); + exit(OK); } - /* - * Reset the driver - */ - if (!strcmp(command, "reset")) { - irlock::reset(); + /* Print driver information */ + if (!strcmp(command, "info")) { + g_irlock->info(); + exit(OK); } - /* - * Test the driver/device - */ + /* test driver */ if (!strcmp(command, "test")) { - irlock::test(); + g_irlock->test(); + exit(OK); } - /* - * Print driver information - */ - if (!strcmp(command, "info") || !strcmp(command, "status")) { - irlock::info(); - } - - irlock::usage(); + /* display usage info */ + irlock_usage(); + exit(0); } From 54308288f3da09f8e17ad6acdb495c3317a3cfe7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 May 2015 14:34:18 +0900 Subject: [PATCH 3/4] irlock: formatting fixes --- src/drivers/drv_irlock.h | 12 ++-- src/drivers/irlock/irlock.cpp | 126 +++++++++++++++++----------------- src/drivers/irlock/module.mk | 33 --------- 3 files changed, 69 insertions(+), 102 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 9f1487a311..06cf5c8645 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,11 +31,11 @@ * ****************************************************************************/ -/* +/** * @file drv_irlock.h * * IR-Lock device API - */ + **/ #pragma once @@ -47,11 +47,11 @@ #define IRLOCK_BASE_DEVICE_PATH "/dev/irlock" #define IRLOCK0_DEVICE_PATH "/dev/irlock0" -#define IRLOCK_OBJECTS_MAX 5 /* up to 5 objects can be detected/reported */ +#define IRLOCK_OBJECTS_MAX 5 /** up to 5 objects can be detected/reported **/ -/* irlock_s structure returned from read calls */ +/** irlock_s structure returned from read calls **/ struct irlock_s { - uint64_t timestamp; // microseconds since system start + uint64_t timestamp; /** microseconds since system start **/ uint16_t signature; uint16_t center_x; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 8ccffc426a..d22d9a38e4 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,14 +31,14 @@ * ****************************************************************************/ -/* +/** * @file irlock.cpp * @author Michael Landes * * Driver for an IR-Lock and Pixy vision sensor connected via I2C. * * Created on: Nov 12, 2014 - */ + **/ #include #include @@ -58,10 +58,10 @@ #include #include -/* Configuration Constants */ +/** Configuration Constants **/ #define IRLOCK_I2C_BUS PX4_I2C_BUS_EXPANSION -#define IRLOCK_I2C_ADDRESS 0x54 //* 7-bit address (non shifted) -#define IRLOCK_CONVERSION_INTERVAL_US 20000U /* us = 20ms = 50Hz */ +#define IRLOCK_I2C_ADDRESS 0x54 /** 7-bit address (non shifted) **/ +#define IRLOCK_CONVERSION_INTERVAL_US 20000U /** us = 20ms = 50Hz **/ #define IRLOCK_SYNC 0xAA55 #define IRLOCK_RESYNC 0x5500 @@ -86,32 +86,32 @@ public: private: - /* start periodic reads from sensor */ - void start(); + /** start periodic reads from sensor **/ + void start(); - /* stop periodic reads from sensor */ - void stop(); + /** stop periodic reads from sensor **/ + void stop(); - /* static function that is called by worker queue, arg will be pointer to instance of this class */ - static void cycle_trampoline(void *arg); + /** static function that is called by worker queue, arg will be pointer to instance of this class **/ + static void cycle_trampoline(void *arg); - /* read from device and schedule next read */ - void cycle(); \ + /** read from device and schedule next read **/ + void cycle(); - /* low level communication with sensor */ - int read_device(); - bool sync_device(); - int read_device_word(uint16_t *word); - int read_device_block(struct irlock_s *block); + /** low level communication with sensor **/ + int read_device(); + bool sync_device(); + int read_device_word(uint16_t *word); + int read_device_block(struct irlock_s *block); - /* internal variables */ + /** internal variables **/ RingBuffer *_reports; bool _sensor_ok; work_s _work; uint32_t _read_failures; }; -/* global pointer for single IRLOCK sensor */ +/** global pointer for single IRLOCK sensor **/ namespace { IRLOCK *g_irlock = nullptr; @@ -121,7 +121,7 @@ void irlock_usage(); extern "C" __EXPORT int irlock_main(int argc, char *argv[]); -/* constructor */ +/** constructor **/ IRLOCK::IRLOCK(int bus, int address) : I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), _reports(nullptr), @@ -131,28 +131,28 @@ IRLOCK::IRLOCK(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -/* destructor */ +/** destructor **/ IRLOCK::~IRLOCK() { stop(); - /* clear reports queue */ + /** clear reports queue **/ if (_reports != nullptr) { delete _reports; } } -/* initialise driver to communicate with sensor */ +/** initialise driver to communicate with sensor **/ int IRLOCK::init() { - /* initialise I2C bus */ + /** initialise I2C bus **/ int ret = I2C::init(); if (ret != OK) { return ret; } - /* allocate buffer storing values read from sensor */ + /** allocate buffer storing values read from sensor **/ _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); if (_reports == nullptr) { @@ -160,20 +160,20 @@ int IRLOCK::init() } else { _sensor_ok = true; - /* start work queue */ + /** start work queue **/ start(); return OK; } } -/* probe the device is on the I2C bus */ +/** probe the device is on the I2C bus **/ int IRLOCK::probe() { /* * IRLock defaults to sending 0x00 when there is no block * data to return, so really all we can do is check to make * sure a transfer completes successfully. - */ + **/ uint8_t byte; if (transfer(nullptr, 0, &byte, 1) != OK) { @@ -183,14 +183,14 @@ int IRLOCK::probe() return OK; } -/* display driver info */ +/** display driver info **/ int IRLOCK::info() { if (g_irlock == nullptr) { errx(1, "irlock device driver is not running"); } - /* display reports in queue */ + /** display reports in queue **/ if (_sensor_ok) { _reports->print_info("report queue: "); warnx("read errors:%lu", (unsigned long)_read_failures); @@ -202,29 +202,29 @@ int IRLOCK::info() return OK; } -/* test driver */ +/** test driver **/ int IRLOCK::test() { - /* exit immediately if driver not running */ + /** exit immediately if driver not running **/ if (g_irlock == nullptr) { errx(1, "irlock device driver is not running"); } - /* exit immediately if sensor is not healty */ + /** exit immediately if sensor is not healty **/ if (!_sensor_ok) { errx(1, "sensor is not healthy"); } - /* instructions to user */ + /** instructions to user **/ warnx("searching for object for 10 seconds"); - /* read from sensor for 10 seconds */ + /** read from sensor for 10 seconds **/ struct irlock_s obj_report; uint64_t start_time = hrt_absolute_time(); while ((hrt_absolute_time() - start_time) < 10000000) { - /* output all objects found */ + /** output all objects found **/ while (_reports->count() > 0) { _reports->get(&obj_report); warnx("sig:%d x:%d y:%d width:%d height:%d", @@ -235,24 +235,24 @@ int IRLOCK::test() (int)obj_report.height); } - /* sleep for 0.05 seconds */ + /** sleep for 0.05 seconds **/ usleep(50000); } return OK; } -/* start periodic reads from sensor */ +/** start periodic reads from sensor **/ void IRLOCK::start() { - // flush ring and reset state machine + /** flush ring and reset state machine **/ _reports->flush(); - // start work queue cycle + /** start work queue cycle **/ work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); } -/* stop periodic reads from sensor */ +/** stop periodic reads from sensor **/ void IRLOCK::stop() { work_cancel(HPWORK, &_work); @@ -262,7 +262,7 @@ void IRLOCK::cycle_trampoline(void *arg) { IRLOCK *device = (IRLOCK *)arg; - /* check global irlock reference and cycle */ + /** check global irlock reference and cycle **/ if (g_irlock != nullptr) { device->cycle(); } @@ -270,10 +270,10 @@ void IRLOCK::cycle_trampoline(void *arg) void IRLOCK::cycle() { - // ignoring failure, if we do, we will be back again right away... + /** ignoring failure, if we do, we will be back again right away... **/ read_device(); - // schedule the next cycle + /** schedule the next cycle **/ work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL_US)); } @@ -287,7 +287,7 @@ ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; } - // try to read + /** try to read **/ while (count--) { if (_reports->get(rbuf)) { ret += sizeof(*rbuf); @@ -300,7 +300,7 @@ ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) return ret; } -/* sync device to ensure reading starts at new frame*/ +/** sync device to ensure reading starts at new frame*/ bool IRLOCK::sync_device() { uint8_t sync_byte; @@ -324,15 +324,15 @@ bool IRLOCK::sync_device() return false; } -/* read all available frames from sensor */ +/** read all available frames from sensor **/ int IRLOCK::read_device() { - // if we sync, then we are starting a new frame, else fail + /** if we sync, then we are starting a new frame, else fail **/ if (!sync_device()) { return -ENOTTY; } - // now read blocks until sync stops, first flush stale queue data + /** now read blocks until sync stops, first flush stale queue data **/ _reports->flush(); int num_objects = 0; @@ -349,7 +349,7 @@ int IRLOCK::read_device() return OK; } -/* read a word (two bytes) from sensor */ +/** read a word (two bytes) from sensor **/ int IRLOCK::read_device_word(uint16_t *word) { uint8_t bytes[2]; @@ -361,7 +361,7 @@ int IRLOCK::read_device_word(uint16_t *word) return status; } -/* read a single block (a full frame) from sensor */ +/** read a single block (a full frame) from sensor **/ int IRLOCK::read_device_block(struct irlock_s *block) { uint8_t bytes[12]; @@ -375,7 +375,7 @@ int IRLOCK::read_device_block(struct irlock_s *block) block->width = bytes[9] << 8 | bytes[8]; block->height = bytes[11] << 8 | bytes[10]; - // crc check + /** crc check **/ if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { _read_failures++; return -EIO; @@ -396,7 +396,7 @@ int irlock_main(int argc, char *argv[]) { int i2cdevice = IRLOCK_I2C_BUS; - /* jump over start/off/etc and look at options first */ + /** jump over start/off/etc and look at options first **/ if (getopt(argc, argv, "b:") != EOF) { i2cdevice = (int)strtol(optarg, NULL, 0); } @@ -408,20 +408,20 @@ int irlock_main(int argc, char *argv[]) const char *command = argv[optind]; - /* start driver */ + /** start driver **/ if (!strcmp(command, "start")) { if (g_irlock != nullptr) { errx(1, "driver has already been started"); } - /* instantiate global instance */ + /** instantiate global instance **/ g_irlock = new IRLOCK(i2cdevice, IRLOCK_I2C_ADDRESS); if (g_irlock == nullptr) { errx(1, "failed to allocated memory for driver"); } - /* initialise global instance */ + /** initialise global instance **/ if (g_irlock->init() != OK) { IRLOCK *tmp_irlock = g_irlock; g_irlock = nullptr; @@ -432,14 +432,14 @@ int irlock_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + /** need the driver past this point **/ if (g_irlock == nullptr) { warnx("not started"); irlock_usage(); exit(1); } - /* stop the driver */ + /** stop the driver **/ if (!strcmp(command, "stop")) { IRLOCK *tmp_irlock = g_irlock; g_irlock = nullptr; @@ -448,19 +448,19 @@ int irlock_main(int argc, char *argv[]) exit(OK); } - /* Print driver information */ + /** Print driver information **/ if (!strcmp(command, "info")) { g_irlock->info(); exit(OK); } - /* test driver */ + /** test driver **/ if (!strcmp(command, "test")) { g_irlock->test(); exit(OK); } - /* display usage info */ + /** display usage info **/ irlock_usage(); exit(0); } diff --git a/src/drivers/irlock/module.mk b/src/drivers/irlock/module.mk index da3e1e42a1..5d9fbf4539 100644 --- a/src/drivers/irlock/module.mk +++ b/src/drivers/irlock/module.mk @@ -1,36 +1,3 @@ -############################################################################ -# -# Copyright (c) 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - # # Makefile to build the IRLock driver. # From 7dcef40516bc173fe54e85517551c6d9de1fa9b9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 May 2015 14:52:11 +0900 Subject: [PATCH 4/4] irlock: report target in radians --- src/drivers/drv_irlock.h | 11 +++++------ src/drivers/irlock/irlock.cpp | 36 +++++++++++++++++++++++------------ 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 06cf5c8645..fa6b3016cd 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -52,10 +52,9 @@ /** irlock_s structure returned from read calls **/ 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 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 **/ }; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index d22d9a38e4..4aeb8c7497 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -67,6 +67,11 @@ #define IRLOCK_RESYNC 0x5500 #define IRLOCK_ADJUST 0xAA +#define IRLOCK_CENTER_X 159 // the x-axis center pixel position +#define IRLOCK_CENTER_Y 99 // the y-axis center pixel position +#define IRLOCK_PIXELS_PER_RADIAN_X 307.9075f // x-axis pixel to radian scaler assuming 60deg FOV on x-axis +#define IRLOCK_PIXELS_PER_RADIAN_Y 326.4713f // y-axis pixel to radian scaler assuming 35deg FOV on y-axis + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -227,12 +232,12 @@ int IRLOCK::test() /** output all objects found **/ while (_reports->count() > 0) { _reports->get(&obj_report); - warnx("sig:%d x:%d y:%d width:%d height:%d", - (int)obj_report.signature, - (int)obj_report.center_x, - (int)obj_report.center_y, - (int)obj_report.width, - (int)obj_report.height); + warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", + (int)obj_report.target_num, + (double)obj_report.angle_x, + (double)obj_report.angle_y, + (double)obj_report.size_x, + (double)obj_report.size_y); } /** sleep for 0.05 seconds **/ @@ -369,18 +374,25 @@ int IRLOCK::read_device_block(struct irlock_s *block) int status = transfer(nullptr, 0, &bytes[0], 12); uint16_t checksum = bytes[1] << 8 | bytes[0]; - block->signature = bytes[3] << 8 | bytes[2]; - block->center_x = bytes[5] << 8 | bytes[4]; - block->center_y = bytes[7] << 8 | bytes[6]; - block->width = bytes[9] << 8 | bytes[8]; - block->height = bytes[11] << 8 | bytes[10]; + uint16_t target_num = bytes[3] << 8 | bytes[2]; + uint16_t pixel_x = bytes[5] << 8 | bytes[4]; + uint16_t pixel_y = bytes[7] << 8 | bytes[6]; + uint16_t pixel_size_x = bytes[9] << 8 | bytes[8]; + uint16_t pixel_size_y = bytes[11] << 8 | bytes[10]; /** crc check **/ - if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { + if (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { _read_failures++; return -EIO; } + /** convert to angles **/ + block->target_num = target_num; + block->angle_x = (((float)(pixel_x-IRLOCK_CENTER_X))/IRLOCK_PIXELS_PER_RADIAN_X); + block->angle_y = (((float)(pixel_y-IRLOCK_CENTER_Y))/IRLOCK_PIXELS_PER_RADIAN_Y); + block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X; + block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y; + block->timestamp = hrt_absolute_time(); return status; }