APM:Libraries
Device.cpp
Go to the documentation of this file.
1 /*
2  * This file is free software: you can redistribute it and/or modify it
3  * under the terms of the GNU General Public License as published by the
4  * Free Software Foundation, either version 3 of the License, or
5  * (at your option) any later version.
6  *
7  * This file is distributed in the hope that it will be useful, but
8  * WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10  * See the GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License along
13  * with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
15 #include "Device.h"
16 
17 #include <AP_HAL/AP_HAL.h>
18 #include <AP_HAL/utility/OwnPtr.h>
19 #include <stdio.h>
20 #include "Scheduler.h"
21 #include "Semaphores.h"
22 #include "Util.h"
23 
24 #if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE
25 
26 using namespace ChibiOS;
27 
28 static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
29 
30 /*
31  per-bus callback thread
32 */
33 void DeviceBus::bus_thread(void *arg)
34 {
35  struct DeviceBus *binfo = (struct DeviceBus *)arg;
36 
37  while (true) {
38  uint64_t now = AP_HAL::micros64();
39  DeviceBus::callback_info *callback;
40 
41  // find a callback to run
42  for (callback = binfo->callbacks; callback; callback = callback->next) {
43  if (now >= callback->next_usec) {
44  while (now >= callback->next_usec) {
45  callback->next_usec += callback->period_usec;
46  }
47  // call it with semaphore held
49  callback->cb();
50  binfo->semaphore.give();
51  }
52  }
53  }
54 
55  // work out when next loop is needed
56  uint64_t next_needed = 0;
57  now = AP_HAL::micros64();
58 
59  for (callback = binfo->callbacks; callback; callback = callback->next) {
60  if (next_needed == 0 ||
61  callback->next_usec < next_needed) {
62  next_needed = callback->next_usec;
63  if (next_needed < now) {
64  next_needed = now;
65  }
66  }
67  }
68 
69  // delay for at most 50ms, to handle newly added callbacks
70  uint32_t delay = 50000;
71  if (next_needed >= now && next_needed - now < delay) {
72  delay = next_needed - now;
73  }
74  // don't delay for less than 400usec, so one thread doesn't
75  // completely dominate the CPU
76  if (delay < 100) {
77  delay = 100;
78  }
79  hal.scheduler->delay_microseconds(delay);
80  }
81  return;
82 }
83 
84 AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
85 {
86  if (!thread_started) {
87  thread_started = true;
88 
89  hal_device = _hal_device;
90  // setup a name for the thread
91  const uint8_t name_len = 7;
92  char *name = (char *)malloc(name_len);
93  switch (hal_device->bus_type()) {
95  snprintf(name, name_len, "I2C:%u",
96  hal_device->bus_num());
97  break;
98 
100  snprintf(name, name_len, "SPI:%u",
101  hal_device->bus_num());
102  break;
103  default:
104  break;
105  }
106 
107  thread_ctx = chThdCreateFromHeap(NULL,
108  THD_WORKING_AREA_SIZE(1024),
109  name,
110  thread_priority, /* Initial priority. */
111  DeviceBus::bus_thread, /* Thread function. */
112  this); /* Thread parameter. */
113  }
115  if (callback == nullptr) {
116  return nullptr;
117  }
118  callback->cb = cb;
119  callback->period_usec = period_usec;
120  callback->next_usec = AP_HAL::micros64() + period_usec;
121 
122  // add to linked list of callbacks on thread
123  callback->next = callbacks;
124  callbacks = callback;
125 
126  return callback;
127 }
128 
129 /*
130  * Adjust the timer for the next call: it needs to be called from the bus
131  * thread, otherwise it will race with it
132  */
134 {
135  if (chThdGetSelfX() != thread_ctx) {
136  return false;
137  }
138 
139  DeviceBus::callback_info *callback = static_cast<DeviceBus::callback_info *>(h);
140 
141  callback->period_usec = period_usec;
142  callback->next_usec = AP_HAL::micros64() + period_usec;
143 
144  return true;
145 }
146 
147 /*
148  setup to use DMA-safe bouncebuffers for device transfers
149  */
150 void DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len,
151  uint8_t *&buf_rx, uint16_t rx_len)
152 {
153  bouncebuffer_setup_tx(buf_tx, tx_len);
154  bouncebuffer_setup_rx(buf_rx, rx_len);
155 }
156 
157 void DeviceBus::bouncebuffer_setup_tx(const uint8_t *&buf_tx, uint16_t tx_len)
158 {
159  if (buf_tx && !IS_DMA_SAFE(buf_tx)) {
160  if (tx_len > bounce_buffer_tx_size) {
161  if (bounce_buffer_tx_size) {
164  }
165  bounce_buffer_tx = (uint8_t *)hal.util->malloc_type(tx_len, AP_HAL::Util::MEM_DMA_SAFE);
166  if (bounce_buffer_tx == nullptr) {
167  AP_HAL::panic("Out of memory for DMA TX");
168  }
169  bounce_buffer_tx_size = tx_len;
170  }
171  memcpy(bounce_buffer_tx, buf_tx, tx_len);
172  buf_tx = bounce_buffer_tx;
173  }
174 }
175 
176 void DeviceBus::bouncebuffer_setup_rx(uint8_t *&buf_rx, uint16_t rx_len)
177 {
178  if (buf_rx && !IS_DMA_SAFE(buf_rx)) {
179  if (rx_len > bounce_buffer_rx_size) {
180  if (bounce_buffer_rx_size) {
183  }
184  bounce_buffer_rx = (uint8_t *)hal.util->malloc_type(rx_len, AP_HAL::Util::MEM_DMA_SAFE);
185  if (bounce_buffer_rx == nullptr) {
186  AP_HAL::panic("Out of memory for DMA RX");
187  }
188  bounce_buffer_rx_size = rx_len;
189  }
190  buf_rx = bounce_buffer_rx;
191  }
192 }
193 
194 /*
195  complete a transfer using DMA bounce buffer
196  */
197 void DeviceBus::bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len)
198 {
199  memcpy(buf_rx, bounce_buffer_rx, rx_len);
200 }
201 
202 #endif // HAL_USE_I2C || HAL_USE_SPI
struct ChibiOS::DeviceBus::callback_info * callbacks
uint16_t bounce_buffer_tx_size
Definition: Device.h:62
uint8_t thread_priority
Definition: Device.h:54
thread_t * thread_ctx
Definition: Device.h:55
void bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len, uint8_t *&buf_rx, uint16_t rx_len)
Definition: Device.cpp:150
AP_HAL::Util * util
Definition: HAL.h:115
#define HAL_SEMAPHORE_BLOCK_FOREVER
Definition: Semaphores.h:5
const char * name
Definition: BusTest.cpp:11
void delay(uint32_t ms)
Definition: system.cpp:91
virtual void free_type(void *ptr, size_t size, Memory_Type mem_type)
Definition: Util.h:116
uint8_t bus_num(void) const
Definition: Device.h:55
Semaphore semaphore
Definition: Device.h:31
void * PeriodicHandle
Definition: Device.h:42
enum BusType bus_type(void) const
Definition: Device.h:50
uint8_t * bounce_buffer_rx
Definition: Device.h:61
static void bus_thread(void *arg)
Definition: Device.cpp:33
uint64_t micros64()
Definition: system.cpp:162
bool take(uint32_t timeout_ms)
Definition: Semaphores.cpp:32
uint8_t * bounce_buffer_tx
Definition: Device.h:60
void * malloc(size_t size)
Definition: malloc.c:61
#define NULL
Definition: hal_types.h:59
const HAL & get_HAL()
virtual void * malloc_type(size_t size, Memory_Type mem_type)
Definition: Util.h:115
void bouncebuffer_setup_tx(const uint8_t *&buf_tx, uint16_t tx_len)
Definition: Device.cpp:157
virtual void delay_microseconds(uint16_t us)=0
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device)
Definition: Device.cpp:84
uint16_t bounce_buffer_rx_size
Definition: Device.h:63
int snprintf(char *str, size_t size, const char *fmt,...)
Definition: stdio.c:64
bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
Definition: Device.cpp:133
bool thread_started
Definition: Device.h:56
AP_HAL::Device::PeriodicCb cb
Definition: Device.h:50
void bouncebuffer_setup_rx(uint8_t *&buf_rx, uint16_t rx_len)
Definition: Device.cpp:176
#define IS_DMA_SAFE(addr)
Definition: Util.h:25
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
struct callback_info * next
Definition: Device.h:49
static const AP_HAL::HAL & hal
Definition: Device.cpp:28
AP_HAL::Device * hal_device
Definition: Device.h:57
void bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len)
Definition: Device.cpp:197
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114