-
Notifications
You must be signed in to change notification settings - Fork 17.2k
/
SPIUARTDriver.cpp
157 lines (127 loc) · 3.81 KB
/
SPIUARTDriver.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include "SPIUARTDriver.h"
#include <assert.h>
#include <stdlib.h>
#include <cstdio>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL &hal;
#ifdef SPIUART_DEBUG
#include <stdio.h>
#define debug(fmt, args ...) do {hal.console->printf("[SPIUARTDriver]: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define debug(fmt, args ...)
#define error(fmt, args ...)
#endif
using namespace Linux;
SPIUARTDriver::SPIUARTDriver()
: UARTDriver(false)
{
}
void SPIUARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (device_path != nullptr) {
UARTDriver::begin(b, rxS, txS);
if (is_initialized()) {
_external = true;
return;
}
}
if (!is_initialized()) {
_dev = hal.spi->get_device("ublox");
if (!_dev) {
return;
}
}
if (rxS < 1024) {
rxS = 2048;
}
if (txS < 1024) {
txS = 2048;
}
_readbuf.set_size(rxS);
_writebuf.set_size(txS);
if (_buffer == nullptr) {
/* Do not allocate new buffer, if we're just changing speed */
_buffer = NEW_NOTHROW uint8_t[rxS];
if (_buffer == nullptr) {
hal.console->printf("Not enough memory\n");
AP_HAL::panic("Not enough memory\n");
}
}
switch (b) {
case 4000000U:
if (is_initialized()) {
/* Do not allow speed changes before device is initialized, because
* it can lead to misconfiguraration. Once the device is initialized,
* it's sage to update speed
*/
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
high_speed_set = true;
debug("Set higher SPI-frequency");
} else {
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
high_speed_set = false;
debug("Set lower SPI-frequency");
}
break;
default:
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
high_speed_set = false;
debug("Set lower SPI-frequency");
debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
break;
}
_initialised = true;
}
int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
{
if (_external) {
return UARTDriver::_write_fd(buf,size);
}
if (!_dev->get_semaphore()->take_nonblocking()) {
return 0;
}
_dev->transfer_fullduplex(buf, _buffer, size);
_dev->get_semaphore()->give();
uint16_t ret = size;
/* Since all SPI-transactions are transfers we need update
* the _readbuf. I do believe there is a way to encapsulate
* this operation since it's the same as in the
* UARTDriver::write().
*/
_readbuf.write(_buffer, size);
return ret;
}
int SPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
{
static uint8_t ff_stub[100] = {0xff};
if (_external) {
return UARTDriver::_read_fd(buf, n);
}
/* Make SPI transactions shorter. It can save SPI bus from keeping too
* long. It's essential for NavIO as MPU9250 is on the same bus and
* doesn't like to be waiting. Making transactions more frequent but shorter
* is a win.
*/
n = MIN(n, 100);
if (!_dev->get_semaphore()->take_nonblocking()) {
return 0;
}
_dev->transfer_fullduplex(ff_stub, buf, n);
_dev->get_semaphore()->give();
return n;
}
void SPIUARTDriver::_timer_tick(void)
{
if (_external) {
UARTDriver::_timer_tick();
return;
}
/* lower the update rate */
if (AP_HAL::micros() - _last_update_timestamp < 10000) {
return;
}
UARTDriver::_timer_tick();
_last_update_timestamp = AP_HAL::micros();
}