From 821fc516fb86f7322cac631604eb4ef242947563 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 24 Oct 2016 14:46:07 +0900 Subject: [PATCH] AP_Beacon: library to accept distances from beacons --- libraries/AP_Beacon/AP_Beacon.cpp | 224 ++++++++++++++++++++++ libraries/AP_Beacon/AP_Beacon.h | 114 +++++++++++ libraries/AP_Beacon/AP_Beacon_Backend.cpp | 105 ++++++++++ libraries/AP_Beacon/AP_Beacon_Backend.h | 61 ++++++ libraries/AP_Beacon/AP_Beacon_Pozyx.cpp | 175 +++++++++++++++++ libraries/AP_Beacon/AP_Beacon_Pozyx.h | 45 +++++ 6 files changed, 724 insertions(+) create mode 100644 libraries/AP_Beacon/AP_Beacon.cpp create mode 100644 libraries/AP_Beacon/AP_Beacon.h create mode 100644 libraries/AP_Beacon/AP_Beacon_Backend.cpp create mode 100644 libraries/AP_Beacon/AP_Beacon_Backend.h create mode 100644 libraries/AP_Beacon/AP_Beacon_Pozyx.cpp create mode 100644 libraries/AP_Beacon/AP_Beacon_Pozyx.h diff --git a/libraries/AP_Beacon/AP_Beacon.cpp b/libraries/AP_Beacon/AP_Beacon.cpp new file mode 100644 index 0000000000..1e6cfd7461 --- /dev/null +++ b/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 . + */ + +#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; +} diff --git a/libraries/AP_Beacon/AP_Beacon.h b/libraries/AP_Beacon/AP_Beacon.h new file mode 100644 index 0000000000..4119a4ea52 --- /dev/null +++ b/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 . + */ +#pragma once + +#include +#include +#include +#include +#include + +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]; +}; diff --git a/libraries/AP_Beacon/AP_Beacon_Backend.cpp b/libraries/AP_Beacon/AP_Beacon_Backend.cpp new file mode 100644 index 0000000000..4241edb880 --- /dev/null +++ b/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 . + */ + +#include "AP_Beacon_Backend.h" +// debug +#include + +/* + 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; +} diff --git a/libraries/AP_Beacon/AP_Beacon_Backend.h b/libraries/AP_Beacon/AP_Beacon_Backend.h new file mode 100644 index 0000000000..05bb33c1d1 --- /dev/null +++ b/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 . + */ +#pragma once + +#include +#include +#include +#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); +}; diff --git a/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp b/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp new file mode 100644 index 0000000000..ca6966883a --- /dev/null +++ b/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 . + */ + +#include +#include "AP_Beacon_Pozyx.h" +#include +#include + +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