AP_HAL: added device id tuples to Device class

this allows a bus connection and device type tuple to be identifed as
a 24 bit number for use in user visible device IDs. Every bus
connection has a unique device ID.
This commit is contained in:
Andrew Tridgell 2016-11-04 20:22:42 +11:00
parent 4ef92b5e39
commit 3d48b6bb3a
1 changed files with 79 additions and 6 deletions

View File

@ -27,8 +27,10 @@
class AP_HAL::Device {
public:
enum BusType {
BUS_TYPE_I2C,
BUS_TYPE_SPI,
BUS_TYPE_UNKNOWN = 0,
BUS_TYPE_I2C = 1,
BUS_TYPE_SPI = 2,
BUS_TYPE_UAVCAN = 3
};
enum Speed {
@ -39,11 +41,26 @@ public:
FUNCTOR_TYPEDEF(PeriodicCb, bool);
typedef void* PeriodicHandle;
const enum BusType bus_type;
Device(enum BusType type)
: bus_type(type)
{ }
{
_bus_id.devid_s.bus_type = type;
}
// return bus type
enum BusType bus_type(void) const {
return _bus_id.devid_s.bus_type;
}
// return 24 bit bus identifier
uint32_t get_bus_id(void) const {
return _bus_id.devid;
}
// set device type within a device class (eg. AP_COMPASS_TYPE_LSM303D)
void set_device_type(uint8_t devtype) {
_bus_id.devid_s.devtype = devtype;
}
virtual ~Device() { }
@ -150,6 +167,62 @@ public:
_read_flag = flag;
}
/**
* make a bus id given bus type, bus number, bus address and
* device type This is for use by devices that do not use one of
* the standard HAL Device types, such as UAVCAN devices
*/
static uint32_t make_bus_id(enum BusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype) {
union DeviceId d;
d.devid_s.bus_type = bus_type;
d.devid_s.bus = bus;
d.devid_s.address = address;
d.devid_s.devtype = devtype;
return d.devid;
}
/**
* return a new bus ID for the same bus connection but a new device type.
* This is used for auxillary bus connections
*/
static uint32_t change_bus_id(uint32_t old_id, uint8_t devtype) {
union DeviceId d;
d.devid = old_id;
d.devid_s.devtype = devtype;
return d.devid;
}
protected:
uint8_t _read_flag = 0;
/*
broken out device elements. The bitfields are used to keep
the overall value small enough to fit in a float accurately,
which makes it possible to transport over the MAVLink
parameter protocol without loss of information.
*/
struct DeviceStructure {
enum BusType bus_type : 3;
uint8_t bus: 5; // which instance of the bus type
uint8_t address; // address on the bus (eg. I2C address)
uint8_t devtype; // device class specific device type
};
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
union DeviceId _bus_id;
// set device address (eg. i2c bus address or spi CS)
void set_device_address(uint8_t address) {
_bus_id.devid_s.address = address;
}
// set device bus number
void set_device_bus(uint8_t bus) {
_bus_id.devid_s.bus = bus;
}
};