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_Beacon_config.h" 18 : 19 : #if AP_BEACON_ENABLED 20 : 21 : #include <AP_Common/AP_Common.h> 22 : #include <AP_Param/AP_Param.h> 23 : #include <AP_Math/AP_Math.h> 24 : #include <AP_Common/Location.h> 25 : 26 : #ifndef AP_BEACON_MAX_BEACONS 27 : #define AP_BEACON_MAX_BEACONS 4 28 : #endif 29 : 30 : #ifndef AP_BEACON_TIMEOUT_MS 31 : #define AP_BEACON_TIMEOUT_MS 300 32 : #endif 33 : 34 : class AP_Beacon_Backend; 35 : 36 : class AP_Beacon 37 : { 38 : public: 39 : friend class AP_Beacon_Backend; 40 : 41 : AP_Beacon(); 42 : 43 : // get singleton instance 44 967109 : static AP_Beacon *get_singleton() { return _singleton; } 45 : 46 : // external position backend types (used by _TYPE parameter) 47 : enum class Type : uint8_t { 48 : None = 0, 49 : Pozyx = 1, 50 : Marvelmind = 2, 51 : Nooploop = 3, 52 : #if AP_BEACON_SITL_ENABLED 53 : SITL = 10 54 : #endif 55 : }; 56 : 57 : // The AP_BeaconState structure is filled in by the backend driver 58 : struct BeaconState { 59 : uint16_t id; // unique id of beacon 60 : bool healthy; // true if beacon is healthy 61 : float distance; // distance from vehicle to beacon (in meters) 62 : uint32_t distance_update_ms; // system time of last update from this beacon 63 : Vector3f position; // location of beacon as an offset from origin in NED in meters 64 : }; 65 : 66 : // initialise any available position estimators 67 : void init(void); 68 : 69 : // return true if beacon feature is enabled 70 : bool enabled(void) const; 71 : 72 : // return true if sensor is basically healthy (we are receiving data) 73 : bool healthy(void) const; 74 : 75 : // update state of all beacons 76 : void update(void); 77 : 78 : // return origin of position estimate system in lat/lon 79 : bool get_origin(Location &origin_loc) const; 80 : 81 : // return vehicle position in NED from position estimate system's origin in meters 82 : bool get_vehicle_position_ned(Vector3f& pos, float& accuracy_estimate) const; 83 : 84 : // return the number of beacons 85 : uint8_t count() const; 86 : 87 : // methods to return beacon specific information 88 : 89 : // return all beacon data 90 : bool get_beacon_data(uint8_t beacon_instance, struct BeaconState& state) const; 91 : 92 : // return individual beacon's id 93 : uint8_t beacon_id(uint8_t beacon_instance) const; 94 : 95 : // return beacon health 96 : bool beacon_healthy(uint8_t beacon_instance) const; 97 : 98 : // return distance to beacon in meters 99 : float beacon_distance(uint8_t beacon_instance) const; 100 : 101 : // return NED position of beacon in meters relative to the beacon systems origin 102 : Vector3f beacon_position(uint8_t beacon_instance) const; 103 : 104 : // return last update time from beacon in milliseconds 105 : uint32_t beacon_last_update_ms(uint8_t beacon_instance) const; 106 : 107 : // update fence boundary array 108 : void update_boundary_points(); 109 : 110 : // return fence boundary array 111 : const Vector2f* get_boundary_points(uint16_t& num_points) const; 112 : 113 : static const struct AP_Param::GroupInfo var_info[]; 114 : 115 : // a method for vehicles to call to make onboard log messages: 116 : void log(); 117 : 118 : private: 119 : 120 : static AP_Beacon *_singleton; 121 : 122 : // check if device is ready 123 : bool device_ready(void) const; 124 : 125 : // find next boundary point from an array of boundary points given the current index into that array 126 : // returns true if a next point can be found 127 : // current_index should be an index into the boundary_pts array 128 : // start_angle is an angle (in radians), the search will sweep clockwise from this angle 129 : // the index of the next point is returned in the next_index argument 130 : // the angle to the next point is returned in the next_angle argument 131 : 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); 132 : 133 : // parameters 134 : AP_Enum<Type> _type; 135 : AP_Float origin_lat; 136 : AP_Float origin_lon; 137 : AP_Float origin_alt; 138 : AP_Int16 orient_yaw; 139 : 140 : // external references 141 : AP_Beacon_Backend *_driver; 142 : 143 : // last known position 144 : Vector3f veh_pos_ned; 145 : float veh_pos_accuracy; 146 : uint32_t veh_pos_update_ms; 147 : 148 : // individual beacon data 149 : uint8_t num_beacons = 0; 150 : BeaconState beacon_state[AP_BEACON_MAX_BEACONS]; 151 : 152 : // fence boundary 153 : Vector2f boundary[AP_BEACON_MAX_BEACONS+1]; // array of boundary points (used for fence) 154 : uint8_t boundary_num_points; // number of points in boundary 155 : uint8_t boundary_num_beacons; // total number of beacon points consumed while building boundary 156 : }; 157 : 158 : namespace AP { 159 : AP_Beacon *beacon(); 160 : }; 161 : 162 : #endif // AP_BEACON_ENABLED
