diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index e412bb57cc..8d4c5cfb87 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -58,6 +58,9 @@ #include #include +#include +#include + /** 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; }