ardupilot/libraries/AP_Beacon/AP_Beacon.h

140 lines
4.8 KiB
C++

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>
class AP_Beacon_Backend;
#define AP_BEACON_MAX_BEACONS 4
#define AP_BEACON_TIMEOUT_MS 300
#define AP_BEACON_MINIMUM_FENCE_BEACONS 3
class AP_Beacon
{
public:
friend class AP_Beacon_Backend;
AP_Beacon(AP_SerialManager &_serial_manager);
// external position backend types (used by _TYPE parameter)
enum AP_BeaconType {
AP_BeaconType_None = 0,
AP_BeaconType_Pozyx = 1,
AP_BeaconType_Marvelmind = 2,
AP_BeaconType_SITL = 10
};
// The AP_BeaconState structure is filled in by the backend driver
struct BeaconState {
uint16_t id; // unique id of beacon
bool healthy; // true if beacon is healthy
float distance; // distance from vehicle to beacon (in meters)
uint32_t distance_update_ms; // system time of last update from this beacon
Vector3f position; // location of beacon as an offset from origin in NED in meters
};
// initialise any available position estimators
void init(void);
// return true if beacon feature is enabled
bool enabled(void);
// return true if sensor is basically healthy (we are receiving data)
bool healthy(void);
// update state of all beacons
void update(void);
// return origin of position estimate system in lat/lon
bool get_origin(Location &origin_loc) const;
// return vehicle position in NED from position estimate system's origin in meters
bool get_vehicle_position_ned(Vector3f& pos, float& accuracy_estimate) const;
// return the number of beacons
uint8_t count() const;
// methods to return beacon specific information
// return all beacon data
bool get_beacon_data(uint8_t beacon_instance, struct BeaconState& state) const;
// return individual beacon's id
uint8_t beacon_id(uint8_t beacon_instance) const;
// return beacon health
bool beacon_healthy(uint8_t beacon_instance) const;
// return distance to beacon in meters
float beacon_distance(uint8_t beacon_instance) const;
// return NED position of beacon in meters relative to the beacon systems origin
Vector3f beacon_position(uint8_t beacon_instance) const;
// return last update time from beacon in milliseconds
uint32_t beacon_last_update_ms(uint8_t beacon_instance) const;
// update fence boundary array
void update_boundary_points();
// return fence boundary array
const Vector2f* get_boundary_points(uint16_t& num_points) const;
static const struct AP_Param::GroupInfo var_info[];
private:
// check if device is ready
bool device_ready(void) const;
// find next boundary point from an array of boundary points given the current index into that array
// returns true if a next point can be found
// current_index should be an index into the boundary_pts array
// start_angle is an angle (in radians), the search will sweep clockwise from this angle
// the index of the next point is returned in the next_index argument
// the angle to the next point is returned in the next_angle argument
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);
// parameters
AP_Int8 _type;
AP_Float origin_lat;
AP_Float origin_lon;
AP_Float origin_alt;
AP_Int16 orient_yaw;
// external references
AP_Beacon_Backend *_driver;
AP_SerialManager &serial_manager;
// last known position
Vector3f veh_pos_ned;
float veh_pos_accuracy;
uint32_t veh_pos_update_ms;
// individual beacon data
uint8_t num_beacons = 0;
BeaconState beacon_state[AP_BEACON_MAX_BEACONS];
// fence boundary
Vector2f boundary[AP_BEACON_MAX_BEACONS+1]; // array of boundary points (used for fence)
uint8_t boundary_num_points; // number of points in boundary
uint8_t boundary_num_beacons; // total number of beacon points consumed while building boundary
};