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 : #pragma once
16 :
17 : #include "AP_RangeFinder_config.h"
18 :
19 : #if AP_RANGEFINDER_ENABLED
20 :
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_HAL/AP_HAL_Boards.h>
23 : #include <AP_HAL/Semaphores.h>
24 : #include <AP_Param/AP_Param.h>
25 : #include <GCS_MAVLink/GCS_MAVLink.h>
26 : #include <AP_MSP/msp.h>
27 : #include "AP_RangeFinder_Params.h"
28 : #include <AP_TemperatureSensor/AP_TemperatureSensor_config.h>
29 :
30 : // Maximum number of range finder instances available on this platform
31 : #ifndef RANGEFINDER_MAX_INSTANCES
32 : #if AP_RANGEFINDER_ENABLED
33 : #define RANGEFINDER_MAX_INSTANCES 10
34 : #else
35 : #define RANGEFINDER_MAX_INSTANCES 1
36 : #endif
37 : #endif
38 :
39 : #define RANGEFINDER_GROUND_CLEARANCE_DEFAULT 0.10
40 : #define RANGEFINDER_PREARM_ALT_MAX_CM 200
41 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
42 : #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 0
43 : #else
44 : #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 50
45 : #endif
46 :
47 : class AP_RangeFinder_Backend;
48 :
49 : class RangeFinder
50 : {
51 : friend class AP_RangeFinder_Backend;
52 : //UAVCAN drivers are initialised in the Backend, hence list of drivers is needed there.
53 : friend class AP_RangeFinder_DroneCAN;
54 : public:
55 : RangeFinder();
56 :
57 : /* Do not allow copies */
58 : CLASS_NO_COPY(RangeFinder);
59 :
60 : // RangeFinder driver types
61 : enum class Type {
62 : NONE = 0,
63 : #if AP_RANGEFINDER_ANALOG_ENABLED
64 : ANALOG = 1,
65 : #endif
66 : #if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED
67 : MBI2C = 2,
68 : #endif
69 : #if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
70 : PLI2C = 3,
71 : #endif
72 : // PX4 = 4, // no longer used, but may be in some user's parameters
73 : #if AP_RANGEFINDER_PWM_ENABLED
74 : PX4_PWM= 5,
75 : #endif
76 : #if AP_RANGEFINDER_BBB_PRU_ENABLED
77 : BBB_PRU= 6,
78 : #endif
79 : #if AP_RANGEFINDER_LWI2C_ENABLED
80 : LWI2C = 7,
81 : #endif
82 : #if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED
83 : LWSER = 8,
84 : #endif
85 : #if AP_RANGEFINDER_BEBOP_ENABLED
86 : BEBOP = 9,
87 : #endif
88 : #if AP_RANGEFINDER_MAVLINK_ENABLED
89 : MAVLink = 10,
90 : #endif
91 : #if AP_RANGEFINDER_USD1_SERIAL_ENABLED
92 : USD1_Serial = 11,
93 : #endif
94 : #if AP_RANGEFINDER_LEDDARONE_ENABLED
95 : LEDDARONE = 12,
96 : #endif
97 : #if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED
98 : MBSER = 13,
99 : #endif
100 : #if AP_RANGEFINDER_TRI2C_ENABLED
101 : TRI2C = 14,
102 : #endif
103 : #if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
104 : PLI2CV3= 15,
105 : #endif
106 : VL53L0X = 16,
107 : #if AP_RANGEFINDER_NMEA_ENABLED
108 : NMEA = 17,
109 : #endif
110 : #if AP_RANGEFINDER_WASP_ENABLED
111 : WASP = 18,
112 : #endif
113 : #if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED
114 : BenewakeTF02 = 19,
115 : #endif
116 : #if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED
117 : BenewakeTFmini = 20,
118 : #endif
119 : #if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
120 : PLI2CV3HP = 21,
121 : #endif
122 : #if AP_RANGEFINDER_PWM_ENABLED
123 : PWM = 22,
124 : #endif
125 : #if AP_RANGEFINDER_BLPING_ENABLED
126 : BLPing = 23,
127 : #endif
128 : #if AP_RANGEFINDER_DRONECAN_ENABLED
129 : UAVCAN = 24,
130 : #endif
131 : #if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED
132 : BenewakeTFminiPlus = 25,
133 : #endif
134 : #if AP_RANGEFINDER_LANBAO_ENABLED
135 : Lanbao = 26,
136 : #endif
137 : #if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED
138 : BenewakeTF03 = 27,
139 : #endif
140 : VL53L1X_Short = 28,
141 : #if AP_RANGEFINDER_LEDDARVU8_ENABLED
142 : LeddarVu8_Serial = 29,
143 : #endif
144 : #if AP_RANGEFINDER_HC_SR04_ENABLED
145 : HC_SR04 = 30,
146 : #endif
147 : #if AP_RANGEFINDER_GYUS42V2_ENABLED
148 : GYUS42v2 = 31,
149 : #endif
150 : #if HAL_MSP_RANGEFINDER_ENABLED
151 : MSP = 32,
152 : #endif
153 : #if AP_RANGEFINDER_USD1_CAN_ENABLED
154 : USD1_CAN = 33,
155 : #endif
156 : #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
157 : Benewake_CAN = 34,
158 : #endif
159 : #if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
160 : TeraRanger_Serial = 35,
161 : #endif
162 : #if AP_RANGEFINDER_LUA_ENABLED
163 : Lua_Scripting = 36,
164 : #endif
165 : #if AP_RANGEFINDER_NOOPLOOP_ENABLED
166 : NoopLoop_P = 37,
167 : #endif
168 : #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
169 : TOFSenseP_CAN = 38,
170 : #endif
171 : #if AP_RANGEFINDER_NRA24_CAN_ENABLED
172 : NRA24_CAN = 39,
173 : #endif
174 : #if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
175 : TOFSenseF_I2C = 40,
176 : #endif
177 : #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
178 : JRE_Serial = 41,
179 : #endif
180 : #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
181 : Ainstein_LR_D1 = 42,
182 : #endif
183 : #if AP_RANGEFINDER_RDS02UF_ENABLED
184 : RDS02UF = 43,
185 : #endif
186 : #if AP_RANGEFINDER_HEXSOONRADAR_ENABLED
187 : HEXSOON_RADAR = 44,
188 : #endif
189 : #if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
190 : LightWare_GRF = 45,
191 : #endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
192 : #if AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
193 : BenewakeTFS20L = 46,
194 : #endif // AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
195 : #if AP_RANGEFINDER_DTS6012M_ENABLED
196 : DTS6012M = 47,
197 : #endif // AP_RANGEFINDER_DTS6012M_ENABLED
198 : #if AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
199 : LightWare_GRF_I2C = 48,
200 : #endif // AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
201 : #if AP_RANGEFINDER_SIM_ENABLED
202 : SIM = 100,
203 : #endif
204 : };
205 :
206 : enum class Function {
207 : LINEAR = 0,
208 : INVERTED = 1,
209 : HYPERBOLA = 2
210 : };
211 :
212 : enum class Status {
213 : NotConnected = 0,
214 : NoData = 1,
215 : OutOfRangeLow = 2,
216 : OutOfRangeHigh = 3,
217 : Good = 4,
218 : PoweredDown = 5,
219 : };
220 :
221 : static constexpr int8_t SIGNAL_QUALITY_MIN = 0;
222 : static constexpr int8_t SIGNAL_QUALITY_MAX = 100;
223 : static constexpr int8_t SIGNAL_QUALITY_UNKNOWN = -1;
224 :
225 : // The RangeFinder_State structure is filled in by the backend driver
226 : struct RangeFinder_State {
227 : float distance_m; // distance in meters
228 : int8_t signal_quality_pct; // measurement quality in percent 0-100, -1 -> quality is unknown
229 : uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
230 : enum RangeFinder::Status status; // sensor status
231 : uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10)
232 : uint32_t last_reading_ms; // system time of last successful update from sensor
233 :
234 : #if AP_TEMPERATURE_SENSOR_ENABLED
235 : float temperature_C; // externally-supplied fluid temperature (e.g. air, water) for speed-of-sound compensation, not the rangefinder sensors own temperature, e.g. from AP_TemperatureSensor (TEMPx_SRC=Rangefinder)
236 : bool temperature_valid; // true if a valid temperature has been set by an external source
237 : uint32_t temperature_update_ms; // system time of last external temperature update
238 : #endif
239 :
240 : const struct AP_Param::GroupInfo *var_info;
241 : };
242 :
243 : static const struct AP_Param::GroupInfo *backend_var_info[RANGEFINDER_MAX_INSTANCES];
244 :
245 : // parameters for each instance
246 : static const struct AP_Param::GroupInfo var_info[];
247 :
248 2057 : void set_log_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; }
249 :
250 : /*
251 : Return the number of range finder instances. Note that if users
252 : sets up rangefinders with a gap in the types then this is the
253 : index of the maximum sensor ID plus one, so this gives the value
254 : that should be used when iterating over all sensors
255 : */
256 175732 : uint8_t num_sensors(void) const {
257 175732 : return num_instances;
258 : }
259 :
260 : // prearm checks
261 : bool prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const;
262 :
263 : // detect and initialise any available rangefinders
264 : void init(enum Rotation orientation_default);
265 :
266 : // update state of all rangefinders. Should be called at around
267 : // 10Hz from main loop
268 : void update(void);
269 :
270 : // Handle an incoming DISTANCE_SENSOR message (from a MAVLink enabled range finder)
271 : void handle_msg(const mavlink_message_t &msg);
272 :
273 : #if HAL_MSP_RANGEFINDER_ENABLED
274 : // Handle an incoming DISTANCE_SENSOR message (from a MSP enabled range finder)
275 : void handle_msp(const MSP::msp_rangefinder_data_message_t &pkt);
276 : #endif
277 : // return true if we have a range finder with the specified orientation
278 : bool has_orientation(enum Rotation orientation) const;
279 :
280 : // find first range finder instance with the specified orientation
281 : AP_RangeFinder_Backend *find_instance(enum Rotation orientation) const;
282 :
283 : AP_RangeFinder_Backend *get_backend(uint8_t id) const;
284 :
285 : // get rangefinder type for an ID
286 : Type get_type(uint8_t id) const {
287 : return id >= RANGEFINDER_MAX_INSTANCES? Type::NONE : Type(params[id].type.get());
288 : }
289 :
290 : // get rangefinder address (for AP_Periph CAN)
291 : uint8_t get_address(uint8_t id) const {
292 : return id >= RANGEFINDER_MAX_INSTANCES? 0 : uint8_t(params[id].address.get());
293 : }
294 :
295 : // methods to return a distance on a particular orientation from
296 : // any sensor which can current supply it
297 : int8_t signal_quality_pct_orient(enum Rotation orientation) const;
298 : #if AP_SCRIPTING_ENABLED
299 : // centimetre accessors - do not use, reduce use where possible
300 0 : uint16_t distance_cm_orient(enum Rotation orientation) const {
301 0 : return distance_orient(orientation) * 100.0;
302 : }
303 0 : int32_t max_distance_cm_orient(enum Rotation orientation) const {
304 0 : return max_distance_orient(orientation) * 100;
305 : }
306 0 : int32_t min_distance_cm_orient(enum Rotation orientation) const {
307 0 : return min_distance_orient(orientation) * 100;
308 : }
309 0 : int32_t ground_clearance_cm_orient(enum Rotation orientation) const {
310 0 : return ground_clearance_orient(orientation) * 100;
311 : }
312 : #endif
313 : // metre accessors - use these in preference to the cm accessors
314 : float distance_orient(enum Rotation orientation) const;
315 : float max_distance_orient(enum Rotation orientation) const;
316 : float min_distance_orient(enum Rotation orientation) const;
317 : float ground_clearance_orient(enum Rotation orientation) const;
318 : MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const;
319 : RangeFinder::Status status_orient(enum Rotation orientation) const;
320 : bool has_data_orient(enum Rotation orientation) const;
321 : uint8_t range_valid_count_orient(enum Rotation orientation) const;
322 : const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
323 : uint32_t last_reading_ms(enum Rotation orientation) const;
324 :
325 : // get temperature reading in C. returns true on success and populates temp argument
326 : bool get_temp(enum Rotation orientation, float &temp) const;
327 :
328 : #if AP_TEMPERATURE_SENSOR_ENABLED
329 : // set an externally-measured temperature (C) for a rangefinder instance.
330 : // used by AP_TemperatureSensor when TEMPx_SRC is set to Rangefinder, for
331 : // sensors (e.g. ultrasonic) whose readings can benefit from external
332 : // temperature compensation but which have no onboard temperature sensor.
333 : void set_temperature_C(uint8_t instance, float temperature_C);
334 : #endif
335 :
336 : /*
337 : set an externally estimated terrain height. Used to enable power
338 : saving (where available) at high altitudes.
339 : */
340 3362760 : void set_estimated_terrain_height(float height) {
341 3362760 : estimated_terrain_height = height;
342 3362760 : }
343 :
344 11991526 : static RangeFinder *get_singleton(void) { return _singleton; }
345 :
346 : protected:
347 : AP_RangeFinder_Params params[RANGEFINDER_MAX_INSTANCES];
348 :
349 : private:
350 : static RangeFinder *_singleton;
351 :
352 : RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
353 : AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
354 : uint8_t num_instances;
355 : HAL_Semaphore detect_sem;
356 : float estimated_terrain_height;
357 : Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests
358 :
359 : void convert_params(void);
360 :
361 : void detect_instance(uint8_t instance, uint8_t& serial_instance);
362 :
363 : bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance, uint8_t serial_instance=0);
364 :
365 : // search all i2c buses for a device at addr
366 : using i2c_probe_fn_t = AP_RangeFinder_Backend* (*)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&, AP_HAL::I2CDevice&);
367 : void probe_i2c_buses(uint8_t instance, uint8_t addr, i2c_probe_fn_t detect_fn);
368 :
369 : uint32_t _log_rfnd_bit = -1;
370 : void Log_RFND() const;
371 : };
372 :
373 : namespace AP {
374 : RangeFinder *rangefinder();
375 : };
376 :
377 : #endif // AP_RANGEFINDER_ENABLED
|
