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_WheelEncoder.h"
17 : #include "WheelEncoder_Quadrature.h"
18 : #include "WheelEncoder_SITL_Quadrature.h"
19 : #include <AP_Logger/AP_Logger.h>
20 :
21 : extern const AP_HAL::HAL& hal;
22 :
23 : // table of user settable parameters
24 : const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
25 : // @Param: _TYPE
26 : // @DisplayName: WheelEncoder type
27 : // @Description: What type of WheelEncoder is connected
28 : // @Values: 0:None,1:Quadrature,10:SITL Quadrature
29 : // @RebootRequired: True
30 : // @User: Standard
31 : AP_GROUPINFO_FLAGS("_TYPE", 0, AP_WheelEncoder, _type[0], 0, AP_PARAM_FLAG_ENABLE),
32 :
33 : // @Param: _CPR
34 : // @DisplayName: WheelEncoder counts per revolution
35 : // @Description: WheelEncoder counts per full revolution of the wheel
36 : // @Increment: 1
37 : // @User: Standard
38 : AP_GROUPINFO("_CPR", 1, AP_WheelEncoder, _counts_per_revolution[0], WHEELENCODER_CPR_DEFAULT),
39 :
40 : // @Param: _RADIUS
41 : // @DisplayName: Wheel radius
42 : // @Description: Wheel radius
43 : // @Units: m
44 : // @Increment: 0.001
45 : // @User: Standard
46 : AP_GROUPINFO("_RADIUS", 2, AP_WheelEncoder, _wheel_radius[0], WHEELENCODER_RADIUS_DEFAULT),
47 :
48 : // @Param: _POS_X
49 : // @DisplayName: Wheel's X position offset
50 : // @Description: X position of the center of the wheel in body frame. Positive X is forward of the origin.
51 : // @Units: m
52 : // @Range: -5 5
53 : // @Increment: 0.01
54 : // @User: Standard
55 :
56 : // @Param: _POS_Y
57 : // @DisplayName: Wheel's Y position offset
58 : // @Description: Y position of the center of the wheel in body frame. Positive Y is to the right of the origin.
59 : // @Units: m
60 : // @Range: -5 5
61 : // @Increment: 0.01
62 : // @User: Standard
63 :
64 : // @Param: _POS_Z
65 : // @DisplayName: Wheel's Z position offset
66 : // @Description: Z position of the center of the wheel in body frame. Positive Z is down from the origin.
67 : // @Units: m
68 : // @Range: -5 5
69 : // @Increment: 0.01
70 : // @User: Standard
71 : AP_GROUPINFO("_POS", 3, AP_WheelEncoder, _pos_offset[0], 0.0f),
72 :
73 : // @Param: _PINA
74 : // @DisplayName: Input Pin A
75 : // @Description: Input Pin A
76 : // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
77 : // @Range: -1 127
78 : // @User: Standard
79 : AP_GROUPINFO("_PINA", 4, AP_WheelEncoder, _pina[0], -1),
80 :
81 : // @Param: _PINB
82 : // @DisplayName: Input Pin B
83 : // @Description: Input Pin B
84 : // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
85 : // @Range: -1 127
86 : // @User: Standard
87 : AP_GROUPINFO("_PINB", 5, AP_WheelEncoder, _pinb[0], -1),
88 :
89 : #if WHEELENCODER_MAX_INSTANCES > 1
90 : // @Param: 2_TYPE
91 : // @DisplayName: Second WheelEncoder type
92 : // @Description: What type of WheelEncoder sensor is connected
93 : // @Values: 0:None,1:Quadrature,10:SITL Quadrature
94 : // @RebootRequired: True
95 : // @User: Standard
96 : AP_GROUPINFO("2_TYPE", 6, AP_WheelEncoder, _type[1], 0),
97 :
98 : // @Param: 2_CPR
99 : // @DisplayName: WheelEncoder 2 counts per revolution
100 : // @Description: WheelEncoder 2 counts per full revolution of the wheel
101 : // @Increment: 1
102 : // @User: Standard
103 : AP_GROUPINFO("2_CPR", 7, AP_WheelEncoder, _counts_per_revolution[1], WHEELENCODER_CPR_DEFAULT),
104 :
105 : // @Param: 2_RADIUS
106 : // @DisplayName: Wheel2's radius
107 : // @Description: Wheel2's radius
108 : // @Units: m
109 : // @Increment: 0.001
110 : // @User: Standard
111 : AP_GROUPINFO("2_RADIUS", 8, AP_WheelEncoder, _wheel_radius[1], WHEELENCODER_RADIUS_DEFAULT),
112 :
113 : // @Param: 2_POS_X
114 : // @DisplayName: Wheel2's X position offset
115 : // @Description: X position of the center of the second wheel in body frame. Positive X is forward of the origin.
116 : // @Units: m
117 : // @Range: -5 5
118 : // @Increment: 0.01
119 : // @User: Standard
120 :
121 : // @Param: 2_POS_Y
122 : // @DisplayName: Wheel2's Y position offset
123 : // @Description: Y position of the center of the second wheel in body frame. Positive Y is to the right of the origin.
124 : // @Units: m
125 : // @Range: -5 5
126 : // @Increment: 0.01
127 : // @User: Standard
128 :
129 : // @Param: 2_POS_Z
130 : // @DisplayName: Wheel2's Z position offset
131 : // @Description: Z position of the center of the second wheel in body frame. Positive Z is down from the origin.
132 : // @Units: m
133 : // @Range: -5 5
134 : // @Increment: 0.01
135 : // @User: Standard
136 : AP_GROUPINFO("2_POS", 9, AP_WheelEncoder, _pos_offset[1], 0.0f),
137 :
138 : // @Param: 2_PINA
139 : // @DisplayName: Second Encoder Input Pin A
140 : // @Description: Second Encoder Input Pin A
141 : // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
142 : // @Range: -1 127
143 : // @User: Standard
144 : AP_GROUPINFO("2_PINA", 10, AP_WheelEncoder, _pina[1], 53),
145 :
146 : // @Param: 2_PINB
147 : // @DisplayName: Second Encoder Input Pin B
148 : // @Description: Second Encoder Input Pin B
149 : // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
150 : // @Range: -1 127
151 : // @User: Standard
152 : AP_GROUPINFO("2_PINB", 11, AP_WheelEncoder, _pinb[1], 52),
153 : #endif
154 :
155 : AP_GROUPEND
156 : };
157 :
158 3705 : AP_WheelEncoder::AP_WheelEncoder(void)
159 : {
160 285 : _singleton = this;
161 :
162 285 : AP_Param::setup_object_defaults(this, var_info);
163 285 : }
164 :
165 : // initialise the AP_WheelEncoder class.
166 283 : void AP_WheelEncoder::init(void)
167 : {
168 283 : if (num_instances != 0) {
169 : // init called a 2nd time?
170 0 : return;
171 : }
172 849 : for (uint8_t i=0; i<WHEELENCODER_MAX_INSTANCES; i++) {
173 566 : switch ((WheelEncoder_Type)_type[i].get()) {
174 :
175 0 : case WheelEncoder_TYPE_QUADRATURE:
176 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
177 : drivers[i] = NEW_NOTHROW AP_WheelEncoder_Quadrature(*this, i, state[i]);
178 : #endif
179 0 : break;
180 :
181 6 : case WheelEncoder_TYPE_SITL_QUADRATURE:
182 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
183 6 : drivers[i] = NEW_NOTHROW AP_WheelEncoder_SITL_Quadrature(*this, i, state[i]);
184 : #endif
185 6 : break;
186 :
187 560 : case WheelEncoder_TYPE_NONE:
188 560 : break;
189 : }
190 :
191 :
192 566 : if (drivers[i] != nullptr) {
193 : // we loaded a driver for this instance, so it must be
194 : // present (although it may not be healthy)
195 6 : num_instances = i+1; // num_instances is a high-water-mark
196 : }
197 : }
198 : }
199 :
200 : // update WheelEncoder state for all instances. This should be called by main loop
201 4357 : void AP_WheelEncoder::update(void)
202 : {
203 10068 : for (uint8_t i=0; i<num_instances; i++) {
204 5711 : if (drivers[i] != nullptr && _type[i] != WheelEncoder_TYPE_NONE) {
205 5535 : drivers[i]->update();
206 : }
207 : }
208 4357 : }
209 :
210 : #if HAL_LOGGING_ENABLED
211 : // log wheel encoder information
212 119840 : void AP_WheelEncoder::Log_Write() const
213 : {
214 : // return immediately if no wheel encoders are enabled
215 119840 : if (!enabled(0) && !enabled(1)) {
216 118994 : return;
217 : }
218 :
219 846 : struct log_WheelEncoder pkt = {
220 : LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG),
221 846 : time_us : AP_HAL::micros64(),
222 846 : distance_0 : get_distance(0),
223 1692 : quality_0 : (uint8_t)get_signal_quality(0),
224 846 : distance_1 : get_distance(1),
225 1692 : quality_1 : (uint8_t)get_signal_quality(1),
226 846 : };
227 846 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
228 : }
229 : #endif
230 :
231 : // check if an instance is healthy
232 0 : bool AP_WheelEncoder::healthy(uint8_t instance) const
233 : {
234 0 : if (instance >= num_instances) {
235 0 : return false;
236 : }
237 0 : return true;
238 : }
239 :
240 : // check if an instance is activated
241 238834 : bool AP_WheelEncoder::enabled(uint8_t instance) const
242 : {
243 238834 : if (instance >= num_instances) {
244 237952 : return false;
245 : }
246 : // if no sensor type is selected, the sensor is not activated.
247 882 : if (_type[instance] == WheelEncoder_TYPE_NONE) {
248 36 : return false;
249 : }
250 846 : return true;
251 : }
252 :
253 : // get the counts per revolution of the encoder
254 5535 : uint16_t AP_WheelEncoder::get_counts_per_revolution(uint8_t instance) const
255 : {
256 : // for invalid instances return zero vector
257 5535 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
258 0 : return 0;
259 : }
260 5535 : return (uint16_t)_counts_per_revolution[instance];
261 : }
262 :
263 : // get the wheel radius in meters
264 9888 : float AP_WheelEncoder::get_wheel_radius(uint8_t instance) const
265 : {
266 : // for invalid instances return zero vector
267 9888 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
268 0 : return 0.0f;
269 : }
270 9888 : return _wheel_radius[instance];
271 : }
272 :
273 : // return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin
274 15423 : const Vector3f &AP_WheelEncoder::get_pos_offset(uint8_t instance) const
275 : {
276 : // for invalid instances return zero vector
277 15423 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
278 0 : return pos_offset_zero;
279 : }
280 15423 : return _pos_offset[instance];
281 : }
282 :
283 : // get total delta angle (in radians) measured by the wheel encoder
284 11762 : float AP_WheelEncoder::get_delta_angle(uint8_t instance) const
285 : {
286 : // for invalid instances return zero
287 11762 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
288 0 : return 0.0f;
289 : }
290 : // protect against divide by zero
291 11762 : if (_counts_per_revolution[instance] == 0) {
292 0 : return 0.0f;
293 : }
294 11762 : return M_2PI * state[instance].distance_count / _counts_per_revolution[instance];
295 : }
296 :
297 : // get the total distance traveled in meters
298 7403 : float AP_WheelEncoder::get_distance(uint8_t instance) const
299 : {
300 : // for invalid instances return zero
301 7403 : return get_delta_angle(instance) * _wheel_radius[instance];
302 : }
303 :
304 : // get the instantaneous rate in radians/second
305 0 : float AP_WheelEncoder::get_rate(uint8_t instance) const
306 : {
307 : // for invalid instances return zero
308 0 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
309 0 : return 0.0f;
310 : }
311 :
312 : // protect against divide by zero
313 0 : if ((state[instance].dt_ms == 0) || _counts_per_revolution[instance] == 0) {
314 0 : return 0;
315 : }
316 :
317 : // calculate delta_angle (in radians) per second
318 0 : return M_2PI * (state[instance].dist_count_change / ((float)_counts_per_revolution[instance])) / (state[instance].dt_ms * 1e-3f);
319 : }
320 :
321 : // get the total number of sensor reading from the encoder
322 0 : uint32_t AP_WheelEncoder::get_total_count(uint8_t instance) const
323 : {
324 : // for invalid instances return zero
325 0 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
326 0 : return 0;
327 : }
328 0 : return state[instance].total_count;
329 : }
330 :
331 : // get the total distance traveled in meters
332 0 : uint32_t AP_WheelEncoder::get_error_count(uint8_t instance) const
333 : {
334 : // for invalid instances return zero
335 0 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
336 0 : return 0;
337 : }
338 0 : return state[instance].error_count;
339 : }
340 :
341 : // get the signal quality for a sensor
342 1692 : float AP_WheelEncoder::get_signal_quality(uint8_t instance) const
343 : {
344 : // protect against divide by zero
345 1692 : if (state[instance].total_count == 0) {
346 1325 : return 0.0f;
347 : }
348 367 : return constrain_float((1.0f - ((float)state[instance].error_count / (float)state[instance].total_count)) * 100.0f, 0.0f, 100.0f);
349 : }
350 :
351 : // get the system time (in milliseconds) of the last update
352 4359 : uint32_t AP_WheelEncoder::get_last_reading_ms(uint8_t instance) const
353 : {
354 : // for invalid instances return zero
355 4359 : if (instance >= WHEELENCODER_MAX_INSTANCES) {
356 0 : return 0;
357 : }
358 4359 : return state[instance].last_reading_ms;
359 : }
360 :
361 : // singleton instance
362 : AP_WheelEncoder *AP_WheelEncoder::_singleton;
363 :
364 : namespace AP {
365 :
366 41770910 : AP_WheelEncoder *wheelencoder()
367 : {
368 41770910 : return AP_WheelEncoder::get_singleton();
369 : }
370 :
371 : }
|
