mirror of https://github.com/ArduPilot/ardupilot
parent
7db77482f5
commit
2351e84592
|
@ -31,53 +31,53 @@
|
|||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
AP_IRLock_PX4::AP_IRLock_PX4() :
|
||||
_fd(0),
|
||||
_last_timestamp(0)
|
||||
_fd(0),
|
||||
_last_timestamp(0)
|
||||
{}
|
||||
|
||||
void AP_IRLock_PX4::init()
|
||||
{
|
||||
_fd = open(IRLOCK0_DEVICE_PATH, O_RDONLY);
|
||||
if (_fd < 0) {
|
||||
hal.console->printf("Unable to open " IRLOCK0_DEVICE_PATH "\n");
|
||||
return;
|
||||
}
|
||||
_fd = open(IRLOCK0_DEVICE_PATH, O_RDONLY);
|
||||
if (_fd < 0) {
|
||||
hal.console->printf("Unable to open " IRLOCK0_DEVICE_PATH "\n");
|
||||
return;
|
||||
}
|
||||
|
||||
_flags.healthy = true;
|
||||
_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;
|
||||
}
|
||||
if (!_flags.healthy) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// read position of all objects
|
||||
struct irlock_s report;
|
||||
uint16_t count = 0;
|
||||
while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s) && report.timestamp >_last_timestamp) {
|
||||
_target_info[count].timestamp = report.timestamp;
|
||||
_target_info[count].target_num = report.target_num;
|
||||
_target_info[count].angle_x = report.angle_x;
|
||||
_target_info[count].angle_y = report.angle_y;
|
||||
_target_info[count].size_x = report.size_x;
|
||||
_target_info[count].size_y = report.size_y;
|
||||
count++;
|
||||
_last_timestamp = report.timestamp;
|
||||
_last_update = hal.scheduler->millis();
|
||||
}
|
||||
// read position of all objects
|
||||
struct irlock_s report;
|
||||
uint16_t count = 0;
|
||||
while(::read(_fd, &report, sizeof(struct irlock_s)) == sizeof(struct irlock_s) && report.timestamp >_last_timestamp) {
|
||||
_target_info[count].timestamp = report.timestamp;
|
||||
_target_info[count].target_num = report.target_num;
|
||||
_target_info[count].angle_x = report.angle_x;
|
||||
_target_info[count].angle_y = report.angle_y;
|
||||
_target_info[count].size_x = report.size_x;
|
||||
_target_info[count].size_y = report.size_y;
|
||||
count++;
|
||||
_last_timestamp = report.timestamp;
|
||||
_last_update = hal.scheduler->millis();
|
||||
}
|
||||
|
||||
// update num_blocks and implement timeout
|
||||
if (count > 0) {
|
||||
_num_targets = count;
|
||||
} else if ((hal.scheduler->millis() - _last_update) > IRLOCK_TIMEOUT_MS) {
|
||||
_num_targets = 0;
|
||||
}
|
||||
// update num_blocks and implement timeout
|
||||
if (count > 0) {
|
||||
_num_targets = count;
|
||||
} else if ((hal.scheduler->millis() - _last_update) > IRLOCK_TIMEOUT_MS) {
|
||||
_num_targets = 0;
|
||||
}
|
||||
|
||||
// return true if new data found
|
||||
return (_num_targets > 0);
|
||||
// return true if new data found
|
||||
return (_num_targets > 0);
|
||||
}
|
||||
|
||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
|
|
|
@ -13,17 +13,17 @@
|
|||
class AP_IRLock_PX4 : public IRLock
|
||||
{
|
||||
public:
|
||||
AP_IRLock_PX4();
|
||||
AP_IRLock_PX4();
|
||||
|
||||
// init - initialize sensor library
|
||||
virtual void init();
|
||||
// init - initialize sensor library
|
||||
virtual void init();
|
||||
|
||||
// retrieve latest sensor data - returns true if new data is available
|
||||
virtual bool update();
|
||||
// retrieve latest sensor data - returns true if new data is available
|
||||
virtual bool update();
|
||||
|
||||
private:
|
||||
int _fd;
|
||||
uint64_t _last_timestamp;
|
||||
int _fd;
|
||||
uint64_t _last_timestamp;
|
||||
};
|
||||
|
||||
#endif /* AP_IRLOCK_PX4_H_ */
|
||||
|
|
|
@ -9,14 +9,14 @@
|
|||
|
||||
// default constructor
|
||||
IRLock::IRLock() :
|
||||
_last_update(0),
|
||||
_num_targets(0)
|
||||
_last_update(0),
|
||||
_num_targets(0)
|
||||
{
|
||||
// clear target info
|
||||
// clear target info
|
||||
memset(_target_info, 0, sizeof(_target_info));
|
||||
|
||||
// will be adjusted when init is called
|
||||
_flags.healthy = false;
|
||||
// will be adjusted when init is called
|
||||
_flags.healthy = false;
|
||||
}
|
||||
|
||||
IRLock::~IRLock() {}
|
||||
|
|
|
@ -31,49 +31,49 @@
|
|||
class IRLock
|
||||
{
|
||||
public:
|
||||
IRLock();
|
||||
virtual ~IRLock();
|
||||
IRLock();
|
||||
virtual ~IRLock();
|
||||
|
||||
// init - initialize sensor library
|
||||
// library won't be useable unless this is first called
|
||||
virtual void init() = 0;
|
||||
// init - initialize sensor library
|
||||
// library won't be useable unless this is first called
|
||||
virtual void init() = 0;
|
||||
|
||||
// true if irlock sensor is online and healthy
|
||||
bool healthy() const { return _flags.healthy; }
|
||||
// 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; }
|
||||
// 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_targets() const { return _num_targets; }
|
||||
// returns the number of blocks in the current frame
|
||||
size_t num_targets() const { return _num_targets; }
|
||||
|
||||
// retrieve latest sensor data - returns true if new data is available
|
||||
virtual bool update() = 0;
|
||||
// retrieve latest sensor data - returns true if new data is available
|
||||
virtual bool update() = 0;
|
||||
|
||||
// get_angle_to_target - retrieve body frame x and y angles (in radians) to target
|
||||
// returns true if angles are available, false if not (i.e. no target)
|
||||
bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const;
|
||||
// get_angle_to_target - retrieve body frame x and y angles (in radians) to target
|
||||
// returns true if angles are available, false if not (i.e. no target)
|
||||
bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const;
|
||||
|
||||
protected:
|
||||
struct AP_IRLock_Flags {
|
||||
uint8_t healthy : 1; // true if sensor is healthy
|
||||
} _flags;
|
||||
struct AP_IRLock_Flags {
|
||||
uint8_t healthy : 1; // true if sensor is healthy
|
||||
} _flags;
|
||||
|
||||
// internals
|
||||
uint32_t _last_update;
|
||||
uint16_t _num_targets;
|
||||
// internals
|
||||
uint32_t _last_update;
|
||||
uint16_t _num_targets;
|
||||
|
||||
// irlock_target_info is a duplicate of the PX4Firmware irlock_s structure
|
||||
typedef struct {
|
||||
uint64_t timestamp; // time target was seen in microseconds since system start
|
||||
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
|
||||
} irlock_target_info;
|
||||
// irlock_target_info is a duplicate of the PX4Firmware irlock_s structure
|
||||
typedef struct {
|
||||
uint64_t timestamp; // time target was seen in microseconds since system start
|
||||
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
|
||||
} irlock_target_info;
|
||||
|
||||
irlock_target_info _target_info[IRLOCK_MAX_TARGETS];
|
||||
irlock_target_info _target_info[IRLOCK_MAX_TARGETS];
|
||||
};
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue