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_Beacon.h"
17 :
18 : #if AP_BEACON_ENABLED
19 :
20 : #include "AP_Beacon_Backend.h"
21 : #include "AP_Beacon_Pozyx.h"
22 : #include "AP_Beacon_Marvelmind.h"
23 : #include "AP_Beacon_Nooploop.h"
24 : #include "AP_Beacon_SITL.h"
25 :
26 : #include <AP_Common/Location.h>
27 : #include <AP_Logger/AP_Logger.h>
28 :
29 : extern const AP_HAL::HAL &hal;
30 :
31 : #ifndef AP_BEACON_MINIMUM_FENCE_BEACONS
32 : #define AP_BEACON_MINIMUM_FENCE_BEACONS 3
33 : #endif
34 :
35 : // table of user settable parameters
36 : const AP_Param::GroupInfo AP_Beacon::var_info[] = {
37 :
38 : // @Param: _TYPE
39 : // @DisplayName: Beacon based position estimation device type
40 : // @Description: What type of beacon based position estimation device is connected
41 : // @Values: 0:None,1:Pozyx,2:Marvelmind,3:Nooploop,10:SITL
42 : // @User: Advanced
43 : AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Beacon, _type, 0, AP_PARAM_FLAG_ENABLE),
44 :
45 : // @Param: _LATITUDE
46 : // @DisplayName: Beacon origin's latitude
47 : // @Description: Beacon origin's latitude
48 : // @Units: deg
49 : // @Increment: 0.000001
50 : // @Range: -90 90
51 : // @User: Advanced
52 : AP_GROUPINFO("_LATITUDE", 1, AP_Beacon, origin_lat, 0),
53 :
54 : // @Param: _LONGITUDE
55 : // @DisplayName: Beacon origin's longitude
56 : // @Description: Beacon origin's longitude
57 : // @Units: deg
58 : // @Increment: 0.000001
59 : // @Range: -180 180
60 : // @User: Advanced
61 : AP_GROUPINFO("_LONGITUDE", 2, AP_Beacon, origin_lon, 0),
62 :
63 : // @Param: _ALT
64 : // @DisplayName: Beacon origin's altitude above sealevel in meters
65 : // @Description: Beacon origin's altitude above sealevel in meters
66 : // @Units: m
67 : // @Increment: 1
68 : // @Range: 0 10000
69 : // @User: Advanced
70 : AP_GROUPINFO("_ALT", 3, AP_Beacon, origin_alt, 0),
71 :
72 : // @Param: _ORIENT_YAW
73 : // @DisplayName: Beacon systems rotation from north in degrees
74 : // @Description: Beacon systems rotation from north in degrees
75 : // @Units: deg
76 : // @Increment: 1
77 : // @Range: -180 +180
78 : // @User: Advanced
79 : AP_GROUPINFO("_ORIENT_YAW", 4, AP_Beacon, orient_yaw, 0),
80 :
81 : AP_GROUPEND
82 : };
83 :
84 10490 : AP_Beacon::AP_Beacon()
85 : {
86 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
87 2096 : if (_singleton != nullptr) {
88 0 : AP_HAL::panic("Fence must be singleton");
89 : }
90 : #endif
91 2098 : _singleton = this;
92 2098 : AP_Param::setup_object_defaults(this, var_info);
93 2098 : }
94 :
95 : // initialise the AP_Beacon class
96 2094 : void AP_Beacon::init(void)
97 : {
98 2094 : if (_driver != nullptr) {
99 : // init called a 2nd time?
100 0 : return;
101 : }
102 :
103 : // create backend
104 2094 : switch ((Type)_type) {
105 0 : case Type::Pozyx:
106 0 : _driver = NEW_NOTHROW AP_Beacon_Pozyx(*this);
107 0 : break;
108 0 : case Type::Marvelmind:
109 0 : _driver = NEW_NOTHROW AP_Beacon_Marvelmind(*this);
110 0 : break;
111 1 : case Type::Nooploop:
112 1 : _driver = NEW_NOTHROW AP_Beacon_Nooploop(*this);
113 1 : break;
114 : #if AP_BEACON_SITL_ENABLED
115 4 : case Type::SITL:
116 4 : _driver = NEW_NOTHROW AP_Beacon_SITL(*this);
117 4 : break;
118 : #endif
119 2089 : case Type::None:
120 2089 : break;
121 : }
122 : }
123 :
124 : // return true if beacon feature is enabled
125 1003925 : bool AP_Beacon::enabled(void) const
126 : {
127 1003925 : return (_type != Type::None);
128 : }
129 :
130 : // return true if sensor is basically healthy (we are receiving data)
131 4604 : bool AP_Beacon::healthy(void) const
132 : {
133 4604 : if (!device_ready()) {
134 32 : return false;
135 : }
136 4572 : return _driver->healthy();
137 : }
138 :
139 : // update state. This should be called often from the main loop
140 41686253 : void AP_Beacon::update(void)
141 : {
142 41686253 : if (!device_ready()) {
143 41531590 : return;
144 : }
145 154663 : _driver->update();
146 :
147 : // update boundary for fence
148 154663 : update_boundary_points();
149 : }
150 :
151 : // return origin of position estimate system
152 153757 : bool AP_Beacon::get_origin(Location &origin_loc) const
153 : {
154 153757 : if (!device_ready()) {
155 749 : return false;
156 : }
157 :
158 : // check for un-initialised origin
159 153008 : if (is_zero(origin_lat) && is_zero(origin_lon) && is_zero(origin_alt)) {
160 77 : return false;
161 : }
162 :
163 : // return origin
164 611724 : origin_loc = Location{
165 152931 : int32_t(origin_lat * 1.0e7f),
166 152931 : int32_t(origin_lon * 1.0e7f),
167 152931 : int32_t(origin_alt * 100),
168 : Location::AltFrame::ABSOLUTE
169 : };
170 :
171 152931 : return true;
172 : }
173 :
174 : // return position in NED from position estimate system's origin in meters
175 158361 : bool AP_Beacon::get_vehicle_position_ned(Vector3f &position, float& accuracy_estimate) const
176 : {
177 158361 : if (!device_ready()) {
178 781 : return false;
179 : }
180 :
181 : // check for timeout
182 157580 : if (AP_HAL::millis() - veh_pos_update_ms > AP_BEACON_TIMEOUT_MS) {
183 0 : return false;
184 : }
185 :
186 : // return position
187 157580 : position = veh_pos_ned;
188 157580 : accuracy_estimate = veh_pos_accuracy;
189 157580 : return true;
190 : }
191 :
192 : // return the number of beacons
193 4609 : uint8_t AP_Beacon::count() const
194 : {
195 4609 : if (!device_ready()) {
196 32 : return 0;
197 : }
198 4577 : return num_beacons;
199 : }
200 :
201 : // return all beacon data
202 0 : bool AP_Beacon::get_beacon_data(uint8_t beacon_instance, struct BeaconState& state) const
203 : {
204 0 : if (!device_ready() || beacon_instance >= num_beacons) {
205 0 : return false;
206 : }
207 0 : state = beacon_state[beacon_instance];
208 0 : return true;
209 : }
210 :
211 : // return individual beacon's id
212 0 : uint8_t AP_Beacon::beacon_id(uint8_t beacon_instance) const
213 : {
214 0 : if (beacon_instance >= num_beacons) {
215 0 : return 0;
216 : }
217 0 : return beacon_state[beacon_instance].id;
218 : }
219 :
220 : // return beacon health
221 615028 : bool AP_Beacon::beacon_healthy(uint8_t beacon_instance) const
222 : {
223 615028 : if (beacon_instance >= num_beacons) {
224 0 : return false;
225 : }
226 615028 : return beacon_state[beacon_instance].healthy;
227 : }
228 :
229 : // return distance to beacon in meters
230 633444 : float AP_Beacon::beacon_distance(uint8_t beacon_instance) const
231 : {
232 633444 : if ( beacon_instance >= num_beacons || !beacon_state[beacon_instance].healthy) {
233 128 : return 0.0f;
234 : }
235 633316 : return beacon_state[beacon_instance].distance;
236 : }
237 :
238 : // return beacon position in meters
239 615060 : Vector3f AP_Beacon::beacon_position(uint8_t beacon_instance) const
240 : {
241 615060 : if (!device_ready() || beacon_instance >= num_beacons) {
242 2996 : Vector3f temp = {};
243 2996 : return temp;
244 : }
245 612064 : return beacon_state[beacon_instance].position;
246 : }
247 :
248 : // return last update time from beacon in milliseconds
249 615028 : uint32_t AP_Beacon::beacon_last_update_ms(uint8_t beacon_instance) const
250 : {
251 615028 : if (_type == Type::None || beacon_instance >= num_beacons) {
252 2996 : return 0;
253 : }
254 612032 : return beacon_state[beacon_instance].distance_update_ms;
255 : }
256 :
257 : // create fence boundary points
258 154663 : void AP_Beacon::update_boundary_points()
259 : {
260 : // we need three beacons at least to create boundary fence.
261 : // update boundary fence if number of beacons changes
262 154663 : if (!device_ready() || num_beacons < AP_BEACON_MINIMUM_FENCE_BEACONS || boundary_num_beacons == num_beacons) {
263 154654 : return;
264 : }
265 :
266 : // record number of beacons so we do not repeat calculations
267 9 : boundary_num_beacons = num_beacons;
268 :
269 : // accumulate beacon points
270 9 : Vector2f beacon_points[AP_BEACON_MAX_BEACONS];
271 41 : for (uint8_t index = 0; index < num_beacons; index++) {
272 32 : const Vector3f& point_3d = beacon_position(index);
273 32 : beacon_points[index].x = point_3d.x;
274 32 : beacon_points[index].y = point_3d.y;
275 : }
276 :
277 : // create polygon around boundary points using the following algorithm
278 : // set the "current point" as the first boundary point
279 : // loop through all the boundary points looking for the point which creates a vector (from the current point to this new point) with the lowest angle
280 : // check if point is already in boundary
281 : // - no: add to boundary, move current point to this new point and repeat the above
282 : // - yes: we've completed the bounding box, delete any boundary points found earlier than the duplicate
283 :
284 9 : Vector2f boundary_points[AP_BEACON_MAX_BEACONS+1]; // array of boundary points
285 9 : uint8_t curr_boundary_idx = 0; // index into boundary_sorted index. always points to the highest filled in element of the array
286 9 : uint8_t curr_beacon_idx = 0; // index into beacon_point array. point indexed is same point as curr_boundary_idx's
287 :
288 : // initialise first point of boundary_sorted with first beacon's position (this point may be removed later if it is found to not be on the outer boundary)
289 9 : boundary_points[curr_boundary_idx] = beacon_points[curr_beacon_idx];
290 :
291 9 : bool boundary_success = false; // true once the boundary has been successfully found
292 9 : bool boundary_failure = false; // true if we fail to build the boundary
293 9 : float start_angle = 0.0f; // starting angle used when searching for next boundary point, on each iteration this climbs but never climbs past PI * 2
294 41 : while (!boundary_success && !boundary_failure) {
295 : // look for next outer point
296 : uint8_t next_idx;
297 : float next_angle;
298 32 : if (get_next_boundary_point(beacon_points, num_beacons, curr_beacon_idx, start_angle, next_idx, next_angle)) {
299 : // add boundary point to boundary_sorted array
300 32 : curr_boundary_idx++;
301 32 : boundary_points[curr_boundary_idx] = beacon_points[next_idx];
302 32 : curr_beacon_idx = next_idx;
303 32 : start_angle = next_angle;
304 :
305 : // check if we have a complete boundary by looking for duplicate points within the boundary_sorted
306 32 : uint8_t dup_idx = 0;
307 32 : bool dup_found = false;
308 83 : while (dup_idx < curr_boundary_idx && !dup_found) {
309 51 : dup_found = (boundary_points[dup_idx] == boundary_points[curr_boundary_idx]);
310 51 : if (!dup_found) {
311 42 : dup_idx++;
312 : }
313 : }
314 : // if duplicate is found, remove all boundary points before the duplicate because they are inner points
315 32 : if (dup_found) {
316 : // note that the closing/duplicate point is not
317 : // included in the boundary points.
318 9 : const uint8_t num_pts = curr_boundary_idx - dup_idx;
319 9 : if (num_pts >= AP_BEACON_MINIMUM_FENCE_BEACONS) { // we consider three points to be a polygon
320 : // success, copy boundary points to boundary array and convert meters to cm
321 41 : for (uint8_t j = 0; j < num_pts; j++) {
322 32 : boundary[j] = boundary_points[j+dup_idx] * 100.0f;
323 : }
324 9 : boundary_num_points = num_pts;
325 9 : boundary_success = true;
326 : } else {
327 : // boundary has too few points
328 0 : boundary_failure = true;
329 : }
330 : }
331 : } else {
332 : // failed to create boundary - give up!
333 0 : boundary_failure = true;
334 : }
335 : }
336 :
337 : // clear boundary on failure
338 9 : if (boundary_failure) {
339 0 : boundary_num_points = 0;
340 : }
341 : }
342 :
343 : // find next boundary point from an array of boundary points given the current index into that array
344 : // returns true if a next point can be found
345 : // current_index should be an index into the boundary_pts array
346 : // start_angle is an angle (in radians), the search will sweep clockwise from this angle
347 : // the index of the next point is returned in the next_index argument
348 : // the angle to the next point is returned in the next_angle argument
349 32 : bool AP_Beacon::get_next_boundary_point(const Vector2f* boundary_pts, uint8_t num_points, uint8_t current_index, float start_angle, uint8_t& next_index, float& next_angle)
350 : {
351 : // sanity check
352 32 : if (boundary_pts == nullptr || current_index >= num_points) {
353 0 : return false;
354 : }
355 :
356 : // get current point
357 32 : Vector2f curr_point = boundary_pts[current_index];
358 :
359 : // search through all points for next boundary point in a clockwise direction
360 32 : float lowest_angle = M_PI_2;
361 32 : float lowest_angle_relative = M_PI_2;
362 32 : bool lowest_found = false;
363 32 : uint8_t lowest_index = 0;
364 148 : for (uint8_t i=0; i < num_points; i++) {
365 116 : if (i != current_index) {
366 84 : Vector2f vec = boundary_pts[i] - curr_point;
367 84 : if (!vec.is_zero()) {
368 84 : float angle = wrap_2PI(atan2f(vec.y, vec.x));
369 84 : float angle_relative = wrap_2PI(angle - start_angle);
370 84 : if ((angle_relative < lowest_angle_relative) || !lowest_found) {
371 46 : lowest_angle = angle;
372 46 : lowest_angle_relative = angle_relative;
373 46 : lowest_index = i;
374 46 : lowest_found = true;
375 : }
376 : }
377 : }
378 : }
379 :
380 : // return results
381 32 : if (lowest_found) {
382 32 : next_index = lowest_index;
383 32 : next_angle = lowest_angle;
384 : }
385 32 : return lowest_found;
386 : }
387 :
388 : // return fence boundary array
389 27793 : const Vector2f* AP_Beacon::get_boundary_points(uint16_t& num_points) const
390 : {
391 27793 : if (!device_ready()) {
392 0 : num_points = 0;
393 0 : return nullptr;
394 : }
395 :
396 27793 : num_points = boundary_num_points;
397 27793 : return boundary;
398 : }
399 :
400 : // check if the device is ready
401 42805100 : bool AP_Beacon::device_ready(void) const
402 : {
403 42805100 : return ((_driver != nullptr) && (_type != Type::None));
404 : }
405 :
406 : #if HAL_LOGGING_ENABLED
407 : // Write beacon sensor (position) data
408 848162 : void AP_Beacon::log()
409 : {
410 848162 : if (!enabled()) {
411 843558 : return;
412 : }
413 : // position
414 4604 : Vector3f pos;
415 4604 : float accuracy = 0.0f;
416 4604 : get_vehicle_position_ned(pos, accuracy);
417 :
418 4604 : const struct log_Beacon pkt_beacon{
419 : LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG),
420 4604 : time_us : AP_HAL::micros64(),
421 9208 : health : (uint8_t)healthy(),
422 4604 : count : (uint8_t)count(),
423 4604 : dist0 : beacon_distance(0),
424 4604 : dist1 : beacon_distance(1),
425 4604 : dist2 : beacon_distance(2),
426 4604 : dist3 : beacon_distance(3),
427 4604 : posx : pos.x,
428 4604 : posy : pos.y,
429 4604 : posz : pos.z
430 4604 : };
431 4604 : AP::logger().WriteBlock(&pkt_beacon, sizeof(pkt_beacon));
432 : }
433 : #endif
434 :
435 : // singleton instance
436 : AP_Beacon *AP_Beacon::_singleton;
437 :
438 : namespace AP {
439 :
440 967109 : AP_Beacon *beacon()
441 : {
442 967109 : return AP_Beacon::get_singleton();
443 : }
444 :
445 : }
446 :
447 : #endif
|
