dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

irlock : add support for precision landing

This commit is contained in:
Nicolas de Palezieux 2018-01-14 23:24:23 +05:30 committed by Lorenz Meier
parent 155c55814b
commit 4fecb6fe20
1 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/drivers/irlock/irlock.cpp
View File

@ -58,6 +58,9 @@
#include <nuttx/wqueue.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/irlock_report.h>
/** Configuration Constants **/
#define IRLOCK_I2C_BUS PX4_I2C_BUS_EXPANSION
#define IRLOCK_I2C_ADDRESS 0x54 /** 7-bit address (non shifted) **/
@ -124,6 +127,9 @@ private:
bool _sensor_ok;
work_s _work;
uint32_t _read_failures;
int _orb_class_instance;
orb_advert_t _irlock_report_topic;
};
/** global pointer for single IRLOCK sensor **/
@ -141,7 +147,9 @@ IRLOCK::IRLOCK(int bus, int address) :
I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000),
_reports(nullptr),
_sensor_ok(false),
_read_failures(0)
_read_failures(0),
_orb_class_instance(-1),
_irlock_report_topic(nullptr)
{
memset(&_work, 0, sizeof(_work));
}
@ -363,6 +371,29 @@ int IRLOCK::read_device()
_reports->force(&report);
// publish over uORB
if (report.num_targets > 0) {
struct irlock_report_s orb_report;
orb_report.timestamp = report.timestamp;
orb_report.signature = report.targets[0].signature;
orb_report.pos_x = report.targets[0].pos_x;
orb_report.pos_y = report.targets[0].pos_y;
orb_report.size_x = report.targets[0].size_x;
orb_report.size_y = report.targets[0].size_y;
if (_irlock_report_topic != nullptr) {
orb_publish(ORB_ID(irlock_report), _irlock_report_topic, &orb_report);
} else {
_irlock_report_topic = orb_advertise_multi(ORB_ID(irlock_report), &orb_report, &_orb_class_instance, ORB_PRIO_LOW);
if (_irlock_report_topic == nullptr) {
DEVICE_LOG("failed to create irlock_report object. Did you start uOrb?");
}
}
}
return OK;
}