Line data Source code
1 : /*
2 : * Copyright (C) 2019 Lucas De Marchi <[email protected]>
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 : #include "AP_RangeFinder_Benewake_TFMiniPlus.h"
18 :
19 : #if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED
20 :
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <AP_HAL/AP_HAL.h>
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : #define DRIVER "TFMiniPlus"
27 : #define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
28 :
29 : /*
30 : * Command format:
31 : *
32 : * uint8_t header;
33 : * uint8_t len;
34 : * uint8_t id;
35 : * uint8_t data[];
36 : * uint8_t checksum;
37 : */
38 :
39 : #define CMD_SET_DATA_OUT 0x5A, 0x05, 0x07
40 : #define CMD_ENABLE_DATA_OUT CMD_SET_DATA_OUT, 0x01, 0x67
41 : #define CMD_DISABLE_DATA_OUT CMD_SET_DATA_OUT, 0x00, 0x66
42 :
43 6 : bool AP_RangeFinder_Benewake_TFMiniPlus::init()
44 : {
45 6 : const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
46 6 : const uint8_t CMD_SYSTEM_RESET[] = { 0x5A, 0x04, 0x04, 0x62 };
47 6 : const uint8_t CMD_OUTPUT_FORMAT_CM[] = { 0x5A, 0x05, 0x05, 0x01, 0x65 };
48 6 : const uint8_t CMD_ENABLE_DATA_OUTPUT[] = { CMD_ENABLE_DATA_OUT };
49 6 : const uint8_t CMD_FRAME_RATE_250HZ[] = { 0x5A, 0x06, 0x03, 0xFA, 0x00, 0x5D };
50 6 : const uint8_t CMD_SAVE_SETTINGS[] = { 0x5A, 0x04, 0x11, 0x6F };
51 : const uint8_t *cmds[] = {
52 : CMD_OUTPUT_FORMAT_CM,
53 : CMD_FRAME_RATE_250HZ,
54 : CMD_ENABLE_DATA_OUTPUT,
55 : CMD_SAVE_SETTINGS,
56 6 : };
57 : uint8_t val[12], i;
58 : bool ret;
59 :
60 6 : WITH_SEMAPHORE(dev.get_semaphore());
61 :
62 6 : dev.set_retries(0);
63 :
64 : /*
65 : * Check we get a response for firmware version to detect if sensor is there
66 : */
67 6 : ret = dev.transfer(CMD_FW_VERSION, sizeof(CMD_FW_VERSION), nullptr, 0);
68 6 : if (!ret) {
69 4 : return false;
70 : }
71 :
72 2 : hal.scheduler->delay(100);
73 :
74 2 : ret = dev.transfer(nullptr, 0, val, 7);
75 4 : if (!ret || val[0] != 0x5A || val[1] != 0x07 || val[2] != 0x01 ||
76 2 : !check_checksum(val, 7)) {
77 0 : return false;
78 : }
79 :
80 2 : if (val[5] * 10000 + val[4] * 100 + val[3] < 20003) {
81 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TFMini: FW ver %u.%u.%u (need>=2.0.3)",
82 : (unsigned)val[5],(unsigned)val[4],(unsigned)val[3]);
83 0 : return false;
84 : }
85 :
86 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TFMiniPlus: found fw version %u.%u.%u",
87 : val[5], val[4], val[3]);
88 :
89 10 : for (i = 0; i < ARRAY_SIZE(cmds); i++) {
90 8 : ret = dev.transfer(cmds[i], cmds[i][1], nullptr, 0);
91 8 : if (!ret) {
92 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TFMiniPlus: Unable to set configuration register %u",
93 : cmds[i][2]);
94 0 : return false;
95 : }
96 8 : hal.scheduler->delay(100);
97 : }
98 :
99 2 : dev.transfer(CMD_SYSTEM_RESET, sizeof(CMD_SYSTEM_RESET), nullptr, 0);
100 :
101 2 : hal.scheduler->delay(100);
102 :
103 2 : dev.register_periodic_callback(20000,
104 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Benewake_TFMiniPlus::timer, void));
105 :
106 2 : return true;
107 6 : }
108 :
109 3172 : void AP_RangeFinder_Benewake_TFMiniPlus::update()
110 : {
111 3172 : WITH_SEMAPHORE(_sem);
112 :
113 : // Power down if commanded
114 3172 : power_state.commanded_power_down = should_power_down();
115 3172 : if (power_state.commanded_power_down || power_state.current_power_down) {
116 147 : set_status(RangeFinder::Status::PoweredDown);
117 : // Clear accumulation
118 147 : accum.sum = 0;
119 147 : accum.count = 0;
120 147 : return;
121 : }
122 :
123 : // Average samples
124 3025 : if (accum.count > 0) {
125 3025 : state.distance_m = (accum.sum * 0.01f) / accum.count;
126 3025 : state.last_reading_ms = AP_HAL::millis();
127 3025 : accum.sum = 0;
128 3025 : accum.count = 0;
129 3025 : update_status();
130 0 : } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
131 : // Timeout for bad data
132 0 : set_status(RangeFinder::Status::NoData);
133 : }
134 3172 : }
135 :
136 7751 : void AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw, le16_t strength_raw,
137 : uint16_t &output_distance_cm)
138 : {
139 7751 : uint16_t strength = le16toh(strength_raw);
140 7751 : const uint16_t MAX_DIST_CM = 1200;
141 7751 : const uint16_t MIN_DIST_CM = 10;
142 :
143 7751 : output_distance_cm = le16toh(distance_raw);
144 :
145 7751 : if (strength < 100 || strength == 0xFFFF || output_distance_cm > MAX_DIST_CM) {
146 : /*
147 : * From manual: "when the signal strength is lower than 100 or equal to
148 : * 65535, the detection is unreliable, TFmini Plus will set distance
149 : * value to 0." - force it to the max distance so status is set to OutOfRangeHigh
150 : * rather than NoData.
151 : */
152 0 : output_distance_cm = MAX(MAX_DIST_CM, max_distance()*100 + BENEWAKE_OUT_OF_RANGE_ADD_CM);
153 : } else {
154 7751 : output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM);
155 : }
156 7751 : }
157 :
158 7753 : bool AP_RangeFinder_Benewake_TFMiniPlus::check_checksum(uint8_t *arr, int pkt_len)
159 : {
160 7753 : uint8_t checksum = 0;
161 : int i;
162 :
163 : /* sum them all except the last (the checksum) */
164 85275 : for (i = 0; i < pkt_len - 1; i++) {
165 77522 : checksum += arr[i];
166 : }
167 :
168 7753 : return checksum == arr[pkt_len - 1];
169 : }
170 :
171 8117 : void AP_RangeFinder_Benewake_TFMiniPlus::timer()
172 : {
173 : bool change_power_state;
174 : bool power_down;
175 : {
176 : // Update power flags with semaphore
177 8117 : WITH_SEMAPHORE(_sem);
178 8117 : change_power_state = power_state.current_power_down != power_state.commanded_power_down;
179 8117 : power_down = power_state.commanded_power_down;
180 8117 : }
181 :
182 8117 : if (change_power_state) {
183 : // Change in power state
184 : bool success;
185 2 : if (power_down) {
186 : // Send power down command
187 1 : const uint8_t CMD_DISABLE[] = { CMD_DISABLE_DATA_OUT };
188 1 : success = dev.transfer(CMD_DISABLE, sizeof(CMD_DISABLE), nullptr, 0);
189 :
190 : } else {
191 : // Send power up command
192 1 : const uint8_t CMD_ENABLE[] = { CMD_ENABLE_DATA_OUT };
193 1 : success = dev.transfer(CMD_ENABLE, sizeof(CMD_ENABLE), nullptr, 0);
194 : }
195 :
196 : // Mark state as changed if command was written
197 2 : if (success) {
198 2 : WITH_SEMAPHORE(_sem);
199 2 : power_state.current_power_down = power_down;
200 2 : }
201 : // Wait until next call before trying to read data
202 366 : return;
203 : }
204 :
205 8115 : if (power_down) {
206 : // Powered down, nothing to do
207 364 : return;
208 : }
209 :
210 7751 : uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
211 : union {
212 : struct PACKED {
213 : uint8_t header1;
214 : uint8_t header2;
215 : le16_t distance;
216 : le16_t strength;
217 : le32_t timestamp;
218 : uint8_t checksum;
219 : } val;
220 : uint8_t arr[11];
221 : } u;
222 : uint16_t distance;
223 :
224 7751 : if (!dev.transfer(CMD_READ_MEASUREMENT, sizeof(CMD_READ_MEASUREMENT), nullptr, 0)) {
225 : // Failed write
226 0 : return;
227 : }
228 :
229 : // Short delay helps avoid bad data, despite datasheet saying its not needed
230 7751 : hal.scheduler->delay_microseconds(10);
231 :
232 7751 : if (!dev.transfer(nullptr, 0, (uint8_t *)&u, sizeof(u))) {
233 : // Failed read
234 0 : return;
235 : }
236 :
237 7751 : if (u.val.header1 != 0x59 || u.val.header2 != 0x59 || !check_checksum(u.arr, sizeof(u))) {
238 0 : return;
239 : }
240 :
241 7751 : process_raw_measure(u.val.distance, u.val.strength, distance);
242 :
243 : {
244 7751 : WITH_SEMAPHORE(_sem);
245 7751 : accum.sum += distance;
246 7751 : accum.count++;
247 7751 : }
248 : }
249 :
250 : #endif // AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED
|
