#include <chrono>
+#include <GHFilter.h>
#include <uwb-def.h>
#define TECH_UNKNOWN 0x00
Node(int pan_id, unsigned long long node_id, int x, int y, int z, int range, int aoa, int pdoa) :
_distance(0), _pan_id(pan_id), _node_id(node_id), _x(x), _y(y), _z(z),
- _range(range), _aoa(aoa), _pdoa(pdoa),
+ _range(range), _filtered_range(), _aoa(aoa), _filtered_aoa(), _pdoa(pdoa),
_is_remote(false), _is_calculated(false), _tech(TECH_UNKNOWN),
- _last_update(std::chrono::steady_clock::now()) {};
+ _last_update(std::chrono::steady_clock::now()) {
+ _filtered_range.setState(range);
+ _filtered_aoa.setState(aoa);
+ };
Node(uwb_node_s *node) :
_distance(node->distance), _pan_id(node->pan_id), _node_id(node->node_id),
- _x(node->x), _y(node->y), _z(node->z), _range(node->range), _aoa(node->aoa), _pdoa(node->pdoa),
+ _x(node->x), _y(node->y), _z(node->z), _range(node->range), _filtered_range(),
+ _aoa(node->aoa), _filtered_aoa(), _pdoa(node->pdoa),
_is_remote(node->is_remote), _is_calculated(false), _tech(TECH_UWB),
- _last_update(std::chrono::steady_clock::now()){};
+ _last_update(std::chrono::steady_clock::now()) {
+ _filtered_range.setState(node->range);
+ _filtered_aoa.setState(node->aoa);
+ };
Node(Node *node) :
_distance(node->_distance), _pan_id(node->_pan_id), _node_id(node->_node_id),
- _x(node->_x), _y(node->_y), _z(node->_z), _range(node->_range), _aoa(node->_aoa), _pdoa(node->_pdoa),
+ _x(node->_x), _y(node->_y), _z(node->_z), _range(node->_range), _filtered_range(node->_filtered_range),
+ _aoa(node->_aoa), _filtered_aoa(node->_filtered_aoa), _pdoa(node->_pdoa),
_is_remote(node->_is_remote), _is_calculated(node->_is_calculated), _tech(node->_tech),
- _last_update(node->_last_update){};
+ _last_update(node->_last_update) {};
//~Node(){UWB_LOGI("%llu removed", _node_id);}
Node(const Node &node) = default;
int getZ(void) {return _z;};
void setZ(int z) {_z = z;};
int getRange(void) {return _range;};
+ float getFilteredRange(void) { return _filtered_range.getState(); };
void setRange(int range) {_range = range;};
int getAoa(void) {return _aoa;};
+ float getFilteredAoa(void) { return _filtered_aoa.getState(); };
void setAoa(int aoa) {_aoa = aoa;};
int getPdoa(void) {return _pdoa;};
void setPdoa(int pdoa) {_pdoa = pdoa;};
std::chrono::steady_clock::time_point getLastUpdate(void) {return _last_update;};
void setLastUpdate(std::chrono::steady_clock::time_point update_time) {_last_update = update_time;};
bool hasSamePosition(Node *node) {return (_x == node->_x && _y == node->_y && _z == node->_z);};
+ void updateFilters(void) {
+ _filtered_range.predict();
+ _filtered_aoa.predict();
+
+ _filtered_range.update(_range);
+ _filtered_aoa.update(_aoa);
+ }
private:
unsigned long long _distance;
int _pan_id;
int _y;
int _z;
int _range;
+ GHFilter _filtered_range;
int _aoa;
+ GHFilter _filtered_aoa;
int _pdoa;
bool _is_remote;
bool _is_calculated;
Name: uwb-manager
Summary: This is the daemon managing UWB related functionalities
-Version: 0.0.5
+Version: 0.0.6
Release: 1
Group: Network & Connectivity/Wireless
License: Apache-2.0
INCLUDE_DIRECTORIES(
${CMAKE_SOURCE_DIR}/include
${CMAKE_SOURCE_DIR}/interface
+ ${CMAKE_SOURCE_DIR}/src/filters
)
FILE(GLOB SRCS *.cpp)
Node &LocationManager::getCurrentPosition(void)
{
+ __UWB_LOG_FUNC_ENTER__;
if (!this->_mqtt_context->isConnected())
this->updateUwbPosition();
+ __UWB_LOG_FUNC_EXIT__;
return _current;
}
std::map<int, std::unique_ptr<Node>> &LocationManager::getNodeMap(void)
{
+ __UWB_LOG_FUNC_ENTER__;
if (!this->_mqtt_context->isConnected())
this->updateUwbPosition();
+ __UWB_LOG_FUNC_EXIT__;
return _node_map;
}
for (; it != uwb_node_list->end(); ++it) {
Node *p_node = (*it).get();
- auto itr = this->_node_map.find(p_node->getNodeId());
- bool added = itr == this->_node_map.end();
-
- p_node->setLastUpdate(std::chrono::steady_clock::now());
- this->_node_map[p_node->getNodeId()] =
- std::unique_ptr<Node>((Node *)new Node{p_node});
-
- if (added) {
- if (this->_node_added_cb)
- this->_node_added_cb(p_node);
- } else {
- if (this->_node_updated_cb)
- this->_node_updated_cb(p_node);
+ auto itr = this->_node_map.find(p_node->getNodeId());
+ bool added = itr == this->_node_map.end();
+
+ p_node->setLastUpdate(std::chrono::steady_clock::now());
+ p_node->updateFilters();
+ this->_node_map[p_node->getNodeId()] =
+ std::unique_ptr<Node>((Node *)new Node{p_node});
+
+ if (added) {
+ if (this->_node_added_cb)
+ this->_node_added_cb(p_node);
+ } else {
+ if (this->_node_updated_cb)
+ this->_node_updated_cb(p_node);
+ }
}
- }
}
void LocationManager::removeOutdatedUwbNodes(void)
*/
for (auto itr = this->_node_map.begin(); itr != this->_node_map.end();) {
- Node *node_ptr = itr->second.get();
- if (std::chrono::duration_cast<std::chrono::milliseconds>(current_mil - node_ptr->getLastUpdate()).count()
- > LocationManager::_default_remove_duration.count()) {
- if (this->_node_removed_cb)
- this->_node_removed_cb(node_ptr);
- itr = this->_node_map.erase(itr);
+ Node *node_ptr = itr->second.get();
+ if (std::chrono::duration_cast<std::chrono::milliseconds>(current_mil - node_ptr->getLastUpdate()).count()
+ > LocationManager::_default_remove_duration.count()) {
+ if (this->_node_removed_cb)
+ this->_node_removed_cb(node_ptr);
+ itr = this->_node_map.erase(itr);
} else
++itr;
}
int UwbPosition::updateUwbOwnNode()
{
- if (this->_uwb_hal == nullptr)
+ __UWB_LOG_FUNC_ENTER__;
+ if (this->_uwb_hal == nullptr) {
+ __UWB_LOG_FUNC_EXIT__;
return 0;
+ }
uwb_node_s *own_node;
int res = this->_uwb_hal->getOwnNode(&own_node);
- if (res != 0 || !own_node)
+ if (res != 0 || !own_node) {
+ __UWB_LOG_FUNC_EXIT__;
return -1;
+ }
this->setOwnNode(Node{own_node});
free(own_node);
+ __UWB_LOG_FUNC_EXIT__;
return 0;
}
--- /dev/null
+/*
+ * Copyright (c) 2021 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __GH_FILTER_H__
+#define __GH_FILTER_H__
+
+class GHFilter {
+public:
+
+ void reset(void) {
+ _state = 0;
+ _P = 0;
+ }
+
+ void setState(float state) {
+ _state = state;
+ return;
+ };
+
+ void setProcessVariance(float P) {
+ _P = P;
+ return;
+ };
+
+ //By system Modeling, P means rate but it expressed as matrix due to
+ //abstract interface
+ void predict(void) {
+ // System Modeling: X(n) = X(n - 1) + P * dt
+ _state = _state + _P * _time_delta;
+
+ //According to System modeling, there's no rate change in prediction
+ };
+
+ void update(float measurement) {
+
+ float residual = measurement - _state;
+
+ //h is the scaling for the change in measurement over time
+ _P = _P + _h * residual / _time_delta;
+
+ //g is the scaling we used for the measurement
+ _state = _state + _g * residual;
+ return;
+ };
+
+ float getState(void) { return _state; };
+private:
+ float _state;
+ float _P;
+
+ //Temporal value for g, h from https://en.wikipedia.org/wiki/Alpha_beta_filter
+ static constexpr float _g = 0.85;
+ static constexpr float _h = 0.005;
+ static constexpr float _time_delta = 1.0;
+};
+
+#endif /* __GH_FILTER_H__ */
projects / platform / core / connectivity / uwb-manager.git / commitdiff