APM:Libraries
Device.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
3  *
4  * This file is free software: you can redistribute it and/or modify it
5  * under the terms of the GNU General Public License as published by the
6  * Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This file is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12  * See the GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License along
15  * with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 #pragma once
18 
19 #include <inttypes.h>
20 
21 #include "AP_HAL_Namespace.h"
22 #include "utility/functor.h"
23 
24 /*
25  * This is an interface abstracting I2C and SPI devices
26  */
28 public:
29  enum BusType {
34  };
35 
36  enum Speed {
39  };
40 
41  FUNCTOR_TYPEDEF(PeriodicCb, void);
42  typedef void* PeriodicHandle;
43 
44  Device(enum BusType type)
45  {
46  _bus_id.devid_s.bus_type = type;
47  }
48 
49  // return bus type
50  enum BusType bus_type(void) const {
51  return _bus_id.devid_s.bus_type;
52  }
53 
54  // return bus number
55  uint8_t bus_num(void) const {
56  return _bus_id.devid_s.bus;
57  }
58 
59  // return 24 bit bus identifier
60  uint32_t get_bus_id(void) const {
61  return _bus_id.devid;
62  }
63 
64  // return address on bus
65  uint8_t get_bus_address(void) const {
66  return _bus_id.devid_s.address;
67  }
68 
69  // set device type within a device class (eg. AP_COMPASS_TYPE_LSM303D)
70  void set_device_type(uint8_t devtype) {
71  _bus_id.devid_s.devtype = devtype;
72  }
73 
74 
75  virtual ~Device() {
76  if (_checked.regs != nullptr) {
77  delete[] _checked.regs;
78  }
79  }
80 
81  /*
82  * Change device address. Note that this is the 7 bit address, it
83  * does not include the bit for read/write. Only works on I2C
84  */
85  virtual void set_address(uint8_t address) {};
86 
87  /*
88  * Set the speed of future transfers. Depending on the bus the speed may
89  * be shared for all devices on the same bus.
90  *
91  * Return: true if speed was successfully set or platform doesn't implement
92  * it; false otherwise.
93  */
94  virtual bool set_speed(Speed speed) = 0;
95 
96  /*
97  * Core transfer function. This does a single bus transaction which
98  * sends send_len bytes and receives recv_len bytes back from the slave.
99  *
100  * Return: true on a successful transfer, false on failure.
101  */
102  virtual bool transfer(const uint8_t *send, uint32_t send_len,
103  uint8_t *recv, uint32_t recv_len) = 0;
104 
113  bool read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
114  {
115  first_reg |= _read_flag;
116  return transfer(&first_reg, 1, recv, recv_len);
117  }
118 
125  bool write_register(uint8_t reg, uint8_t val, bool checked=false)
126  {
127  uint8_t buf[2] = { reg, val };
128  if (checked) {
129  set_checked_register(reg, val);
130  }
131  return transfer(buf, sizeof(buf), nullptr, 0);
132  }
133 
137  void set_checked_register(uint8_t reg, uint8_t val);
138 
143  bool setup_checked_registers(uint8_t num_regs, uint8_t frequency=10);
144 
149  bool check_next_register(void);
150 
156  bool read(uint8_t *recv, uint32_t recv_len)
157  {
158  return transfer(nullptr, 0, recv, recv_len);
159  }
160 
161  /*
162  * Get the semaphore for the bus this device is in. This is intended for
163  * drivers to use during initialization phase only.
164  */
165  virtual AP_HAL::Semaphore *get_semaphore() = 0;
166 
167  /*
168  * Register a periodic callback for this bus. All callbacks on the
169  * same bus are made from the same thread with lock already taken. In
170  * other words, the callback is not executed on the main thread (or the
171  * thread which registered the callback), but in a separate per-bus
172  * thread.
173  *
174  * After registering the periodic callback, the other functions should not
175  * be used anymore from other contexts. If it really needs to be done, the
176  * lock must be taken.
177  *
178  * Return: A handle for this periodic callback. To cancel the callback
179  * call #unregister_callback() or return false on the callback.
180  */
181  virtual PeriodicHandle register_periodic_callback(uint32_t period_usec, PeriodicCb) = 0;
182 
183  /*
184  * Adjust the time for the periodic callback registered with
185  * #register_periodic_callback. Note that the time will be re-calculated
186  * from the moment this call is made and expire after @period_usec.
187  *
188  * Return: true if periodic callback was successfully adjusted, false otherwise.
189  */
190  virtual bool adjust_periodic_callback(PeriodicHandle h, uint32_t period_usec) = 0;
191 
192  /*
193  * Cancel a periodic callback on this bus.
194  *
195  * Return: true if callback was successfully unregistered, false
196  * otherwise.
197  */
198  virtual bool unregister_callback(PeriodicHandle h) { return false; }
199 
200 
201  /*
202  allows to set callback that will be called after DMA transfer complete.
203  if this callback is set then any read/write operation will return directly after transfer setup and
204  bus semaphore must not be released until register_completion_callback(0) called from callback itself
205  */
206  virtual void register_completion_callback(AP_HAL::MemberProc proc) {}
208 
209  /*
210  * support for direct control of SPI chip select. Needed for
211  * devices with unusual SPI transfer patterns that include
212  * specific delays
213  */
214  virtual bool set_chip_select(bool set) { return false; }
215 
221  void set_read_flag(uint8_t flag)
222  {
223  _read_flag = flag;
224  }
225 
226 
232  static uint32_t make_bus_id(enum BusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype) {
233  union DeviceId d;
235  d.devid_s.bus = bus;
236  d.devid_s.address = address;
237  d.devid_s.devtype = devtype;
238  return d.devid;
239  }
240 
245  static uint32_t change_bus_id(uint32_t old_id, uint8_t devtype) {
246  union DeviceId d;
247  d.devid = old_id;
248  d.devid_s.devtype = devtype;
249  return d.devid;
250  }
251 
255  uint32_t get_bus_id_devtype(uint8_t devtype) {
256  return change_bus_id(get_bus_id(), devtype);
257  }
258 
259  /* set number of retries on transfers */
260  virtual void set_retries(uint8_t retries) {};
261 
262 protected:
263  uint8_t _read_flag = 0;
264 
265  /*
266  broken out device elements. The bitfields are used to keep
267  the overall value small enough to fit in a float accurately,
268  which makes it possible to transport over the MAVLink
269  parameter protocol without loss of information.
270  */
272  enum BusType bus_type : 3;
273  uint8_t bus: 5; // which instance of the bus type
274  uint8_t address; // address on the bus (eg. I2C address)
275  uint8_t devtype; // device class specific device type
276  };
277 
278  union DeviceId {
279  struct DeviceStructure devid_s;
280  uint32_t devid;
281  };
282 
284 
285  // set device address (eg. i2c bus address or spi CS)
286  void set_device_address(uint8_t address) {
288  }
289 
290  // set device bus number
291  void set_device_bus(uint8_t bus) {
293  }
294 
295 private:
296  // checked registers
297  struct checkreg {
298  uint8_t regnum;
299  uint8_t value;
300  };
301  struct {
302  uint8_t n_allocated;
303  uint8_t n_set;
304  uint8_t next;
305  uint8_t frequency;
306  uint8_t counter;
307  struct checkreg *regs;
308  } _checked;
309 };
static uint32_t make_bus_id(enum BusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype)
Definition: Device.h:232
uint8_t frequency
Definition: Device.h:305
virtual bool unregister_callback(PeriodicHandle h)
Definition: Device.h:198
FUNCTOR_TYPEDEF(PeriodicCb, void)
static uint32_t change_bus_id(uint32_t old_id, uint8_t devtype)
Definition: Device.h:245
virtual PeriodicHandle register_periodic_callback(uint32_t period_usec, PeriodicCb)=0
uint8_t _read_flag
Definition: Device.h:263
struct checkreg * regs
Definition: Device.h:307
uint8_t n_set
Definition: Device.h:303
uint32_t get_bus_id(void) const
Definition: Device.h:60
void set_device_bus(uint8_t bus)
Definition: Device.h:291
virtual AP_HAL::Semaphore * get_semaphore()=0
virtual void set_retries(uint8_t retries)
Definition: Device.h:260
uint8_t get_bus_address(void) const
Definition: Device.h:65
virtual void register_completion_callback(AP_HAL::Proc proc)
Definition: Device.h:207
void(* Proc)(void)
uint8_t counter
Definition: Device.h:306
uint32_t get_bus_id_devtype(uint8_t devtype)
Definition: Device.h:255
struct DeviceStructure devid_s
Definition: Device.h:279
uint8_t bus_num(void) const
Definition: Device.h:55
bool setup_checked_registers(uint8_t num_regs, uint8_t frequency=10)
Definition: Device.cpp:39
Device(enum BusType type)
Definition: Device.h:44
uint8_t next
Definition: Device.h:304
struct AP_HAL::Device::@38 _checked
void set_device_type(uint8_t devtype)
Definition: Device.h:70
virtual bool set_speed(Speed speed)=0
virtual void register_completion_callback(AP_HAL::MemberProc proc)
Definition: Device.h:206
void * PeriodicHandle
Definition: Device.h:42
enum BusType bus_type(void) const
Definition: Device.h:50
virtual bool adjust_periodic_callback(PeriodicHandle h, uint32_t period_usec)=0
virtual bool set_chip_select(bool set)
Definition: Device.h:214
virtual bool transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)=0
void set_checked_register(uint8_t reg, uint8_t val)
Definition: Device.cpp:60
virtual void set_address(uint8_t address)
Definition: Device.h:85
union DeviceId _bus_id
Definition: Device.h:283
void set_read_flag(uint8_t flag)
Definition: Device.h:221
bool check_next_register(void)
Definition: Device.cpp:85
bool read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
Definition: Device.h:113
void uint32_t uint32_t uint32_t flag
Definition: systick.h:80
bool read(uint8_t *recv, uint32_t recv_len)
Definition: Device.h:156
void set_device_address(uint8_t address)
Definition: Device.h:286
bool write_register(uint8_t reg, uint8_t val, bool checked=false)
Definition: Device.h:125
uint8_t n_allocated
Definition: Device.h:302
virtual ~Device()
Definition: Device.h:75