mirror of https://github.com/ArduPilot/ardupilot
IRLock: Initial implementation
Define the IRLock sensor object using PX4 level driver
This commit is contained in:
Michael Landes
Randy Mackay
parent
2769487ac0
commit
59928ed677
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
|
|
@ -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_ */
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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__ */
|
||||
Loading…
Reference in New Issue