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 :
16 : #include "AP_Proximity_config.h"
17 :
18 : #if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
19 :
20 : #include "AP_Proximity_LightWareSF40C.h"
21 :
22 : #include <AP_Common/AP_Common.h>
23 : #include <AP_HAL/AP_HAL.h>
24 : #include <AP_HAL/utility/sparse-endian.h>
25 : #include <AP_Math/crc.h>
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : #define PROXIMITY_SF40C_HEADER 0xAA
30 : #define PROXIMITY_SF40C_DESIRED_OUTPUT_RATE 3
31 :
32 : // update the state of the sensor
33 0 : void AP_Proximity_LightWareSF40C::update(void)
34 : {
35 0 : if (_uart == nullptr) {
36 0 : return;
37 : }
38 :
39 : // initialise sensor if necessary
40 0 : initialise();
41 :
42 : // process incoming messages
43 0 : process_replies();
44 :
45 : // check for timeout and set health status
46 0 : if ((_last_distance_received_ms == 0) || ((AP_HAL::millis() - _last_distance_received_ms) > PROXIMITY_SF40C_TIMEOUT_MS)) {
47 0 : set_status(AP_Proximity::Status::NoData);
48 : } else {
49 0 : set_status(AP_Proximity::Status::Good);
50 : }
51 : }
52 :
53 : // initialise sensor
54 0 : void AP_Proximity_LightWareSF40C::initialise()
55 : {
56 : // exit immediately if we've sent initialisation requests in the last second
57 0 : uint32_t now_ms = AP_HAL::millis();
58 0 : if ((now_ms - _last_request_ms) < 1000) {
59 0 : return;
60 : }
61 0 : _last_request_ms = now_ms;
62 :
63 : // re-fetch motor state
64 0 : request_motor_state();
65 :
66 : // get token from sensor (required for reseting)
67 0 : if (!got_token()) {
68 0 : request_token();
69 0 : return;
70 : }
71 :
72 : // if no replies in last 15 seconds reboot sensor
73 0 : if ((now_ms > 30000) && (now_ms - _last_reply_ms > 15000)) {
74 0 : restart_sensor();
75 0 : return;
76 : }
77 :
78 : // if motor is starting up give more time to succeed or fail
79 0 : if ((_sensor_state.motor_state != MotorState::RUNNING_NORMALLY) &&
80 0 : (_sensor_state.motor_state != MotorState::FAILED_TO_COMMUNICATE)) {
81 0 : return;
82 : }
83 :
84 : // if motor fails, reset sensor and re-try everything
85 0 : if (_sensor_state.motor_state == MotorState::FAILED_TO_COMMUNICATE) {
86 0 : restart_sensor();
87 0 : return;
88 : }
89 :
90 : // motor is running correctly (motor_state is RUNNING_NORMALLY) so request start of streaming
91 0 : if (!_sensor_state.streaming || (_sensor_state.output_rate != PROXIMITY_SF40C_DESIRED_OUTPUT_RATE)) {
92 0 : request_stream_start();
93 0 : return;
94 : }
95 : }
96 :
97 : // restart sensor and re-init our state
98 0 : void AP_Proximity_LightWareSF40C::restart_sensor()
99 : {
100 : // return immediately if no token or a restart has been requested within the last 30sec
101 0 : uint32_t now_ms = AP_HAL::millis();
102 0 : if ((_last_restart_ms != 0) && ((now_ms - _last_restart_ms) < 30000)) {
103 0 : return;
104 : }
105 :
106 : // restart sensor and re-initialise sensor state
107 0 : request_reset();
108 0 : clear_token();
109 0 : _last_restart_ms = now_ms;
110 0 : _sensor_state.motor_state = MotorState::UNKNOWN;
111 0 : _sensor_state.streaming = false;
112 0 : _sensor_state.output_rate = 0;
113 : }
114 :
115 : // send message to sensor
116 0 : void AP_Proximity_LightWareSF40C::send_message(MessageID msgid, bool write, const uint8_t *payload, uint16_t payload_len)
117 : {
118 0 : if ((_uart == nullptr) || (payload_len > PROXIMITY_SF40C_PAYLOAD_LEN_MAX)) {
119 0 : return;
120 : }
121 :
122 : // check for sufficient space in outgoing buffer
123 0 : if (_uart->txspace() < payload_len + 6U) {
124 0 : return;
125 : }
126 :
127 : // write header
128 0 : _uart->write((uint8_t)PROXIMITY_SF40C_HEADER);
129 0 : uint16_t crc = crc_xmodem_update(0, PROXIMITY_SF40C_HEADER);
130 :
131 : // write flags including payload length
132 0 : const uint16_t flags = ((payload_len+1) << 6) | (write ? 0x01 : 0);
133 0 : _uart->write(LOWBYTE(flags));
134 0 : crc = crc_xmodem_update(crc, LOWBYTE(flags));
135 0 : _uart->write(HIGHBYTE(flags));
136 0 : crc = crc_xmodem_update(crc, HIGHBYTE(flags));
137 :
138 : // msgid
139 0 : _uart->write((uint8_t)msgid);
140 0 : crc = crc_xmodem_update(crc, (uint8_t)msgid);
141 :
142 : // payload
143 0 : if ((payload_len > 0) && (payload != nullptr)) {
144 0 : for (uint16_t i = 0; i < payload_len; i++) {
145 0 : _uart->write(payload[i]);
146 0 : crc = crc_xmodem_update(crc, payload[i]);
147 : }
148 : }
149 :
150 : // checksum
151 0 : _uart->write(LOWBYTE(crc));
152 0 : _uart->write(HIGHBYTE(crc));
153 : }
154 :
155 : // request motor state
156 0 : void AP_Proximity_LightWareSF40C::request_motor_state()
157 : {
158 0 : send_message(MessageID::MOTOR_STATE, false, (const uint8_t *)nullptr, 0);
159 0 : }
160 :
161 : // request start of streaming of distances
162 0 : void AP_Proximity_LightWareSF40C::request_stream_start()
163 : {
164 : // request output rate
165 0 : const uint8_t desired_rate = PROXIMITY_SF40C_DESIRED_OUTPUT_RATE; // 0 = 20010, 1 = 10005, 2 = 6670, 3 = 2001
166 0 : send_message(MessageID::OUTPUT_RATE, true, &desired_rate, sizeof(desired_rate));
167 :
168 : // request streaming to start
169 0 : const le32_t val = htole32(3);
170 0 : send_message(MessageID::STREAM, true, (const uint8_t*)&val, sizeof(val));
171 0 : }
172 :
173 : // request token of sensor
174 0 : void AP_Proximity_LightWareSF40C::request_token()
175 : {
176 : // request token
177 0 : send_message(MessageID::TOKEN, false, nullptr, 0);
178 0 : }
179 :
180 : // request reset of sensor
181 0 : void AP_Proximity_LightWareSF40C::request_reset()
182 : {
183 : // send reset request
184 0 : send_message(MessageID::RESET, true, _sensor_state.token, ARRAY_SIZE(_sensor_state.token));
185 0 : }
186 :
187 : // check for replies from sensor
188 0 : void AP_Proximity_LightWareSF40C::process_replies()
189 : {
190 0 : if (_uart == nullptr) {
191 0 : return;
192 : }
193 :
194 0 : int16_t nbytes = _uart->available();
195 0 : while (nbytes-- > 0) {
196 0 : const int16_t r = _uart->read();
197 0 : if ((r < 0) || (r > 0xFF)) {
198 0 : continue;
199 : }
200 0 : parse_byte((uint8_t)r);
201 : }
202 : }
203 :
204 : // process one byte received on serial port
205 : // state is stored in _msg structure
206 0 : void AP_Proximity_LightWareSF40C::parse_byte(uint8_t b)
207 : {
208 : // check that payload buffer is large enough
209 : static_assert(ARRAY_SIZE(_msg.payload) == PROXIMITY_SF40C_PAYLOAD_LEN_MAX, "AP_Proximity_LightWareSF40C: check _msg.payload array size");
210 :
211 : // process byte depending upon current state
212 0 : switch (_msg.state) {
213 :
214 0 : case ParseState::HEADER:
215 0 : if (b == PROXIMITY_SF40C_HEADER) {
216 0 : _msg.crc_expected = crc_xmodem_update(0, b);
217 0 : _msg.state = ParseState::FLAGS_L;
218 : }
219 0 : break;
220 :
221 0 : case ParseState::FLAGS_L:
222 0 : _msg.flags_low = b;
223 0 : _msg.crc_expected = crc_xmodem_update(_msg.crc_expected, b);
224 0 : _msg.state = ParseState::FLAGS_H;
225 0 : break;
226 :
227 0 : case ParseState::FLAGS_H:
228 0 : _msg.flags_high = b;
229 0 : _msg.crc_expected = crc_xmodem_update(_msg.crc_expected, b);
230 0 : _msg.payload_len = UINT16_VALUE(_msg.flags_high, _msg.flags_low) >> 6;
231 0 : if ((_msg.payload_len == 0) || (_msg.payload_len > PROXIMITY_SF40C_PAYLOAD_LEN_MAX)) {
232 : // invalid payload length, abandon message
233 0 : _msg.state = ParseState::HEADER;
234 : } else {
235 0 : _msg.state = ParseState::MSG_ID;
236 : }
237 0 : break;
238 :
239 0 : case ParseState::MSG_ID:
240 0 : _msg.msgid = (MessageID)b;
241 0 : _msg.crc_expected = crc_xmodem_update(_msg.crc_expected, b);
242 0 : if (_msg.payload_len > 1) {
243 0 : _msg.state = ParseState::PAYLOAD;
244 : } else {
245 0 : _msg.state = ParseState::CRC_L;
246 : }
247 0 : _msg.payload_recv = 0;
248 0 : break;
249 :
250 0 : case ParseState::PAYLOAD:
251 0 : if (_msg.payload_recv < (_msg.payload_len - 1)) {
252 0 : _msg.payload[_msg.payload_recv] = b;
253 0 : _msg.payload_recv++;
254 0 : _msg.crc_expected = crc_xmodem_update(_msg.crc_expected, b);
255 : }
256 0 : if (_msg.payload_recv >= (_msg.payload_len - 1)) {
257 0 : _msg.state = ParseState::CRC_L;
258 : }
259 0 : break;
260 :
261 0 : case ParseState::CRC_L:
262 0 : _msg.crc_low = b;
263 0 : _msg.state = ParseState::CRC_H;
264 0 : break;
265 :
266 0 : case ParseState::CRC_H:
267 0 : _msg.crc_high = b;
268 0 : if (_msg.crc_expected == UINT16_VALUE(_msg.crc_high, _msg.crc_low)) {
269 0 : process_message();
270 0 : _last_reply_ms = AP_HAL::millis();
271 : }
272 0 : _msg.state = ParseState::HEADER;
273 0 : break;
274 : }
275 0 : }
276 :
277 : // process the latest message held in the _msg structure
278 0 : void AP_Proximity_LightWareSF40C::process_message()
279 : {
280 : // process payload
281 0 : switch (_msg.msgid) {
282 0 : case MessageID::TOKEN:
283 : // copy token into _sensor_state.token variable
284 0 : if (_msg.payload_recv == ARRAY_SIZE(_sensor_state.token)) {
285 0 : memcpy(_sensor_state.token, _msg.payload, ARRAY_SIZE(_sensor_state.token));
286 : }
287 0 : break;
288 0 : case MessageID::RESET:
289 : // no need to do anything
290 0 : break;
291 0 : case MessageID::STREAM:
292 0 : if (_msg.payload_recv == sizeof(uint32_t)) {
293 0 : _sensor_state.streaming = (buff_to_uint32(_msg.payload[0], _msg.payload[1], _msg.payload[2], _msg.payload[3]) == 3);
294 : }
295 0 : break;
296 0 : case MessageID::DISTANCE_OUTPUT: {
297 0 : _last_distance_received_ms = AP_HAL::millis();
298 0 : const uint16_t point_total = buff_to_uint16(_msg.payload[8], _msg.payload[9]);
299 0 : const uint16_t point_count = buff_to_uint16(_msg.payload[10], _msg.payload[11]);
300 0 : const uint16_t point_start_index = buff_to_uint16(_msg.payload[12], _msg.payload[13]);
301 : // sanity check point_total
302 0 : if (point_total == 0) {
303 0 : break;
304 : }
305 :
306 : // prepare to push to object database
307 0 : Vector3f current_pos;
308 0 : Matrix3f body_to_ned;
309 0 : const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
310 :
311 : // process each point
312 0 : const float angle_inc_deg = (1.0f / point_total) * 360.0f;
313 0 : const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;
314 0 : const float angle_correction = params.yaw_correction;
315 0 : const uint16_t dist_min_cm = distance_min_m() * 100;
316 0 : const uint16_t dist_max_cm = distance_max_m() * 100;
317 :
318 : // mini sectors are used to combine several readings together
319 0 : uint8_t combined_count = 0;
320 0 : float combined_angle_deg = 0;
321 0 : float combined_dist_m = INT16_MAX;
322 0 : for (uint16_t i = 0; i < point_count; i++) {
323 0 : const uint16_t idx = 14 + (i * 2);
324 0 : const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
325 0 : const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
326 0 : const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
327 :
328 : // if point is on a new face then finish off previous face
329 0 : if (face != _face) {
330 : // update boundary used for avoidance
331 0 : if (_face_distance_valid) {
332 0 : frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance);
333 : } else {
334 : // mark previous face invalid
335 0 : frontend.boundary.reset_face(_face, state.instance);
336 : }
337 : // init for new face
338 0 : _face = face;
339 0 : _face_distance_valid = false;
340 : }
341 :
342 : // check reading is not within an ignore zone
343 0 : const float dist_m = dist_cm * 0.01f;
344 0 : if (!ignore_reading(angle_deg, dist_m)) {
345 : // check distance reading is valid
346 0 : if ((dist_cm >= dist_min_cm) && (dist_cm <= dist_max_cm)) {
347 : // update shortest distance for this face
348 0 : if (!_face_distance_valid || dist_m < _face_distance) {
349 0 : _face_distance = dist_m;
350 0 : _face_distance_valid = true;
351 : }
352 :
353 : // calculate shortest of last few readings
354 0 : if (dist_m < combined_dist_m) {
355 0 : combined_dist_m = dist_m;
356 0 : combined_angle_deg = angle_deg;
357 : }
358 0 : combined_count++;
359 : }
360 : }
361 :
362 : // send combined distance to object database
363 0 : if ((i+1 >= point_count) || (combined_count >= PROXIMITY_SF40C_COMBINE_READINGS)) {
364 0 : if ((combined_dist_m < INT16_MAX) && database_ready) {
365 0 : database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_pos,body_to_ned);
366 : }
367 0 : combined_count = 0;
368 0 : combined_dist_m = INT16_MAX;
369 : }
370 : }
371 0 : break;
372 : }
373 0 : case MessageID::MOTOR_STATE:
374 0 : if (_msg.payload_recv == 1) {
375 0 : _sensor_state.motor_state = (MotorState)_msg.payload[0];
376 : }
377 0 : break;
378 0 : case MessageID::OUTPUT_RATE:
379 0 : if (_msg.payload_recv == 1) {
380 0 : _sensor_state.output_rate = _msg.payload[0];
381 : }
382 0 : break;
383 :
384 : // unsupported messages
385 0 : case MessageID::PRODUCT_NAME:
386 : case MessageID::HARDWARE_VERSION:
387 : case MessageID::FIRMWARE_VERSION:
388 : case MessageID::SERIAL_NUMBER:
389 : case MessageID::TEXT_MESSAGE:
390 : case MessageID::USER_DATA:
391 : case MessageID::SAVE_PARAMETERS:
392 : case MessageID::STAGE_FIRMWARE:
393 : case MessageID::COMMIT_FIRMWARE:
394 : case MessageID::INCOMING_VOLTAGE:
395 : case MessageID::LASER_FIRING:
396 : case MessageID::TEMPERATURE:
397 : case MessageID::BAUD_RATE:
398 : case MessageID::DISTANCE:
399 : case MessageID::MOTOR_VOLTAGE:
400 : case MessageID::FORWARD_OFFSET:
401 : case MessageID::REVOLUTIONS:
402 : case MessageID::ALARM_STATE:
403 : case MessageID::ALARM1:
404 : case MessageID::ALARM2:
405 : case MessageID::ALARM3:
406 : case MessageID::ALARM4:
407 : case MessageID::ALARM5:
408 : case MessageID::ALARM6:
409 : case MessageID::ALARM7:
410 0 : break;
411 : }
412 0 : }
413 :
414 : // convert buffer to uint32, uint16
415 0 : uint32_t AP_Proximity_LightWareSF40C::buff_to_uint32(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3) const
416 : {
417 0 : uint32_t leval = (uint32_t)b0 | (uint32_t)b1 << 8 | (uint32_t)b2 << 16 | (uint32_t)b3 << 24;
418 0 : return leval;
419 : }
420 :
421 0 : uint16_t AP_Proximity_LightWareSF40C::buff_to_uint16(uint8_t b0, uint8_t b1) const
422 : {
423 0 : uint16_t leval = (uint16_t)b0 | (uint16_t)b1 << 8;
424 0 : return leval;
425 : }
426 :
427 : #endif // AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
|
