AP_Beacon: library to accept distances from beacons

libraries/AP_Beacon/AP_Beacon.cpp

@@ -0,0 +1,224 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+
+#include "AP_Beacon.h"
+#include "AP_Beacon_Backend.h"
+#include "AP_Beacon_Pozyx.h"
+#include "AP_Beacon_SITL.h"
+
+extern const AP_HAL::HAL &hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_Beacon::var_info[] = {
+
+    // @Param: _TYPE
+    // @DisplayName: Beacon based position estimation device type
+    // @Description: What type of beacon based position estimation device is connected
+    // @Values: 0:None,1:Pozyx
+    // @User: Advanced
+    AP_GROUPINFO("_TYPE",    0, AP_Beacon, _type, 0),
+
+    // @Param: _LATITUDE
+    // @DisplayName: Beacon origin's latitude
+    // @Description: Beacon origin's latitude
+    // @Units: degrees
+    // @Increment: 0.000001
+    // @Range: -90 90
+    // @User: Advanced
+    AP_GROUPINFO("_LATITUDE", 1, AP_Beacon, origin_lat, 0),
+
+    // @Param: _LONGITUDE
+    // @DisplayName: Beacon origin's longitude
+    // @Description: Beacon origin's longitude
+    // @Units: degrees
+    // @Increment: 0.000001
+    // @Range: -180 180
+    // @User: Advanced
+    AP_GROUPINFO("_LONGITUDE", 2, AP_Beacon, origin_lon, 0),
+
+    // @Param: _ALT
+    // @DisplayName: Beacon origin's altitude above sealevel in meters
+    // @Description: Beacon origin's altitude above sealevel in meters
+    // @Units: meters
+    // @Increment: 1
+    // @Range: 0 10000
+    // @User: Advanced
+    AP_GROUPINFO("_ALT", 3, AP_Beacon, origin_alt, 0),
+
+    // @Param: _ORIENT_YAW
+    // @DisplayName: Beacon systems rotation from north in degrees
+    // @Description: Beacon systems rotation from north in degrees
+    // @Units: degrees
+    // @Increment: 1
+    // @Range: -180 +180
+    // @User: Advanced
+    AP_GROUPINFO("_ORIENT_YAW", 4, AP_Beacon, orient_yaw, 0),
+
+    AP_GROUPEND
+};
+
+AP_Beacon::AP_Beacon(AP_SerialManager &_serial_manager) :
+    serial_manager(_serial_manager)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// initialise the AP_Beacon class
+void AP_Beacon::init(void)
+{
+    if (_driver != nullptr) {
+        // init called a 2nd time?
+        return;
+    }
+
+    // create backend
+    if (_type == AP_BeaconType_Pozyx) {
+        _driver = new AP_Beacon_Pozyx(*this, serial_manager);
+    }
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (_type == AP_BeaconType_SITL) {
+        _driver = new AP_Beacon_SITL(*this);
+    }
+#endif
+}
+
+// return true if beacon feature is enabled
+bool AP_Beacon::enabled(void)
+{
+    return (_type != AP_BeaconType_None);
+}
+
+// return true if sensor is basically healthy (we are receiving data)
+bool AP_Beacon::healthy(void)
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None) {
+        return false;
+    }
+    return _driver->healthy();
+}
+
+// update state. This should be called often from the main loop
+void AP_Beacon::update(void)
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None) {
+        return;
+    }
+    _driver->update();
+}
+
+// return origin of position estimate system
+bool AP_Beacon::get_origin(Location &origin_loc) const
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None) {
+        return false;
+    }
+
+    // check for unitialised 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;
+}
+
+// return position in NED from position estimate system's origin
+bool AP_Beacon::get_vehicle_position_ned(Vector3f &position, float& accuracy_estimate) const
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None) {
+        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;
+}
+
+// return the number of beacons
+uint8_t AP_Beacon::count() const
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None) {
+        return 0;
+    }
+    return num_beacons;
+}
+
+// return all beacon data
+bool AP_Beacon::get_beacon_data(uint8_t beacon_instance, struct BeaconState& state) const
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None || beacon_instance >= num_beacons) {
+        return false;
+    }
+    state = beacon_state[beacon_instance];
+    return true;
+}
+
+// return individual beacon's id
+uint8_t AP_Beacon::beacon_id(uint8_t beacon_instance) const
+{
+    if (beacon_instance >= num_beacons) {
+        return 0;
+    }
+    return beacon_state[beacon_instance].id;
+}
+
+// return beacon health
+bool AP_Beacon::beacon_healthy(uint8_t beacon_instance) const
+{
+    if (beacon_instance >= num_beacons) {
+        return false;
+    }
+    return beacon_state[beacon_instance].healthy;
+}
+
+// return distance to beacon in meters
+float AP_Beacon::beacon_distance(uint8_t beacon_instance) const
+{
+    if (!beacon_state[beacon_instance].healthy || beacon_instance >= num_beacons) {
+        return 0.0f;
+    }
+    return beacon_state[beacon_instance].distance;
+}
+
+// return beacon position
+Vector3f AP_Beacon::beacon_position(uint8_t beacon_instance) const
+{
+    if (_driver == nullptr || _type == AP_BeaconType_None || beacon_instance >= num_beacons) {
+        Vector3f temp = {};
+        return temp;
+    }
+    return beacon_state[beacon_instance].position;
+}
+
+// return last update time from beacon
+uint32_t AP_Beacon::beacon_last_update_ms(uint8_t beacon_instance) const
+{
+    if (_type == AP_BeaconType_None || beacon_instance >= num_beacons) {
+        return 0;
+    }
+    return beacon_state[beacon_instance].distance_update_ms;
+}

libraries/AP_Beacon/AP_Beacon.h

@@ -0,0 +1,114 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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
+
+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
+    };
+
+    // 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
+    bool get_origin(Location &origin_loc) const;
+
+    // return vehicle position in NED from position estimate system's origin
+    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 relative to the beacon systems origin
+    Vector3f beacon_position(uint8_t beacon_instance) const;
+
+    // return last update time from beacon
+    uint32_t beacon_last_update_ms(uint8_t beacon_instance) const;
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    // 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];
+};

libraries/AP_Beacon/AP_Beacon_Backend.cpp

@@ -0,0 +1,105 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+
+#include "AP_Beacon_Backend.h"
+// debug
+#include <stdio.h>
+
+/*
+  base class constructor. 
+  This incorporates initialisation as well.
+*/
+AP_Beacon_Backend::AP_Beacon_Backend(AP_Beacon &frontend) :
+    _frontend(frontend)
+{
+}
+
+// set vehicle position, pos should be in the beacon's local frame
+void AP_Beacon_Backend::set_vehicle_position(const Vector3f& pos, float accuracy_estimate)
+{
+    _frontend.veh_pos_update_ms = AP_HAL::millis();
+    _frontend.veh_pos_accuracy = accuracy_estimate;
+    _frontend.veh_pos_ned = correct_for_orient_yaw(pos);
+
+    // debug
+    //::printf("vehicle x:%4.2f y:%4.2f z:%4.2f error:%4.2f\n", (double)pos.x, (double)pos.y, (double)pos.z, (double)accuracy_estimate);
+}
+
+// set individual beacon distance in meters
+void AP_Beacon_Backend::set_beacon_distance(uint8_t beacon_instance, float distance)
+{
+    // sanity check instance
+    if (beacon_instance > AP_BEACON_MAX_BEACONS) {
+        return;
+    }
+
+    // setup new beacon
+    if (beacon_instance >= _frontend.num_beacons) {
+        _frontend.num_beacons = beacon_instance+1;
+    }
+
+    _frontend.beacon_state[beacon_instance].distance_update_ms = AP_HAL::millis();
+    _frontend.beacon_state[beacon_instance].distance = distance;
+    _frontend.beacon_state[beacon_instance].healthy = true;
+
+    //::printf("beacon %d dist:%4.2f\n", (int)beacon_instance, (double)distance);
+}
+
+// configure beacon's position in meters from origin
+// pos should be in the beacon's local frame
+void AP_Beacon_Backend::set_beacon_position(uint8_t beacon_instance, const Vector3f& pos)
+{
+    // sanity check instance
+    if (beacon_instance > AP_BEACON_MAX_BEACONS) {
+        return;
+    }
+
+    // setup new beacon
+    if (beacon_instance >= _frontend.num_beacons) {
+        _frontend.num_beacons = beacon_instance+1;
+    }
+
+    //::printf("beacon %d x:%4.2f y:%4.2f z:%4.2f\n", (int)beacon_instance, (double)pos.x, (double)pos.y, (double)pos.z);
+
+    // set position after correcting yaw
+    _frontend.beacon_state[beacon_instance].position = correct_for_orient_yaw(pos);
+}
+
+// rotate vector to correct for beacon system yaw orientation
+Vector3f AP_Beacon_Backend::correct_for_orient_yaw(const Vector3f &vector)
+{
+    // exit immediately if no correction
+    if (_frontend.orient_yaw == 0) {
+        return vector;
+    }
+
+    // check for change in parameter value and update constants
+    if (orient_yaw_deg != _frontend.orient_yaw) {
+        _frontend.orient_yaw = wrap_180(_frontend.orient_yaw.get());
+
+        // calculate rotation constants
+        orient_yaw_deg = _frontend.orient_yaw;
+        orient_cos_yaw = cosf(radians(orient_yaw_deg));
+        orient_sin_yaw = sinf(radians(orient_yaw_deg));
+    }
+
+    // rotate x,y by -orient_yaw
+    Vector3f vec_rotated;
+    vec_rotated.x = vector.x*orient_cos_yaw - vector.y*orient_sin_yaw;
+    vec_rotated.y = vector.x*orient_sin_yaw + vector.y*orient_cos_yaw;
+    vec_rotated.z = vector.z;
+    return vec_rotated;
+}

libraries/AP_Beacon/AP_Beacon_Backend.h

@@ -0,0 +1,61 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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_Math/AP_Math.h>
+#include <AP_HAL/AP_HAL.h>
+#include "AP_Beacon.h"
+
+class AP_Beacon_Backend
+{
+public:
+    // constructor. This incorporates initialisation as well.
+	AP_Beacon_Backend(AP_Beacon &frontend);
+
+	// return true if sensor is basically healthy (we are receiving data)
+    virtual bool healthy() = 0;
+
+    // update
+    virtual void update() = 0;
+
+    // set vehicle position, pos should be in the beacon's local frame
+    void set_vehicle_position(const Vector3f& pos, float accuracy_estimate);
+
+    // set individual beacon distance in meters
+    void set_beacon_distance(uint8_t beacon_instance, float distance);
+
+    // configure beacon's position in meters from origin
+    // pos should be in the beacon's local frame
+    void set_beacon_position(uint8_t beacon_instance, const Vector3f& pos);
+
+    float get_beacon_origin_lat(void) const { return _frontend.origin_lat; }
+    float get_beacon_origin_lon(void) const { return _frontend.origin_lon; }
+    float get_beacon_origin_alt(void) const { return _frontend.origin_alt; }
+
+protected:
+
+    // references
+    AP_Beacon &_frontend;
+
+    // yaw correction
+    int16_t orient_yaw_deg; // cached version of orient_yaw parameter
+    float orient_cos_yaw = 0.0f;
+    float orient_sin_yaw = 1.0f;
+
+    // yaw correction methods
+    Vector3f correct_for_orient_yaw(const Vector3f &vector);
+};

libraries/AP_Beacon/AP_Beacon_Pozyx.cpp

@@ -0,0 +1,175 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_Beacon_Pozyx.h"
+#include <ctype.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+// constructor
+AP_Beacon_Pozyx::AP_Beacon_Pozyx(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
+    AP_Beacon_Backend(frontend),
+    linebuf_len(0)
+{
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
+    if (uart != nullptr) {
+        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
+    }
+}
+
+// return true if sensor is basically healthy (we are receiving data)
+bool AP_Beacon_Pozyx::healthy()
+{
+    // healthy if we have parsed a message within the past 300ms
+    return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS);
+}
+
+// update the state of the sensor
+void AP_Beacon_Pozyx::update(void)
+{
+    static uint8_t counter = 0;
+    counter++;
+    if (counter > 200) {
+        counter = 0;
+        //::printf("b\n");
+    }
+
+    if (uart == nullptr) {
+        return;
+    }
+
+    // read any available characters
+    int16_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        char c = uart->read();
+
+        switch (parse_state) {
+
+            default:
+            case ParseState_WaitingForHeader:
+                if (c == AP_BEACON_POZYX_HEADER) {
+                    parse_state = ParseState_WaitingForMsgId;
+                    linebuf_len = 0;
+                }
+                break;
+
+            case ParseState_WaitingForMsgId:
+                parse_msg_id = c;
+                if (parse_msg_id == AP_BEACON_POZYX_MSGID_BEACON_CONFIG ||
+                    parse_msg_id == AP_BEACON_POZYX_MSGID_BEACON_DIST ||
+                    parse_msg_id == AP_BEACON_POZYX_MSGID_POSITION) {
+                    parse_state = ParseState_WaitingForLen;
+                } else {
+                    // invalid message id
+                    parse_state = ParseState_WaitingForHeader;
+                }
+                break;
+
+            case ParseState_WaitingForLen:
+                parse_msg_len = c;
+                if (parse_msg_len > AP_BEACON_POZYX_MSG_LEN_MAX) {
+                    // invalid message length
+                    parse_state = ParseState_WaitingForHeader;
+                } else {
+                    parse_state = ParseState_WaitingForContents;
+                }
+                break;
+
+            case ParseState_WaitingForContents:
+                // add to buffer
+                linebuf[linebuf_len++] = c;
+                if ((linebuf_len == parse_msg_len) || (linebuf_len == sizeof(linebuf))) {
+                    // process buffer
+                    parse_buffer();
+                    // reset state for next message
+                    parse_state = ParseState_WaitingForHeader;
+                }
+                break;
+        }
+    }
+}
+
+// parse buffer
+void AP_Beacon_Pozyx::parse_buffer()
+{
+    // check crc
+    uint8_t checksum = 0;
+    checksum ^= parse_msg_id;
+    checksum ^= parse_msg_len;
+    for (uint8_t i=0; i<linebuf_len-1; i++) {
+        checksum ^= linebuf[i];
+    }
+    // return if failed checksum check
+    if (checksum != linebuf[linebuf_len-1]) {
+        return;
+    }
+
+    bool parsed = false;
+    switch (parse_msg_id) {
+
+        case AP_BEACON_POZYX_MSGID_BEACON_CONFIG:
+            {
+                uint8_t beacon_id = linebuf[0];
+                //uint8_t beacon_count = linebuf[1];
+                int32_t beacon_x = (uint32_t)linebuf[5] << 24 | (uint32_t)linebuf[4] << 16 | (uint32_t)linebuf[3] << 8 | (uint32_t)linebuf[2];
+                int32_t beacon_y = (uint32_t)linebuf[9] << 24 | (uint32_t)linebuf[8] << 16 | (uint32_t)linebuf[7] << 8 | (uint32_t)linebuf[6];
+                int32_t beacon_z = (uint32_t)linebuf[13] << 24 | (uint32_t)linebuf[12] << 16 | (uint32_t)linebuf[11] << 8 | (uint32_t)linebuf[10];
+                Vector3f beacon_pos(beacon_x / 1000.0f, beacon_y / 1000.0f, beacon_z / 1000.0f);
+                if (beacon_pos.length() <= AP_BEACON_DISTANCE_MAX) {
+                    set_beacon_position(beacon_id, beacon_pos);
+                    parsed = true;
+                }
+            }
+            break;
+
+        case AP_BEACON_POZYX_MSGID_BEACON_DIST:
+            {
+                uint8_t beacon_id = linebuf[0];
+                uint32_t beacon_distance = (uint32_t)linebuf[4] << 24 | (uint32_t)linebuf[3] << 16 | (uint32_t)linebuf[2] << 8 | (uint32_t)linebuf[1];
+                float beacon_dist = beacon_distance/1000.0f;
+                if (beacon_dist <= AP_BEACON_DISTANCE_MAX) {
+                    set_beacon_distance(beacon_id, beacon_dist);
+                    parsed = true;
+                }
+            }
+            break;
+
+        case AP_BEACON_POZYX_MSGID_POSITION:
+            {
+                int32_t vehicle_x = (uint32_t)linebuf[3] << 24 | (uint32_t)linebuf[2] << 16 | (uint32_t)linebuf[1] << 8 | (uint32_t)linebuf[0];
+                int32_t vehicle_y = (uint32_t)linebuf[7] << 24 | (uint32_t)linebuf[6] << 16 | (uint32_t)linebuf[5] << 8 | (uint32_t)linebuf[4];
+                int32_t vehicle_z = (uint32_t)linebuf[11] << 24 | (uint32_t)linebuf[10] << 16 | (uint32_t)linebuf[9] << 8 | (uint32_t)linebuf[8];
+                int16_t position_error = (uint32_t)linebuf[13] << 8 | (uint32_t)linebuf[12];
+                Vector3f veh_pos(Vector3f(vehicle_x / 1000.0f, vehicle_y / 1000.0f, vehicle_z / 1000.0f));
+                if (veh_pos.length() <= AP_BEACON_DISTANCE_MAX) {
+                    set_vehicle_position(veh_pos, position_error);
+                    parsed = true;
+                }
+            }
+            break;
+
+        default:
+            // unrecognised message id
+            break;
+    }
+
+    // record success
+    if (parsed) {
+        last_update_ms = AP_HAL::millis();
+    }
+}

libraries/AP_Beacon/AP_Beacon_Pozyx.h

@@ -0,0 +1,45 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
+
+#include "AP_Beacon_Backend.h"
+
+#define AP_BEACON_POZYX_MSG_LEN_MAX         20      // messages from uno/pozyx are no more than 20bytes
+#define AP_BEACON_POZYX_HEADER              0x01    // messages start with this character
+#define AP_BEACON_POZYX_MSGID_BEACON_CONFIG 0x02    // message contains anchor config information
+#define AP_BEACON_POZYX_MSGID_BEACON_DIST   0x03    // message contains individual beacon distance
+#define AP_BEACON_POZYX_MSGID_POSITION      0x04    // message contains vehicle position information
+#define AP_BEACON_DISTANCE_MAX              200.0f  // sanity check beacon and vehicle messages to be within this distance from origin
+
+class AP_Beacon_Pozyx : public AP_Beacon_Backend
+{
+
+public:
+    // constructor
+    AP_Beacon_Pozyx(AP_Beacon &frontend, AP_SerialManager &serial_manager);
+
+    // return true if sensor is basically healthy (we are receiving data)
+    bool healthy();
+
+    // update
+    void update();
+
+private:
+
+    enum ParseState{
+        ParseState_WaitingForHeader = 0,
+        ParseState_WaitingForMsgId = 1,
+        ParseState_WaitingForLen = 2,
+        ParseState_WaitingForContents = 3
+    } parse_state;
+
+    // parse buffer
+    void parse_buffer();
+
+    uint8_t parse_msg_id;
+    uint8_t parse_msg_len;
+
+    AP_HAL::UARTDriver *uart = nullptr;
+    uint8_t linebuf[AP_BEACON_POZYX_MSG_LEN_MAX];
+    uint8_t linebuf_len = 0;
+    uint32_t last_update_ms = 0;
+};