Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #include "AP_RangeFinder_PulsedLightLRF.h"
16 :
17 : #if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
18 :
19 : #include <stdio.h>
20 :
21 : #include <AP_HAL/AP_HAL.h>
22 : #include <AP_HAL/utility/sparse-endian.h>
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : /* LL40LS Registers addresses */
27 : #define LL40LS_MEASURE_REG 0x00 /* Measure range register */
28 : #define LL40LS_SIG_COUNT_VAL 0x02
29 : #define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */
30 : #define LL40LS_COUNT 0x11
31 : #define LL40LS_HW_VERSION 0x41
32 : #define LL40LS_INTERVAL 0x45
33 : #define LL40LS_SW_VERSION 0x4f
34 :
35 : // bit values
36 : #define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */
37 : #define LL40LS_AUTO_INCREMENT 0x80
38 : #define LL40LS_COUNT_CONTINUOUS 0xff
39 : #define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
40 :
41 : // i2c address
42 : #define LL40LS_ADDR 0x62
43 :
44 0 : AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus,
45 : RangeFinder::RangeFinder_State &_state,
46 : AP_RangeFinder_Params &_params,
47 0 : RangeFinder::Type _rftype)
48 : : AP_RangeFinder_Backend(_state, _params)
49 0 : , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
50 0 : , rftype(_rftype)
51 : {
52 0 : }
53 :
54 : /*
55 : detect if a PulsedLight rangefinder is connected. We'll detect by
56 : look for the version registers
57 : */
58 0 : AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus,
59 : RangeFinder::RangeFinder_State &_state,
60 : AP_RangeFinder_Params &_params,
61 : RangeFinder::Type rftype)
62 : {
63 : AP_RangeFinder_PulsedLightLRF *sensor
64 0 : = NEW_NOTHROW AP_RangeFinder_PulsedLightLRF(bus, _state, _params, rftype);
65 0 : if (!sensor ||
66 0 : !sensor->init()) {
67 0 : delete sensor;
68 0 : return nullptr;
69 : }
70 0 : return sensor;
71 : }
72 :
73 : /*
74 : called at 50Hz
75 : */
76 0 : void AP_RangeFinder_PulsedLightLRF::timer(void)
77 : {
78 0 : if (check_reg_counter++ == 10) {
79 0 : check_reg_counter = 0;
80 0 : if (!_dev->check_next_register()) {
81 : // re-send the acquire. this handles the case of power
82 : // cycling while running in continuous mode
83 0 : _dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE);
84 : }
85 : }
86 :
87 0 : switch (phase) {
88 0 : case PHASE_COLLECT: {
89 : be16_t val;
90 : // read the high and low byte distance registers
91 0 : if (_dev->read_registers(LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT, (uint8_t*)&val, sizeof(val))) {
92 0 : uint16_t _distance_cm = be16toh(val);
93 : // remove momentary spikes
94 0 : if (abs(_distance_cm - last_distance_cm) < 100) {
95 0 : state.distance_m = _distance_cm * 0.01f;
96 0 : state.last_reading_ms = AP_HAL::millis();
97 0 : update_status();
98 : }
99 0 : last_distance_cm = _distance_cm;
100 : } else {
101 0 : set_status(RangeFinder::Status::NoData);
102 : }
103 0 : if (!v2_hardware) {
104 : // for v2 hw we use continuous mode
105 0 : phase = PHASE_MEASURE;
106 : }
107 0 : if (!v3hp_hardware) {
108 : // for v3hp hw we start PHASE_MEASURE immediately after PHASE_COLLECT
109 0 : break;
110 : }
111 : }
112 : FALLTHROUGH;
113 : case PHASE_MEASURE:
114 0 : if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) {
115 0 : phase = PHASE_COLLECT;
116 : }
117 0 : break;
118 : }
119 0 : }
120 :
121 :
122 : /*
123 : a table of settings for a lidar
124 : */
125 : struct settings_table {
126 : uint8_t reg;
127 : uint8_t value;
128 : };
129 :
130 : /*
131 : register setup table for V1 Lidar
132 : */
133 : static const struct settings_table settings_v1[] = {
134 : { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET },
135 : };
136 :
137 : /*
138 : register setup table for V2 Lidar
139 : */
140 : static const struct settings_table settings_v2[] = {
141 : { LL40LS_INTERVAL, 0x28 }, // 0x28 == 50Hz
142 : { LL40LS_COUNT, LL40LS_COUNT_CONTINUOUS },
143 : { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE },
144 : };
145 :
146 : /*
147 : register setup table for V3HP Lidar
148 : */
149 : static const struct settings_table settings_v3hp[] = {
150 : { LL40LS_SIG_COUNT_VAL, 0x80 }, // 0x80 = 128 acquisitions
151 : };
152 :
153 : /*
154 : initialise the sensor to required settings
155 : */
156 0 : bool AP_RangeFinder_PulsedLightLRF::init(void)
157 : {
158 0 : if (!_dev) {
159 0 : return false;
160 : }
161 0 : _dev->get_semaphore()->take_blocking();
162 0 : _dev->set_retries(3);
163 :
164 : // LidarLite needs split transfers
165 0 : _dev->set_split_transfers(true);
166 :
167 0 : if (rftype == RangeFinder::Type::PLI2CV3) {
168 0 : v2_hardware = true;
169 0 : } else if (rftype == RangeFinder::Type::PLI2CV3HP) {
170 0 : v3hp_hardware = true;
171 : } else {
172 : // auto-detect v1 vs v2
173 0 : if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) &&
174 0 : hw_version > 0 &&
175 0 : _dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) &&
176 0 : sw_version > 0)) {
177 0 : printf("PulsedLightI2C: bad version 0x%02x 0x%02x\n", (unsigned)hw_version, (unsigned)sw_version);
178 : // invalid version information
179 0 : goto failed;
180 : }
181 0 : v2_hardware = (hw_version >= 0x15);
182 : }
183 :
184 : const struct settings_table *table;
185 : uint8_t num_settings;
186 :
187 0 : if (v2_hardware) {
188 0 : table = settings_v2;
189 0 : num_settings = sizeof(settings_v2) / sizeof(settings_table);
190 0 : phase = PHASE_COLLECT;
191 0 : } else if (v3hp_hardware) {
192 0 : table = settings_v3hp;
193 0 : num_settings = sizeof(settings_v3hp) / sizeof(settings_table);
194 0 : phase = PHASE_MEASURE;
195 : } else {
196 0 : table = settings_v1;
197 0 : num_settings = sizeof(settings_v1) / sizeof(settings_table);
198 0 : phase = PHASE_MEASURE;
199 : }
200 :
201 0 : _dev->setup_checked_registers(num_settings);
202 :
203 0 : for (uint8_t i = 0; i < num_settings; i++) {
204 0 : if (!_dev->write_register(table[i].reg, table[i].value, true)) {
205 0 : goto failed;
206 : }
207 : }
208 :
209 0 : printf("Found LidarLite device=0x%x v2=%d v3hp=%d\n", unsigned(_dev->get_bus_id()), (int)v2_hardware, (int)v3hp_hardware);
210 :
211 0 : _dev->get_semaphore()->give();
212 :
213 0 : _dev->register_periodic_callback(20000,
214 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, void));
215 0 : return true;
216 :
217 0 : failed:
218 0 : _dev->get_semaphore()->give();
219 0 : return false;
220 : }
221 :
222 : #endif // AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
|
