Line data Source code
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 :
16 : #include "Device.h"
17 :
18 : #include <stdio.h>
19 : #include <AP_Common/AP_Common.h>
20 :
21 : /*
22 : using checked registers allows a device check that a set of critical
23 : register values don't change at runtime. This is useful on key
24 : sensors (such as IMUs) which may experience brownouts or other
25 : issues in flight
26 :
27 : To use register checking call setup_checked_registers() once to
28 : allocate the space for the checked register values. The set the
29 : checked flag on any write_register() calls that you want protected.
30 :
31 : Periodically (say at 50Hz) you should then call
32 : check_next_register(). If that returns false then the sensor has had
33 : a corrupted register value. Marking the sensor as unhealthy is
34 : appropiate. The bad value will be corrected
35 : */
36 :
37 : /*
38 : setup nregs checked registers
39 : */
40 0 : bool AP_HAL::Device::setup_checked_registers(uint8_t nregs, uint8_t frequency)
41 : {
42 0 : if (_checked.regs != nullptr) {
43 0 : delete[] _checked.regs;
44 0 : _checked.n_allocated = 0;
45 0 : _checked.n_set = 0;
46 0 : _checked.next = 0;
47 : }
48 0 : _checked.regs = NEW_NOTHROW struct checkreg[nregs];
49 0 : if (_checked.regs == nullptr) {
50 0 : return false;
51 : }
52 0 : _checked.n_allocated = nregs;
53 0 : _checked.frequency = frequency;
54 0 : _checked.counter = 0;
55 0 : return true;
56 : }
57 :
58 14 : void AP_HAL::Device::set_device_type(uint8_t devtype) {
59 14 : _bus_id.devid_s.devtype = devtype;
60 14 : }
61 :
62 :
63 0 : bool AP_HAL::Device::read_bank_registers(uint8_t bank, uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
64 : {
65 0 : first_reg |= _read_flag;
66 0 : return transfer_bank(bank, &first_reg, 1, recv, recv_len);
67 : }
68 :
69 0 : bool AP_HAL::Device::write_bank_register(uint8_t bank, uint8_t reg, uint8_t val, bool checked)
70 : {
71 0 : uint8_t buf[2] = { reg, val };
72 0 : if (checked) {
73 0 : set_checked_register(bank, reg, val);
74 : }
75 0 : return transfer_bank(bank, buf, sizeof(buf), nullptr, 0);
76 : }
77 :
78 : /*
79 : set value of one checked register
80 : */
81 0 : void AP_HAL::Device::set_checked_register(uint8_t reg, uint8_t val)
82 : {
83 0 : set_checked_register(0, reg, val);
84 0 : }
85 :
86 0 : void AP_HAL::Device::set_checked_register(uint8_t bank, uint8_t reg, uint8_t val)
87 : {
88 0 : if (_checked.regs == nullptr) {
89 0 : return;
90 : }
91 0 : struct checkreg *regs = _checked.regs;
92 0 : for (uint8_t i=0; i<_checked.n_set; i++) {
93 0 : if (regs[i].regnum == reg && regs[i].bank == bank) {
94 0 : regs[i].value = val;
95 0 : return;
96 : }
97 : }
98 0 : if (_checked.n_set == _checked.n_allocated) {
99 0 : printf("Not enough checked registers for reg 0x%02x on device 0x%x\n",
100 0 : (unsigned)reg, (unsigned)get_bus_id());
101 0 : return;
102 : }
103 0 : regs[_checked.n_set].bank = bank;
104 0 : regs[_checked.n_set].regnum = reg;
105 0 : regs[_checked.n_set].value = val;
106 0 : _checked.n_set++;
107 : }
108 :
109 : /*
110 : check one register value
111 : */
112 0 : bool AP_HAL::Device::check_next_register(void)
113 : {
114 0 : if (_checked.n_set == 0) {
115 0 : return true;
116 : }
117 0 : if (++_checked.counter < _checked.frequency) {
118 0 : return true;
119 : }
120 0 : _checked.counter = 0;
121 :
122 0 : struct checkreg ® = _checked.regs[_checked.next];
123 : uint8_t v, v2;
124 :
125 0 : if (_bank_select) {
126 0 : if (!_bank_select(reg.bank)) {
127 : // Cannot set bank
128 : #if 0
129 : printf("Device 0x%x set bank 0x%02x\n",
130 : (unsigned)get_bus_id(),
131 : (unsigned)reg.bank);
132 : #endif
133 0 : _checked.last_reg_fail = reg;
134 0 : return false;
135 : }
136 : }
137 :
138 0 : if ((!read_registers(reg.regnum, &v, 1) || v != reg.value) &&
139 0 : (!read_registers(reg.regnum, &v2, 1) || v2 != reg.value)) {
140 : // a register has changed value unexpectedly. Try changing it back
141 : // and re-check it next time
142 : #if 0
143 : printf("Device 0x%x fixing 0x%02x 0x%02x -> 0x%02x\n",
144 : (unsigned)get_bus_id(),
145 : (unsigned)reg.regnum, (unsigned)v, (unsigned)reg.value);
146 : #endif
147 0 : write_register(reg.regnum, reg.value);
148 0 : _checked.last_reg_fail = reg;
149 0 : _checked.last_reg_fail.value = v;
150 0 : return false;
151 : }
152 0 : _checked.next = (_checked.next+1) % _checked.n_set;
153 0 : return true;
154 : }
155 :
156 : /*
157 : check one register value, returning information on the failure
158 : */
159 0 : bool AP_HAL::Device::check_next_register(struct checkreg &fail)
160 : {
161 0 : if (check_next_register()) {
162 0 : return true;
163 : }
164 0 : fail = _checked.last_reg_fail;
165 0 : return false;
166 : }
167 :
168 :
169 1990487 : bool AP_HAL::Device::write_register(uint8_t reg, uint8_t val, bool checked)
170 : {
171 1990487 : uint8_t buf[2] = { reg, val };
172 1990487 : if (checked) {
173 0 : set_checked_register(reg, val);
174 : }
175 1990487 : bool result = transfer(buf, sizeof(buf), nullptr, 0);
176 1990487 : if (_register_rw_callback && result) {
177 0 : _register_rw_callback(reg, &val, 1, true);
178 : }
179 1990487 : return result;
180 : }
181 :
182 :
183 83323 : bool AP_HAL::Device::read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
184 : {
185 83323 : uint8_t read_reg = first_reg;
186 83323 : first_reg |= _read_flag;
187 83323 : bool result = transfer(&first_reg, 1, recv, recv_len);
188 83323 : if (_register_rw_callback != nullptr && result) {
189 0 : _register_rw_callback(read_reg, recv, recv_len, false);
190 : }
191 83323 : return result;
192 : }
193 :
194 0 : bool AP_HAL::Device::transfer_bank(uint8_t bank, const uint8_t *send, uint32_t send_len,
195 : uint8_t *recv, uint32_t recv_len)
196 : {
197 0 : if (_bank_select) {
198 0 : if (!_bank_select(bank)) {
199 0 : return false;
200 : }
201 : }
202 0 : return transfer(send, send_len, recv, recv_len);
203 : }
204 :
205 :
206 : /**
207 : * Some devices connected on the I2C or SPI bus require a bit to be set on
208 : * the register address in order to perform a read operation. This sets a
209 : * flag to be used by #read_registers(). The flag's default value is zero.
210 : */
211 0 : void AP_HAL::Device::set_read_flag(uint8_t flag)
212 : {
213 0 : _read_flag = flag;
214 0 : }
215 :
216 :
217 : /**
218 : * make a bus id given bus type, bus number, bus address and
219 : * device type This is for use by devices that do not use one of
220 : * the standard HAL Device types, such as UAVCAN devices
221 : */
222 23751 : uint32_t AP_HAL::Device::make_bus_id(enum BusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype) {
223 23751 : union DeviceId d {};
224 23751 : d.devid_s.bus_type = bus_type;
225 23751 : d.devid_s.bus = bus;
226 23751 : d.devid_s.address = address;
227 23751 : d.devid_s.devtype = devtype;
228 23751 : return d.devid;
229 : }
230 :
231 : /**
232 : * return a new bus ID for the same bus connection but a new device type.
233 : * This is used for auxiliary bus connections
234 : */
235 4050 : uint32_t AP_HAL::Device::change_bus_id(uint32_t old_id, uint8_t devtype) {
236 : union DeviceId d;
237 4050 : d.devid = old_id;
238 4050 : d.devid_s.devtype = devtype;
239 4050 : return d.devid;
240 : }
241 :
242 : /**
243 : * return bus ID with a new devtype
244 : */
245 0 : uint32_t AP_HAL::Device::get_bus_id_devtype(uint8_t devtype) const {
246 0 : return change_bus_id(get_bus_id(), devtype);
247 : }
248 :
249 : /**
250 : * get bus type
251 : */
252 0 : enum AP_HAL::Device::BusType AP_HAL::Device::devid_get_bus_type(uint32_t dev_id) {
253 : union DeviceId d;
254 0 : d.devid = dev_id;
255 0 : return d.devid_s.bus_type;
256 : }
257 :
258 0 : uint8_t AP_HAL::Device::devid_get_bus(uint32_t dev_id) {
259 : union DeviceId d;
260 0 : d.devid = dev_id;
261 0 : return d.devid_s.bus;
262 : }
263 :
264 0 : uint8_t AP_HAL::Device::devid_get_address(uint32_t dev_id) {
265 : union DeviceId d;
266 0 : d.devid = dev_id;
267 0 : return d.devid_s.address;
268 : }
269 :
270 25 : uint8_t AP_HAL::Device::devid_get_devtype(uint32_t dev_id) {
271 : union DeviceId d;
272 25 : d.devid = dev_id;
273 25 : return d.devid_s.devtype;
274 : }
|
