mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
AP_IRLock: allow specification of i2c bus
This commit is contained in:
parent
c73584c9b8
commit
2eb363a950
@ -32,9 +32,13 @@ extern const AP_HAL::HAL& hal;
|
|||||||
|
|
||||||
#define IRLOCK_SYNC 0xAA55AA55
|
#define IRLOCK_SYNC 0xAA55AA55
|
||||||
|
|
||||||
void AP_IRLock_I2C::init()
|
void AP_IRLock_I2C::init(int8_t bus)
|
||||||
{
|
{
|
||||||
dev = std::move(hal.i2c_mgr->get_device(1, IRLOCK_I2C_ADDRESS));
|
if (bus < 0) {
|
||||||
|
// default to i2c external bus
|
||||||
|
bus = 1;
|
||||||
|
}
|
||||||
|
dev = std::move(hal.i2c_mgr->get_device(bus, IRLOCK_I2C_ADDRESS));
|
||||||
if (!dev) {
|
if (!dev) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -10,7 +10,7 @@ class AP_IRLock_I2C : public IRLock
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// init - initialize sensor library
|
// init - initialize sensor library
|
||||||
void init();
|
void init(int8_t bus) override;
|
||||||
|
|
||||||
// retrieve latest sensor data - returns true if new data is available
|
// retrieve latest sensor data - returns true if new data is available
|
||||||
bool update();
|
bool update();
|
||||||
|
@ -36,7 +36,7 @@ AP_IRLock_SITL::AP_IRLock_SITL() :
|
|||||||
sock(true)
|
sock(true)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void AP_IRLock_SITL::init()
|
void AP_IRLock_SITL::init(int8_t bus)
|
||||||
{
|
{
|
||||||
SITL::SITL *sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
|
SITL::SITL *sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
|
||||||
// try to bind to a specific port so that if we restart ArduPilot
|
// try to bind to a specific port so that if we restart ArduPilot
|
||||||
|
@ -16,7 +16,7 @@ public:
|
|||||||
AP_IRLock_SITL();
|
AP_IRLock_SITL();
|
||||||
|
|
||||||
// init - initialize sensor library
|
// init - initialize sensor library
|
||||||
virtual void init();
|
virtual void init(int8_t bus);
|
||||||
|
|
||||||
// retrieve latest sensor data - returns true if new data is available
|
// retrieve latest sensor data - returns true if new data is available
|
||||||
virtual bool update();
|
virtual bool update();
|
||||||
|
@ -28,7 +28,7 @@ class IRLock
|
|||||||
public:
|
public:
|
||||||
// init - initialize sensor library
|
// init - initialize sensor library
|
||||||
// library won't be useable unless this is first called
|
// library won't be useable unless this is first called
|
||||||
virtual void init() = 0;
|
virtual void init(int8_t bus) = 0;
|
||||||
|
|
||||||
// true if irlock sensor is online and healthy
|
// true if irlock sensor is online and healthy
|
||||||
bool healthy() const { return _flags.healthy; }
|
bool healthy() const { return _flags.healthy; }
|
||||||
|
Loading…
Reference in New Issue
Block a user