Exemplo n.º 1
0
// return the number of beacons
uint8_t AP_Beacon::count() const
{
    if (!device_ready()) {
        return 0;
    }
    return num_beacons;
}
Exemplo n.º 2
0
// return true if sensor is basically healthy (we are receiving data)
bool AP_Beacon::healthy(void)
{
    if (!device_ready()) {
        return false;
    }
    return _driver->healthy();
}
Exemplo n.º 3
0
// update state. This should be called often from the main loop
void AP_Beacon::update(void)
{
    if (!device_ready()) {
        return;
    }
    _driver->update();
}
Exemplo n.º 4
0
// return beacon position in meters
Vector3f AP_Beacon::beacon_position(uint8_t beacon_instance) const
{
    if (!device_ready() || beacon_instance >= num_beacons) {
        Vector3f temp = {};
        return temp;
    }
    return beacon_state[beacon_instance].position;
}
Exemplo n.º 5
0
// return all beacon data
bool AP_Beacon::get_beacon_data(uint8_t beacon_instance, struct BeaconState& state) const
{
    if (!device_ready() || beacon_instance >= num_beacons) {
        return false;
    }
    state = beacon_state[beacon_instance];
    return true;
}
Exemplo n.º 6
0
// return fence boundary array
const Vector2f* AP_Beacon::get_boundary_points(uint16_t& num_points) const
{
    if (!device_ready()) {
        num_points = 0;
        return nullptr;
    }

    num_points = boundary_num_points;
    return boundary;
}
Exemplo n.º 7
0
// update state. This should be called often from the main loop
void AP_Beacon::update(void)
{
    if (!device_ready()) {
        return;
    }
    _driver->update();

    // update boundary for fence
    update_boundary_points();
}
Exemplo n.º 8
0
// return position in NED from position estimate system's origin in meters
bool AP_Beacon::get_vehicle_position_ned(Vector3f &position, float& accuracy_estimate) const
{
    if (!device_ready()) {
        return false;
    }

    // check for timeout
    if (AP_HAL::millis() - veh_pos_update_ms > AP_BEACON_TIMEOUT_MS) {
        return false;
    }

    // return position
    position = veh_pos_ned;
    accuracy_estimate = veh_pos_accuracy;
    return true;
}
Exemplo n.º 9
0
// return origin of position estimate system
bool AP_Beacon::get_origin(Location &origin_loc) const
{
    if (!device_ready()) {
        return false;
    }

    // check for un-initialised origin
    if (is_zero(origin_lat) && is_zero(origin_lon) && is_zero(origin_alt)) {
        return false;
    }

    // return origin
    origin_loc = {};
    origin_loc.lat = origin_lat * 1.0e7f;
    origin_loc.lng = origin_lon * 1.0e7f;
    origin_loc.alt = origin_alt * 100;

    return true;
}
Exemplo n.º 10
0
// return origin of position estimate system
bool AP_Beacon::get_origin(Location &origin_loc) const
{
    if (!device_ready()) {
        return false;
    }

    // check for un-initialised origin
    if (is_zero(origin_lat) && is_zero(origin_lon) && is_zero(origin_alt)) {
        return false;
    }

    // return origin
    origin_loc.lat = origin_lat * 1.0e7;
    origin_loc.lng = origin_lon * 1.0e7;
    origin_loc.alt = origin_alt * 100;
    origin_loc.options = 0; // all flags to zero meaning alt-above-sea-level

    return true;
}
Exemplo n.º 11
0
int device_alloc(struct device **device, const char *path)
{
	struct device *dev;
	int r;

	if (!path) {
		*device = NULL;
		return 0;
	}

	dev = malloc(sizeof(struct device));
	if (!dev)
		return -ENOMEM;

	memset(dev, 0, sizeof(struct device));
	dev->loop_fd = -1;

	r = device_ready(path);
	if (!r) {
		dev->init_done = 1;
	} else if (r == -ENOTBLK) {
		/* alloc loop later */
	} else if (r < 0) {
		free(dev);
		return -ENOTBLK;
	}

	dev->path = strdup(path);
	if (!dev->path) {
		free(dev);
		return -ENOMEM;
	}

	*device = dev;
	return 0;
}
Exemplo n.º 12
0
// create fence boundary points
void AP_Beacon::update_boundary_points()
{
    // we need three beacons at least to create boundary fence.
    // update boundary fence if number of beacons changes
    if (!device_ready() || num_beacons < AP_BEACON_MINIMUM_FENCE_BEACONS || boundary_num_beacons == num_beacons) {
        return;
    }

    // record number of beacons so we do not repeat calculations
    boundary_num_beacons = num_beacons;

    // accumulate beacon points
    Vector2f beacon_points[AP_BEACON_MAX_BEACONS];
    for (uint8_t index = 0; index < num_beacons; index++) {
        const Vector3f& point_3d = beacon_position(index);
        beacon_points[index].x = point_3d.x;
        beacon_points[index].y = point_3d.y;
    }

    // create polygon around boundary points using the following algorithm
    //     set the "current point" as the first boundary point
    //     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
    //     check if point is already in boundary
    //       - no: add to boundary, move current point to this new point and repeat the above
    //       - yes: we've completed the bounding box, delete any boundary points found earlier than the duplicate

    Vector2f boundary_points[AP_BEACON_MAX_BEACONS+1];  // array of boundary points
    uint8_t curr_boundary_idx = 0;                      // index into boundary_sorted index.  always points to the highest filled in element of the array
    uint8_t curr_beacon_idx = 0;                        // index into beacon_point array.  point indexed is same point as curr_boundary_idx's

    // 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)
    boundary_points[curr_boundary_idx] = beacon_points[curr_beacon_idx];

    bool boundary_success = false;  // true once the boundary has been successfully found
    bool boundary_failure = false;  // true if we fail to build the boundary
    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
    while (!boundary_success && !boundary_failure) {
        // look for next outer point
        uint8_t next_idx;
        float next_angle;
        if (get_next_boundary_point(beacon_points, num_beacons, curr_beacon_idx, start_angle, next_idx, next_angle)) {
            // add boundary point to boundary_sorted array
            curr_boundary_idx++;
            boundary_points[curr_boundary_idx] = beacon_points[next_idx];
            curr_beacon_idx = next_idx;
            start_angle = next_angle;

            // check if we have a complete boundary by looking for duplicate points within the boundary_sorted
            uint8_t dup_idx = 0;
            bool dup_found = false;
            while (dup_idx < curr_boundary_idx && !dup_found) {
                dup_found = (boundary_points[dup_idx] == boundary_points[curr_boundary_idx]);
                if (!dup_found) {
                    dup_idx++;
                }
            }
            // if duplicate is found, remove all boundary points before the duplicate because they are inner points
            if (dup_found) {
                uint8_t num_pts = curr_boundary_idx - dup_idx + 1;
                if (num_pts > AP_BEACON_MINIMUM_FENCE_BEACONS) {
                    // success, copy boundary points to boundary array and convert meters to cm
                    for (uint8_t j = 0; j < num_pts; j++) {
                        boundary[j] = boundary_points[j+dup_idx] * 100.0f;
                    }
                    boundary_num_points = num_pts;
                    boundary_success = true;
                } else {
                    // boundary has too few points
                    boundary_failure = true;
                }
            }
        } else {
            // failed to create boundary - give up!
            boundary_failure = true;
        }
    }

    // clear boundary on failure
    if (boundary_failure) {
        boundary_num_points = 0;
    }
}