APM:Libraries
AP_Beacon.cpp
Go to the documentation of this file.
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 #include "AP_Beacon_Backend.h"
18 #include "AP_Beacon_Pozyx.h"
19 #include "AP_Beacon_Marvelmind.h"
20 #include "AP_Beacon_SITL.h"
21 
22 extern const AP_HAL::HAL &hal;
23 
24 // table of user settable parameters
26 
27  // @Param: _TYPE
28  // @DisplayName: Beacon based position estimation device type
29  // @Description: What type of beacon based position estimation device is connected
30  // @Values: 0:None,1:Pozyx,2:Marvelmind
31  // @User: Advanced
32  AP_GROUPINFO("_TYPE", 0, AP_Beacon, _type, 0),
33 
34  // @Param: _LATITUDE
35  // @DisplayName: Beacon origin's latitude
36  // @Description: Beacon origin's latitude
37  // @Units: deg
38  // @Increment: 0.000001
39  // @Range: -90 90
40  // @User: Advanced
41  AP_GROUPINFO("_LATITUDE", 1, AP_Beacon, origin_lat, 0),
42 
43  // @Param: _LONGITUDE
44  // @DisplayName: Beacon origin's longitude
45  // @Description: Beacon origin's longitude
46  // @Units: deg
47  // @Increment: 0.000001
48  // @Range: -180 180
49  // @User: Advanced
50  AP_GROUPINFO("_LONGITUDE", 2, AP_Beacon, origin_lon, 0),
51 
52  // @Param: _ALT
53  // @DisplayName: Beacon origin's altitude above sealevel in meters
54  // @Description: Beacon origin's altitude above sealevel in meters
55  // @Units: m
56  // @Increment: 1
57  // @Range: 0 10000
58  // @User: Advanced
59  AP_GROUPINFO("_ALT", 3, AP_Beacon, origin_alt, 0),
60 
61  // @Param: _ORIENT_YAW
62  // @DisplayName: Beacon systems rotation from north in degrees
63  // @Description: Beacon systems rotation from north in degrees
64  // @Units: deg
65  // @Increment: 1
66  // @Range: -180 +180
67  // @User: Advanced
68  AP_GROUPINFO("_ORIENT_YAW", 4, AP_Beacon, orient_yaw, 0),
69 
71 };
72 
74  serial_manager(_serial_manager)
75 {
77 }
78 
79 // initialise the AP_Beacon class
80 void AP_Beacon::init(void)
81 {
82  if (_driver != nullptr) {
83  // init called a 2nd time?
84  return;
85  }
86 
87  // create backend
88  if (_type == AP_BeaconType_Pozyx) {
90  } else if (_type == AP_BeaconType_Marvelmind) {
92  }
93 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
94  if (_type == AP_BeaconType_SITL) {
95  _driver = new AP_Beacon_SITL(*this);
96  }
97 #endif
98 }
99 
100 // return true if beacon feature is enabled
102 {
103  return (_type != AP_BeaconType_None);
104 }
105 
106 // return true if sensor is basically healthy (we are receiving data)
108 {
109  if (!device_ready()) {
110  return false;
111  }
112  return _driver->healthy();
113 }
114 
115 // update state. This should be called often from the main loop
117 {
118  if (!device_ready()) {
119  return;
120  }
121  _driver->update();
122 
123  // update boundary for fence
125 }
126 
127 // return origin of position estimate system
128 bool AP_Beacon::get_origin(Location &origin_loc) const
129 {
130  if (!device_ready()) {
131  return false;
132  }
133 
134  // check for un-initialised origin
136  return false;
137  }
138 
139  // return origin
140  origin_loc.lat = origin_lat * 1.0e7;
141  origin_loc.lng = origin_lon * 1.0e7;
142  origin_loc.alt = origin_alt * 100;
143  origin_loc.options = 0; // all flags to zero meaning alt-above-sea-level
144 
145  return true;
146 }
147 
148 // return position in NED from position estimate system's origin in meters
149 bool AP_Beacon::get_vehicle_position_ned(Vector3f &position, float& accuracy_estimate) const
150 {
151  if (!device_ready()) {
152  return false;
153  }
154 
155  // check for timeout
157  return false;
158  }
159 
160  // return position
161  position = veh_pos_ned;
162  accuracy_estimate = veh_pos_accuracy;
163  return true;
164 }
165 
166 // return the number of beacons
167 uint8_t AP_Beacon::count() const
168 {
169  if (!device_ready()) {
170  return 0;
171  }
172  return num_beacons;
173 }
174 
175 // return all beacon data
176 bool AP_Beacon::get_beacon_data(uint8_t beacon_instance, struct BeaconState& state) const
177 {
178  if (!device_ready() || beacon_instance >= num_beacons) {
179  return false;
180  }
181  state = beacon_state[beacon_instance];
182  return true;
183 }
184 
185 // return individual beacon's id
186 uint8_t AP_Beacon::beacon_id(uint8_t beacon_instance) const
187 {
188  if (beacon_instance >= num_beacons) {
189  return 0;
190  }
191  return beacon_state[beacon_instance].id;
192 }
193 
194 // return beacon health
195 bool AP_Beacon::beacon_healthy(uint8_t beacon_instance) const
196 {
197  if (beacon_instance >= num_beacons) {
198  return false;
199  }
200  return beacon_state[beacon_instance].healthy;
201 }
202 
203 // return distance to beacon in meters
204 float AP_Beacon::beacon_distance(uint8_t beacon_instance) const
205 {
206  if (!beacon_state[beacon_instance].healthy || beacon_instance >= num_beacons) {
207  return 0.0f;
208  }
209  return beacon_state[beacon_instance].distance;
210 }
211 
212 // return beacon position in meters
213 Vector3f AP_Beacon::beacon_position(uint8_t beacon_instance) const
214 {
215  if (!device_ready() || beacon_instance >= num_beacons) {
216  Vector3f temp = {};
217  return temp;
218  }
219  return beacon_state[beacon_instance].position;
220 }
221 
222 // return last update time from beacon in milliseconds
223 uint32_t AP_Beacon::beacon_last_update_ms(uint8_t beacon_instance) const
224 {
225  if (_type == AP_BeaconType_None || beacon_instance >= num_beacons) {
226  return 0;
227  }
228  return beacon_state[beacon_instance].distance_update_ms;
229 }
230 
231 // create fence boundary points
233 {
234  // we need three beacons at least to create boundary fence.
235  // update boundary fence if number of beacons changes
237  return;
238  }
239 
240  // record number of beacons so we do not repeat calculations
242 
243  // accumulate beacon points
244  Vector2f beacon_points[AP_BEACON_MAX_BEACONS];
245  for (uint8_t index = 0; index < num_beacons; index++) {
246  const Vector3f& point_3d = beacon_position(index);
247  beacon_points[index].x = point_3d.x;
248  beacon_points[index].y = point_3d.y;
249  }
250 
251  // create polygon around boundary points using the following algorithm
252  // set the "current point" as the first boundary point
253  // 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
254  // check if point is already in boundary
255  // - no: add to boundary, move current point to this new point and repeat the above
256  // - yes: we've completed the bounding box, delete any boundary points found earlier than the duplicate
257 
258  Vector2f boundary_points[AP_BEACON_MAX_BEACONS+1]; // array of boundary points
259  uint8_t curr_boundary_idx = 0; // index into boundary_sorted index. always points to the highest filled in element of the array
260  uint8_t curr_beacon_idx = 0; // index into beacon_point array. point indexed is same point as curr_boundary_idx's
261 
262  // 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)
263  boundary_points[curr_boundary_idx] = beacon_points[curr_beacon_idx];
264 
265  bool boundary_success = false; // true once the boundary has been successfully found
266  bool boundary_failure = false; // true if we fail to build the boundary
267  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
268  while (!boundary_success && !boundary_failure) {
269  // look for next outer point
270  uint8_t next_idx;
271  float next_angle;
272  if (get_next_boundary_point(beacon_points, num_beacons, curr_beacon_idx, start_angle, next_idx, next_angle)) {
273  // add boundary point to boundary_sorted array
274  curr_boundary_idx++;
275  boundary_points[curr_boundary_idx] = beacon_points[next_idx];
276  curr_beacon_idx = next_idx;
277  start_angle = next_angle;
278 
279  // check if we have a complete boundary by looking for duplicate points within the boundary_sorted
280  uint8_t dup_idx = 0;
281  bool dup_found = false;
282  while (dup_idx < curr_boundary_idx && !dup_found) {
283  dup_found = (boundary_points[dup_idx] == boundary_points[curr_boundary_idx]);
284  if (!dup_found) {
285  dup_idx++;
286  }
287  }
288  // if duplicate is found, remove all boundary points before the duplicate because they are inner points
289  if (dup_found) {
290  uint8_t num_pts = curr_boundary_idx - dup_idx + 1;
291  if (num_pts > AP_BEACON_MINIMUM_FENCE_BEACONS) {
292  // success, copy boundary points to boundary array and convert meters to cm
293  for (uint8_t j = 0; j < num_pts; j++) {
294  boundary[j] = boundary_points[j+dup_idx] * 100.0f;
295  }
296  boundary_num_points = num_pts;
297  boundary_success = true;
298  } else {
299  // boundary has too few points
300  boundary_failure = true;
301  }
302  }
303  } else {
304  // failed to create boundary - give up!
305  boundary_failure = true;
306  }
307  }
308 
309  // clear boundary on failure
310  if (boundary_failure) {
312  }
313 }
314 
315 // find next boundary point from an array of boundary points given the current index into that array
316 // returns true if a next point can be found
317 // current_index should be an index into the boundary_pts array
318 // start_angle is an angle (in radians), the search will sweep clockwise from this angle
319 // the index of the next point is returned in the next_index argument
320 // the angle to the next point is returned in the next_angle argument
321 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)
322 {
323  // sanity check
324  if (boundary_pts == nullptr || current_index >= num_points) {
325  return false;
326  }
327 
328  // get current point
329  Vector2f curr_point = boundary_pts[current_index];
330 
331  // search through all points for next boundary point in a clockwise direction
332  float lowest_angle = M_PI_2;
333  float lowest_angle_relative = M_PI_2;
334  bool lowest_found = false;
335  uint8_t lowest_index = 0;
336  for (uint8_t i=0; i < num_points; i++) {
337  if (i != current_index) {
338  Vector2f vec = boundary_pts[i] - curr_point;
339  if (!vec.is_zero()) {
340  float angle = wrap_2PI(atan2f(vec.y, vec.x));
341  float angle_relative = wrap_2PI(angle - start_angle);
342  if ((angle_relative < lowest_angle_relative) || !lowest_found) {
343  lowest_angle = angle;
344  lowest_angle_relative = angle_relative;
345  lowest_index = i;
346  lowest_found = true;
347  }
348  }
349  }
350  }
351 
352  // return results
353  if (lowest_found) {
354  next_index = lowest_index;
355  next_angle = lowest_angle;
356  }
357  return lowest_found;
358 }
359 
360 // return fence boundary array
361 const Vector2f* AP_Beacon::get_boundary_points(uint16_t& num_points) const
362 {
363  if (!device_ready()) {
364  num_points = 0;
365  return nullptr;
366  }
367 
368  num_points = boundary_num_points;
369  return boundary;
370 }
371 
372 // check if the device is ready
373 bool AP_Beacon::device_ready(void) const
374 {
375  return ((_driver != nullptr) && (_type != AP_BeaconType_None));
376 }
float veh_pos_accuracy
Definition: AP_Beacon.h:128
static AP_SerialManager serial_manager
Definition: AHRS_Test.cpp:24
bool device_ready(void) const
Definition: AP_Beacon.cpp:373
#define M_PI_2
Definition: definitions.h:15
float beacon_distance(uint8_t beacon_instance) const
Definition: AP_Beacon.cpp:204
const Vector2f * get_boundary_points(uint16_t &num_points) const
Definition: AP_Beacon.cpp:361
void init(void)
Definition: AP_Beacon.cpp:80
#define AP_GROUPINFO(name, idx, clazz, element, def)
Definition: AP_Param.h:102
virtual void update()=0
const AP_HAL::HAL & hal
Definition: AC_PID_test.cpp:14
AP_Beacon(AP_SerialManager &_serial_manager)
Definition: AP_Beacon.cpp:73
#define AP_BEACON_MINIMUM_FENCE_BEACONS
Definition: AP_Beacon.h:27
#define AP_BEACON_MAX_BEACONS
Definition: AP_Beacon.h:25
AP_Float origin_lat
Definition: AP_Beacon.h:117
AP_SerialManager & serial_manager
Definition: AP_Beacon.h:124
static bool get_next_boundary_point(const Vector2f *boundary, uint8_t num_points, uint8_t current_index, float start_angle, uint8_t &next_index, float &next_angle)
Definition: AP_Beacon.cpp:321
int32_t lat
param 3 - Latitude * 10**7
Definition: AP_Common.h:143
bool beacon_healthy(uint8_t beacon_instance) const
Definition: AP_Beacon.cpp:195
AP_Float origin_alt
Definition: AP_Beacon.h:119
uint32_t distance_update_ms
Definition: AP_Beacon.h:49
T y
Definition: vector2.h:37
bool get_origin(Location &origin_loc) const
Definition: AP_Beacon.cpp:128
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
Definition: AP_Common.h:142
uint8_t beacon_id(uint8_t beacon_instance) const
Definition: AP_Beacon.cpp:186
bool get_vehicle_position_ned(Vector3f &pos, float &accuracy_estimate) const
Definition: AP_Beacon.cpp:149
bool is_zero(const T fVal1)
Definition: AP_Math.h:40
bool enabled(void)
Definition: AP_Beacon.cpp:101
Vector2f boundary[AP_BEACON_MAX_BEACONS+1]
Definition: AP_Beacon.h:136
T y
Definition: vector3.h:67
bool healthy(void)
Definition: AP_Beacon.cpp:107
uint32_t millis()
Definition: system.cpp:157
#define AP_BEACON_TIMEOUT_MS
Definition: AP_Beacon.h:26
uint8_t num_beacons
Definition: AP_Beacon.h:132
Vector3f beacon_position(uint8_t beacon_instance) const
Definition: AP_Beacon.cpp:213
AP_Beacon_Backend * _driver
Definition: AP_Beacon.h:123
static int state
Definition: Util.cpp:20
uint32_t veh_pos_update_ms
Definition: AP_Beacon.h:129
bool get_beacon_data(uint8_t beacon_instance, struct BeaconState &state) const
Definition: AP_Beacon.cpp:176
uint32_t beacon_last_update_ms(uint8_t beacon_instance) const
Definition: AP_Beacon.cpp:223
T x
Definition: vector2.h:37
int32_t lng
param 4 - Longitude * 10**7
Definition: AP_Common.h:144
float wrap_2PI(const T radian)
Definition: AP_Math.cpp:167
uint8_t boundary_num_points
Definition: AP_Beacon.h:137
void update(void)
Definition: AP_Beacon.cpp:116
bool is_zero(void) const
Definition: vector2.h:105
Vector3f veh_pos_ned
Definition: AP_Beacon.h:127
uint8_t boundary_num_beacons
Definition: AP_Beacon.h:138
BeaconState beacon_state[AP_BEACON_MAX_BEACONS]
Definition: AP_Beacon.h:133
static const struct AP_Param::GroupInfo var_info[]
Definition: AP_Beacon.h:100
AP_Int8 _type
Definition: AP_Beacon.h:116
AP_Float origin_lon
Definition: AP_Beacon.h:118
virtual bool healthy()=0
uint8_t count() const
Definition: AP_Beacon.cpp:167
#define AP_GROUPEND
Definition: AP_Param.h:121
T x
Definition: vector3.h:67
uint8_t options
Definition: AP_Common.h:136
void update_boundary_points()
Definition: AP_Beacon.cpp:232
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
Definition: AP_Param.cpp:1214