Line data Source code
1 : #include "AP_RangeFinder_config.h"
2 :
3 : #if AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
4 :
5 : #include "AP_RangeFinder_LightWare_GRF_I2C.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_HAL/utility/sparse-endian.h>
9 : #include <GCS_MAVLink/GCS.h>
10 :
11 : extern const AP_HAL::HAL& hal;
12 :
13 : #define GRF_I2C_PRODUCT_NAME_PAYLOAD_LEN 16
14 : #define GRF_I2C_U32_PAYLOAD_LEN 4
15 : #define GRF_I2C_DISTANCE_PAYLOAD_LEN 8
16 : #define GRF_I2C_MAX_WRITE_PAYLOAD_LEN 16
17 : #define GRF_I2C_HANDSHAKE_ATTEMPTS 4
18 : #define GRF_I2C_HANDSHAKE_RETRY_MS 30
19 : #define GRF_I2C_POLL_HZ 50
20 :
21 : const AP_Param::GroupInfo AP_RangeFinder_LightWare_GRF_I2C::var_info[] = {
22 : // @Group: GRF_
23 : // @Path: AP_RangeFinder_LightWare_GRF_Common.cpp
24 : AP_SUBGROUPINFO(return_selection, "GRF_", 12, AP_RangeFinder_LightWare_GRF_I2C, AP_RangeFinder_LightWare_GRF_Common),
25 :
26 : AP_GROUPEND
27 : };
28 :
29 3 : AP_RangeFinder_LightWare_GRF_I2C::AP_RangeFinder_LightWare_GRF_I2C(
30 : RangeFinder::RangeFinder_State &_state,
31 : AP_RangeFinder_Params &_params,
32 3 : AP_HAL::I2CDevice &_dev)
33 3 : : AP_RangeFinder_Backend_I2C(_state, _params, _dev)
34 : {
35 3 : state.var_info = var_info;
36 3 : }
37 :
38 : // Write a command
39 2 : bool AP_RangeFinder_LightWare_GRF_I2C::write_command(MessageID msgid,
40 : const uint8_t *payload,
41 : uint16_t payload_len)
42 : {
43 : uint8_t tx[1 + GRF_I2C_MAX_WRITE_PAYLOAD_LEN];
44 2 : if (1U + payload_len > sizeof(tx)) {
45 0 : return false;
46 : }
47 2 : tx[0] = (uint8_t)msgid;
48 2 : if (payload_len > 0 && payload != nullptr) {
49 2 : memcpy(&tx[1], payload, payload_len);
50 : }
51 2 : return dev.transfer(tx, 1U + payload_len, nullptr, 0);
52 : }
53 :
54 : // Read a register and hand back the raw bytes.
55 3457 : bool AP_RangeFinder_LightWare_GRF_I2C::read_payload(MessageID msgid,
56 : uint8_t *payload_buf,
57 : uint16_t payload_len)
58 : {
59 3457 : const uint8_t reg = (uint8_t)msgid;
60 3457 : return dev.transfer(®, 1, payload_buf, payload_len);
61 : }
62 :
63 3 : bool AP_RangeFinder_LightWare_GRF_I2C::init()
64 : {
65 3 : WITH_SEMAPHORE(dev.get_semaphore());
66 3 : dev.set_retries(2);
67 :
68 : // Ask the sensor for its product name. The reply starts with "GRF" if
69 : // we've found a GRF-family rangefinder. Retry a few times in case the
70 : // sensor is still waking up.
71 : uint8_t name[GRF_I2C_PRODUCT_NAME_PAYLOAD_LEN];
72 3 : bool detected = false;
73 12 : for (uint8_t attempt = 0; attempt < GRF_I2C_HANDSHAKE_ATTEMPTS && !detected; attempt++) {
74 10 : if (!read_payload(MessageID::PRODUCT_NAME, name, sizeof(name)) ||
75 1 : !matches_product_name(name, sizeof(name))) {
76 8 : hal.scheduler->delay(GRF_I2C_HANDSHAKE_RETRY_MS);
77 8 : continue;
78 : }
79 1 : detected = true;
80 : }
81 3 : if (!detected) {
82 2 : return false;
83 : }
84 1 : name[sizeof(name) - 1] = '\0';
85 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LightWare %s detected (I2C)", (const char*)name);
86 :
87 : // Configuration happens inside the timer so we don't hold up boot.
88 1 : _config_step = ConfigStep::WRITE_UPDATE_RATE;
89 1 : _config_retry = 0;
90 :
91 1 : dev.register_periodic_callback(1000000UL / GRF_I2C_POLL_HZ,
92 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWare_GRF_I2C::timer, void));
93 :
94 1 : return true;
95 3 : }
96 :
97 : // Run one tick of the config FSM: each call does exactly one I2C op.
98 : // Each WRITE_* step is followed by a VERIFY_* step that reads the register
99 : // back; mismatches retry the write a few times then give up and move on.
100 4 : void AP_RangeFinder_LightWare_GRF_I2C::run_config_step()
101 : {
102 : uint8_t payload[GRF_I2C_U32_PAYLOAD_LEN];
103 : uint8_t rx[GRF_I2C_U32_PAYLOAD_LEN];
104 :
105 : // Cap the user-set rate so a misconfigured GRF_RATE doesn't escape onto
106 : // the bus. The ceiling is well above the datasheet max — sane values
107 : // pass through untouched.
108 4 : const uint32_t rate_value = constrain_int16(update_rate, 1, GRF_MAX_RATE_HZ);
109 :
110 4 : switch (_config_step) {
111 1 : case ConfigStep::WRITE_UPDATE_RATE:
112 1 : put_le32_ptr(payload, rate_value);
113 1 : (void)write_command(MessageID::UPDATE_RATE, payload, sizeof(payload));
114 1 : _config_step = ConfigStep::VERIFY_UPDATE_RATE;
115 4 : return;
116 :
117 1 : case ConfigStep::VERIFY_UPDATE_RATE:
118 1 : put_le32_ptr(payload, rate_value);
119 2 : if (!read_payload(MessageID::UPDATE_RATE, rx, sizeof(rx)) ||
120 1 : memcmp(rx, payload, sizeof(rx)) != 0) {
121 0 : if (++_config_retry < 3) {
122 0 : _config_step = ConfigStep::WRITE_UPDATE_RATE;
123 0 : return;
124 : }
125 : }
126 1 : _config_retry = 0;
127 1 : _config_step = ConfigStep::WRITE_DISTANCE_OUTPUT;
128 1 : return;
129 :
130 1 : case ConfigStep::WRITE_DISTANCE_OUTPUT:
131 1 : put_le32_ptr(payload, build_distance_output_bitmask());
132 1 : (void)write_command(MessageID::DISTANCE_OUTPUT, payload, sizeof(payload));
133 1 : _config_step = ConfigStep::VERIFY_DISTANCE_OUTPUT;
134 1 : return;
135 :
136 1 : case ConfigStep::VERIFY_DISTANCE_OUTPUT:
137 1 : put_le32_ptr(payload, build_distance_output_bitmask());
138 2 : if (!read_payload(MessageID::DISTANCE_OUTPUT, rx, sizeof(rx)) ||
139 1 : memcmp(rx, payload, sizeof(rx)) != 0) {
140 0 : if (++_config_retry < 3) {
141 0 : _config_step = ConfigStep::WRITE_DISTANCE_OUTPUT;
142 0 : return;
143 : }
144 : }
145 1 : _config_retry = 0;
146 1 : _config_step = ConfigStep::DONE;
147 1 : return;
148 :
149 0 : case ConfigStep::DONE:
150 0 : return;
151 : }
152 : }
153 :
154 3450 : void AP_RangeFinder_LightWare_GRF_I2C::timer()
155 : {
156 : // Finish boot-time config before we start polling for distance readings.
157 : // The gap between timer calls gives the sensor time to apply each write.
158 3450 : if (_config_step != ConfigStep::DONE) {
159 4 : run_config_step();
160 4 : return;
161 : }
162 :
163 : uint8_t rx[GRF_I2C_DISTANCE_PAYLOAD_LEN];
164 3446 : if (!read_payload(MessageID::DISTANCE_DATA_CM, rx, sizeof(rx))) {
165 0 : return;
166 : }
167 :
168 : float dist_m;
169 3446 : if (!parse_distance_cm_payload(rx, sizeof(rx), dist_m)) {
170 0 : return;
171 : }
172 :
173 3446 : WITH_SEMAPHORE(_sem);
174 3446 : _accum.sum_m += dist_m;
175 3446 : _accum.count++;
176 3446 : }
177 :
178 1348 : void AP_RangeFinder_LightWare_GRF_I2C::update()
179 : {
180 1348 : WITH_SEMAPHORE(_sem);
181 :
182 1348 : if (_accum.count > 0) {
183 1348 : state.distance_m = _accum.sum_m / _accum.count;
184 1348 : state.last_reading_ms = AP_HAL::millis();
185 1348 : _accum.sum_m = 0;
186 1348 : _accum.count = 0;
187 1348 : update_status();
188 0 : } else if (AP_HAL::millis() - state.last_reading_ms > 500) {
189 0 : set_status(RangeFinder::Status::NoData);
190 : }
191 1348 : }
192 :
193 : #endif // AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
|
