PX4 Firmware
PX4 Autopilot Software http://px4.io
Device.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file Device.hpp
36  *
37  * Definitions for the generic base classes in the device framework.
38  */
39 
40 #ifndef _DEVICE_DEVICE_HPP
41 #define _DEVICE_DEVICE_HPP
42 
43 /*
44  * Includes here should only cover the needs of the framework definitions.
45  */
46 #include <px4_platform_common/px4_config.h>
47 #include <px4_platform_common/posix.h>
48 
49 #include <drivers/drv_device.h>
50 
51 #define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__)
52 #define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__)
53 
54 /**
55  * Namespace encapsulating all device framework classes, functions and data.
56  */
57 namespace device
58 {
59 
60 /**
61  * Fundamental base class for all physical drivers (I2C, SPI).
62  *
63  * This class provides the basic driver template for I2C and SPI devices
64  */
66 {
67 public:
68 
69  // no copy, assignment, move, move assignment
70  Device(const Device &) = delete;
71  Device &operator=(const Device &) = delete;
72  Device(Device &&) = delete;
73  Device &operator=(Device &&) = delete;
74 
75  /**
76  * Destructor.
77  *
78  * Public so that anonymous devices can be destroyed.
79  */
80  virtual ~Device() = default;
81 
82  /*
83  * Direct access methods.
84  */
85 
86  /**
87  * Initialise the driver and make it ready for use.
88  *
89  * @return OK if the driver initialized OK, negative errno otherwise;
90  */
91  virtual int init() { return PX4_OK; }
92 
93  /**
94  * Read directly from the device.
95  *
96  * The actual size of each unit quantity is device-specific.
97  *
98  * @param offset The device address at which to start reading
99  * @param data The buffer into which the read values should be placed.
100  * @param count The number of items to read.
101  * @return The number of items read on success, negative errno otherwise.
102  */
103  virtual int read(unsigned address, void *data, unsigned count) { return -ENODEV; }
104 
105  /**
106  * Write directly to the device.
107  *
108  * The actual size of each unit quantity is device-specific.
109  *
110  * @param address The device address at which to start writing.
111  * @param data The buffer from which values should be read.
112  * @param count The number of items to write.
113  * @return The number of items written on success, negative errno otherwise.
114  */
115  virtual int write(unsigned address, void *data, unsigned count) { return -ENODEV; }
116 
117  /**
118  * Perform a device-specific operation.
119  *
120  * @param operation The operation to perform.
121  * @param arg An argument to the operation.
122  * @return Negative errno on error, OK or positive value on success.
123  */
124  virtual int ioctl(unsigned operation, unsigned &arg)
125  {
126  switch (operation) {
127  case DEVIOCGDEVICEID:
128  return (int)_device_id.devid;
129  }
130 
131  return -ENODEV;
132  }
133 
134  /** Device bus types for DEVID */
136  DeviceBusType_UNKNOWN = 0,
137  DeviceBusType_I2C = 1,
138  DeviceBusType_SPI = 2,
139  DeviceBusType_UAVCAN = 3,
140  DeviceBusType_SIMULATION = 4,
141  };
142 
143  /*
144  broken out device elements. The bitfields are used to keep
145  the overall value small enough to fit in a float accurately,
146  which makes it possible to transport over the MAVLink
147  parameter protocol without loss of information.
148  */
151  uint8_t bus: 5; // which instance of the bus type
152  uint8_t address; // address on the bus (eg. I2C address)
153  uint8_t devtype; // device class specific device type
154  };
155 
156  union DeviceId {
157  struct DeviceStructure devid_s;
158  uint32_t devid;
159  };
160 
161  uint32_t get_device_id() const { return _device_id.devid; }
162 
163  /**
164  * Return the bus type the device is connected to.
165  *
166  * @return The bus type
167  */
168  DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; }
169  void set_device_bus_type(DeviceBusType bus_type) { _device_id.devid_s.bus_type = bus_type; }
170 
171  static const char *get_device_bus_string(DeviceBusType bus)
172  {
173  switch (bus) {
174  case DeviceBusType_I2C:
175  return "I2C";
176 
177  case DeviceBusType_SPI:
178  return "SPI";
179 
180  case DeviceBusType_UAVCAN:
181  return "UAVCAN";
182 
183  case DeviceBusType_SIMULATION:
184  return "SIMULATION";
185 
186  case DeviceBusType_UNKNOWN:
187  default:
188  return "UNKNOWN";
189  }
190  }
191 
192  /**
193  * Return the bus ID the device is connected to.
194  *
195  * @return The bus ID
196  */
197  uint8_t get_device_bus() const { return _device_id.devid_s.bus; }
198  void set_device_bus(uint8_t bus) { _device_id.devid_s.bus = bus; }
199 
200  /**
201  * Return the bus address of the device.
202  *
203  * @return The bus address
204  */
205  uint8_t get_device_address() const { return _device_id.devid_s.address; }
206  void set_device_address(int address) { _device_id.devid_s.address = address; }
207 
208  /**
209  * Return the device type
210  *
211  * @return The device type
212  */
213  uint8_t get_device_type() const { return _device_id.devid_s.devtype; }
214  void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; }
215 
216  /**
217  * Print decoded device id string to a buffer.
218  *
219  * @param buffer buffer to write to
220  * @param length buffer length
221  * @param id The device id.
222  * @param return number of bytes written
223  */
224  static int device_id_print_buffer(char *buffer, int length, uint32_t id)
225  {
226  DeviceId dev_id{};
227  dev_id.devid = id;
228 
229  int num_written = snprintf(buffer, length, "Type: 0x%02X, %s:%d (0x%02X)", dev_id.devid_s.devtype,
230  get_device_bus_string(dev_id.devid_s.bus_type), dev_id.devid_s.bus, dev_id.devid_s.address);
231 
232  buffer[length - 1] = 0; // ensure 0-termination
233 
234  return num_written;
235  }
236 
237  virtual bool external() const { return false; }
238 
239 protected:
240  union DeviceId _device_id {}; /**< device identifier information */
241 
242  const char *_name{nullptr}; /**< driver name */
243  bool _debug_enabled{false}; /**< if true, debug messages are printed */
244 
245  explicit Device(const char *name) : _name(name)
246  {
247  set_device_bus_type(DeviceBusType_UNKNOWN);
248  }
249 
250  Device(const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
251  : _name(name)
252  {
253  set_device_bus_type(bus_type);
254  set_device_bus(bus);
255  set_device_address(address);
256  set_device_type(devtype);
257  }
258 
259  Device(DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
260  {
261  set_device_bus_type(bus_type);
262  set_device_bus(bus);
263  set_device_address(address);
264  set_device_type(devtype);
265  }
266 
267 };
268 
269 } // namespace device
270 
271 #endif /* _DEVICE_DEVICE_HPP */
virtual int init()
Initialise the driver and make it ready for use.
Definition: Device.hpp:91
void set_device_bus(uint8_t bus)
Definition: Device.hpp:198
Definition: I2C.hpp:51
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
uint8_t get_device_bus() const
Return the bus ID the device is connected to.
Definition: Device.hpp:197
DeviceBusType
Device bus types for DEVID.
Definition: Device.hpp:135
void set_device_address(int address)
Definition: Device.hpp:206
Device(const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype=0)
Definition: Device.hpp:250
uint32_t get_device_id() const
Definition: Device.hpp:161
void set_device_bus_type(DeviceBusType bus_type)
Definition: Device.hpp:169
Generic device / sensor interface.
device identifier information
Definition: Device.hpp:240
uint8_t * data
Definition: dataman.cpp:149
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
Definition: Device.hpp:115
virtual int ioctl(unsigned operation, unsigned &arg)
Perform a device-specific operation.
Definition: Device.hpp:124
Device(DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype=0)
Definition: Device.hpp:259
void set_device_type(uint8_t devtype)
Definition: Device.hpp:214
const char * name
Definition: tests_main.c:58
virtual bool external() const
Definition: Device.hpp:237
Device(const char *name)
Definition: Device.hpp:245
static int device_id_print_buffer(char *buffer, int length, uint32_t id)
Print decoded device id string to a buffer.
Definition: Device.hpp:224
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
uint8_t get_device_type() const
Return the device type.
Definition: Device.hpp:213
DeviceBusType get_device_bus_type() const
Return the bus type the device is connected to.
Definition: Device.hpp:168
uint8_t get_device_address() const
Return the bus address of the device.
Definition: Device.hpp:205
virtual int read(unsigned address, void *data, unsigned count)
Read directly from the device.
Definition: Device.hpp:103
static const char * get_device_bus_string(DeviceBusType bus)
Definition: Device.hpp:171