Line data Source code
1 : #include "AP_RangeFinder_config.h"
2 :
3 : #if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
4 :
5 : #include "AP_RangeFinder_LightWare_GRF.h"
6 : #include <AP_HAL/AP_HAL.h>
7 : #include <AP_HAL/utility/sparse-endian.h>
8 : #include <GCS_MAVLink/GCS.h>
9 :
10 : #define GRF_STREAM_CM_DISTANCES 5
11 :
12 : extern const AP_HAL::HAL& hal;
13 :
14 : const AP_Param::GroupInfo AP_RangeFinder_LightWareGRF::var_info[] = {
15 : // @Group: GRF_
16 : // @Path: AP_RangeFinder_LightWare_GRF_Common.cpp
17 : AP_SUBGROUPINFO(return_selection, "GRF_", 12, AP_RangeFinder_LightWareGRF, AP_RangeFinder_LightWare_GRF_Common),
18 :
19 : AP_GROUPEND
20 : };
21 :
22 1 : AP_RangeFinder_LightWareGRF::AP_RangeFinder_LightWareGRF(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params)
23 : : AP_RangeFinder_Backend_Serial(_state, _params),
24 1 : AP_LightWareSerial(AP_RangeFinder_Backend_Serial::uart)
25 : {
26 1 : state.var_info = var_info;
27 1 : }
28 :
29 : // Parses config responses and advances setup step
30 7 : void AP_RangeFinder_LightWareGRF::check_config(const MessageID &resp_cmd_id, const uint8_t* response_buf, const uint16_t& response_len)
31 : {
32 7 : bool valid = false;
33 :
34 7 : switch (config_step) {
35 1 : case ConfigStep::HANDSHAKE:
36 2 : valid = (resp_cmd_id == MessageID::PRODUCT_NAME &&
37 1 : matches_product_name(response_buf, response_len));
38 1 : if (valid) {
39 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LightWare %s detected", (const char*)response_buf);
40 : }
41 1 : break;
42 :
43 2 : case ConfigStep::UPDATE_RATE:
44 2 : if (resp_cmd_id == MessageID::UPDATE_RATE && response_len >= 4) {
45 1 : const uint8_t response_update_rate = (uint8_t)UINT32_VALUE(response_buf[3], response_buf[2], response_buf[1], response_buf[0]);
46 1 : const uint8_t expected = constrain_int16(update_rate, 1, GRF_MAX_RATE_HZ);
47 1 : valid = (response_update_rate == expected);
48 : }
49 2 : break;
50 :
51 2 : case ConfigStep::DISTANCE_OUTPUT:
52 2 : valid = (resp_cmd_id == MessageID::DISTANCE_OUTPUT);
53 2 : break;
54 :
55 2 : case ConfigStep::STREAM:
56 2 : valid = (resp_cmd_id == MessageID::STREAM);
57 2 : break;
58 :
59 0 : case ConfigStep::DONE:
60 0 : break;
61 : }
62 :
63 7 : if (valid && config_step != ConfigStep::DONE) {
64 4 : config_step = static_cast<ConfigStep>(uint8_t(config_step) + 1);
65 4 : return;
66 : }
67 : }
68 :
69 : // Send configuration messages to the rangefinder
70 16 : void AP_RangeFinder_LightWareGRF::configure_rangefinder()
71 : {
72 16 : if (config_step == ConfigStep::DONE) {
73 0 : return;
74 : }
75 :
76 16 : const uint32_t now = AP_HAL::millis();
77 16 : if (now - last_config_message_ms < 100) {
78 8 : return;
79 : }
80 :
81 8 : switch (config_step) {
82 :
83 2 : case ConfigStep::HANDSHAKE:
84 2 : uart->write((uint8_t*)"UUU", 3); // Try to switch GRF sensor to serial mode
85 2 : send_message((uint8_t)MessageID::PRODUCT_NAME, false, nullptr, 0);
86 2 : break;
87 :
88 2 : case ConfigStep::UPDATE_RATE: {
89 : // Cap to a sane ceiling so a misconfigured GRF_RATE doesn't go on the wire.
90 2 : const uint8_t rate = constrain_int16(update_rate, 1, GRF_MAX_RATE_HZ);
91 2 : const uint8_t payload[4] = {rate, 0, 0, 0};
92 2 : send_message((uint8_t)MessageID::UPDATE_RATE, true, payload, 4);
93 2 : break;
94 : }
95 :
96 2 : case ConfigStep::DISTANCE_OUTPUT: {
97 : uint8_t payload[4];
98 2 : put_le32_ptr(payload, build_distance_output_bitmask());
99 2 : send_message((uint8_t)MessageID::DISTANCE_OUTPUT, true, payload, sizeof(payload));
100 2 : break;
101 : }
102 :
103 2 : case ConfigStep::STREAM: {
104 2 : const uint8_t payload[4] = {GRF_STREAM_CM_DISTANCES, 0, 0, 0};
105 2 : send_message((uint8_t)MessageID::STREAM, true, payload, 4);
106 2 : break;
107 : }
108 :
109 0 : case ConfigStep::DONE:
110 0 : break;
111 : }
112 :
113 8 : last_config_message_ms = now;
114 : }
115 :
116 : // Processes the latest message held in the _msg structure
117 3412 : void AP_RangeFinder_LightWareGRF::process_message(float &sum_m, uint8_t &count)
118 : {
119 3412 : MessageID cmd_id = (MessageID)_msg.msgid;
120 :
121 3412 : if (config_step != ConfigStep::DONE) {
122 : // sensor configuration in progress
123 7 : check_config(cmd_id, _msg.payload, _msg.payload_len);
124 7 : return;
125 : }
126 :
127 3405 : if (cmd_id != MessageID::DISTANCE_DATA_CM) {
128 : // beyond the configuration steps we only expect distance data messages
129 0 : return;
130 : }
131 :
132 : float dist_m;
133 3405 : if (parse_distance_cm_payload(_msg.payload, _msg.payload_len, dist_m)) {
134 3405 : sum_m += dist_m;
135 3405 : count++;
136 : }
137 : }
138 :
139 : // Called periodically to fetch a new range reading
140 1369 : bool AP_RangeFinder_LightWareGRF::get_reading(float &reading_m)
141 : {
142 1369 : if (uart == nullptr) {
143 0 : return false;
144 : }
145 :
146 1369 : if (config_step != ConfigStep::DONE) {
147 16 : configure_rangefinder();
148 : }
149 :
150 1369 : float sum_m = 0.0f;
151 1369 : uint8_t count = 0;
152 :
153 : // process up to 1K of characters per iteration
154 1369 : uint32_t nbytes = MIN(uart->available(), 1024U);
155 49115 : while (nbytes-- > 0) {
156 : uint8_t c;
157 47746 : if (!uart->read(c)) {
158 0 : continue;
159 : }
160 47746 : if (parse_byte(c)) {
161 3412 : process_message(sum_m, count);
162 : }
163 : }
164 1369 : if (count > 0) {
165 1354 : reading_m = sum_m / count;
166 1354 : return true;
167 : }
168 15 : return false;
169 : }
170 :
171 : #endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
|
