yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

local_position_estimator: move to WQ and delete unused SubscriptionPollable

Browse Source
This commit is contained in:
Daniel Agar
2019-09-30 10:38:40 -04:00
parent db69ff0a6e
commit f271efaf62
19 changed files with 372 additions and 816 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/roboclaw/RoboClaw.hpp
View File

@@ -47,7 +47,6 @@
#include <stdio.h>
#include <termios.h>
#include <lib/parameters/param.h>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wheel_encoders.h>
#include <uORB/topics/actuator_armed.h>

39
src/lib/controllib/block/Block.cpp
View File

@@ -42,8 +42,6 @@
#include <cstring>
#include <uORB/SubscriptionPollable.hpp>
namespace control
{
@@ -99,24 +97,6 @@ void Block::updateParams()
updateParamsSubclass();
}
void Block::updateSubscriptions()
{
uORB::SubscriptionPollableNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != nullptr) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
PX4_ERR("exceeded max subscriptions for block: %s", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
void SuperBlock::setDt(float dt)
{
Block::setDt(dt);
@@ -154,25 +134,6 @@ void SuperBlock::updateChildParams()
}
}
void SuperBlock::updateChildSubscriptions()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != nullptr) {
if (count++ > maxChildrenPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
PX4_ERR("exceeded max children for block: %s", name);
break;
}
child->updateSubscriptions();
child = child->getSibling();
}
}
} // namespace control
template class List<uORB::SubscriptionPollableNode *>;
template class List<control::BlockParamBase *>;

14
src/lib/controllib/block/Block.hpp
View File

@@ -40,15 +40,14 @@
#pragma once
#include <containers/List.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <controllib/block/BlockParam.hpp>
#include <cstdint>
namespace control
{
static constexpr uint8_t maxChildrenPerBlock = 100;
static constexpr uint8_t maxParamsPerBlock = 110;
static constexpr uint8_t maxSubscriptionsPerBlock = 100;
static constexpr uint8_t blockNameLengthMax = 40;
// forward declaration
@@ -74,7 +73,6 @@ public:
void getName(char *name, size_t n);
virtual void updateParams();
virtual void updateSubscriptions();
virtual void setDt(float dt) { _dt = dt; }
float getDt() { return _dt; }
@@ -84,14 +82,12 @@ protected:
virtual void updateParamsSubclass() {}
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionPollableNode *> &getSubscriptions() { return _subscriptions; }
List<BlockParamBase *> &getParams() { return _params; }
const char *_name;
SuperBlock *_parent;
float _dt{0.0f};
List<uORB::SubscriptionPollableNode *> _subscriptions;
List<BlockParamBase *> _params;
};
@@ -119,17 +115,9 @@ public:
if (getChildren().getHead() != nullptr) { updateChildParams(); }
}
void updateSubscriptions() override
{
Block::updateSubscriptions();
if (getChildren().getHead() != nullptr) { updateChildSubscriptions(); }
}
protected:
List<Block *> &getChildren() { return _children; }
void updateChildParams();
void updateChildSubscriptions();
List<Block *> _children;
};

338
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@@ -18,36 +18,12 @@ static const float LAND_RATE = 10.0f; // rate of land detector correction
static const char *msg_label = "[lpe] "; // rate of land detector correction
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
// this block has no parent, and has name LPE
SuperBlock(nullptr, "LPE"),
ModuleParams(nullptr),
// subscriptions, set rate, add to list
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
_sub_angular_velocity(ORB_ID(vehicle_angular_velocity), 1000 / 100, 0, &getSubscriptions()),
// set flow max update rate higher than expected to we don't lose packets
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
// main prediction loop, 100 hz
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
// status updates 2 hz
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
// gps 10 hz
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
// vision 50 hz
_sub_visual_odom(ORB_ID(vehicle_visual_odometry), 1000 / 50, 0, &getSubscriptions()),
// mocap 50 hz
_sub_mocap_odom(ORB_ID(vehicle_mocap_odometry), 1000 / 50, 0, &getSubscriptions()),
// all distance sensors, 10 hz
_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
_dist_subs(),
_sub_lidar(nullptr),
_sub_sonar(nullptr),
_sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),
_sub_airdata(ORB_ID(vehicle_air_data), 0, 0, &getSubscriptions()),
// map projection
_map_ref(),
@@ -66,6 +42,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// low pass
_xLowPass(this, "X_LP"),
// use same lp constant for agl
_aglLowPass(this, "X_LP"),
@@ -74,7 +51,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_tDelay(this, ""),
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_origin(0),
_timeStampLastBaro(hrt_absolute_time()),
@@ -135,22 +111,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_ref_lon(0.0),
_ref_alt(0.0)
{
_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz
// assign distance subs to array
_dist_subs[0] = &_sub_dist0;
_dist_subs[1] = &_sub_dist1;
_dist_subs[2] = &_sub_dist2;
_dist_subs[3] = &_sub_dist3;
// setup event triggering based on new flow messages to integrate
_polls[POLL_FLOW].fd = _sub_flow.getHandle();
_polls[POLL_FLOW].events = POLLIN;
_polls[POLL_PARAM].fd = _sub_param_update.getHandle();
_polls[POLL_PARAM].events = POLLIN;
_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
_polls[POLL_SENSORS].events = POLLIN;
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
@@ -160,33 +128,50 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_map_ref.init_done = false;
// print fusion settings to console
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_param_lpe_fusion.get() & FUSE_GPS) != 0,
(_param_lpe_fusion.get() & FUSE_FLOW) != 0,
(_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
(_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
(_param_lpe_fusion.get() & FUSE_LAND) != 0,
(_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_param_lpe_fusion.get() & FUSE_BARO) != 0);
PX4_INFO("fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_param_lpe_fusion.get() & FUSE_GPS) != 0,
(_param_lpe_fusion.get() & FUSE_FLOW) != 0,
(_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
(_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
(_param_lpe_fusion.get() & FUSE_LAND) != 0,
(_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_param_lpe_fusion.get() & FUSE_BARO) != 0);
}
bool
BlockLocalPositionEstimator::init()
{
if (!_sensors_sub.registerCallback()) {
PX4_ERR("sensor combined callback registration failed!");
return false;
}
return true;
}
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
float t,
const Vector<float, BlockLocalPositionEstimator::n_x> &x,
const Vector<float, BlockLocalPositionEstimator::n_u> &u)
{
return _A * x + _B * u;
return m_A * x + m_B * u;
}
void BlockLocalPositionEstimator::update()
void BlockLocalPositionEstimator::Run()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (should_exit()) {
_sensors_sub.unregisterCallback();
exit_and_cleanup();
return;
}
if (ret < 0) {
sensor_combined_s imu;
if (!_sensors_sub.update(&imu)) {
return;
}
@@ -198,17 +183,17 @@ void BlockLocalPositionEstimator::update()
setDt(dt);
// auto-detect connected rangefinders while not armed
_sub_armed.update();
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
// detect distance sensors
for (size_t i = 0; i < N_DIST_SUBS; i++) {
uORB::SubscriptionPollable<distance_sensor_s> *s = _dist_subs[i];
auto *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar) { continue; }
if (s->updated()) {
s->update();
if (s->update()) {
if (s->get().timestamp == 0) { continue; }
@@ -257,27 +242,28 @@ void BlockLocalPositionEstimator::update()
_lastArmedState = armedState;
// see which updates are available
bool paramsUpdated = _sub_param_update.updated();
bool paramsUpdated = _sub_param_update.update();
_baroUpdated = false;
if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.update()) {
if (_sub_airdata.get().timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = _sub_airdata.get().timestamp;
}
}
_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.updated();
_visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
_mocapUpdated = _sub_mocap_odom.updated();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.update();
_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.update();
_visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.update();
_mocapUpdated = _sub_mocap_odom.update();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->update();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->update();
_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
bool targetPositionUpdated = _sub_landing_target_pose.updated();
bool targetPositionUpdated = _sub_landing_target_pose.update();
// get new data
updateSubscriptions();
_sub_att.update();
_sub_angular_velocity.update();
// update parameters
if (paramsUpdated) {
@@ -289,7 +275,7 @@ void BlockLocalPositionEstimator::update()
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
if (math::max(m_P(X_vx, X_vx), m_P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
vxy_stddev_ok = true;
}
@@ -314,7 +300,7 @@ void BlockLocalPositionEstimator::update()
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _param_lpe_z_pub.get();
bool z_stddev_ok = sqrtf(m_P(X_z, X_z)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_Z) {
// if valid and baro has timed out, set to not valid
@@ -329,7 +315,7 @@ void BlockLocalPositionEstimator::update()
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
bool tz_stddev_ok = sqrtf(m_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_TZ) {
if (!tz_stddev_ok) {
@@ -385,7 +371,7 @@ void BlockLocalPositionEstimator::update()
for (size_t i = 0; i < n_x; i++) {
for (size_t j = 0; j <= i; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
if (!PX4_ISFINITE(m_P(i, j))) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) not finite", msg_label, i, j);
reinit_P = true;
@@ -393,7 +379,7 @@ void BlockLocalPositionEstimator::update()
if (i == j) {
// make sure diagonal elements are positive
if (_P(i, i) <= 0) {
if (m_P(i, i) <= 0) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) negative", msg_label, i, j);
reinit_P = true;
@@ -402,7 +388,7 @@ void BlockLocalPositionEstimator::update()
} else {
// copy elememnt from upper triangle to force
// symmetry
_P(j, i) = _P(i, j);
m_P(j, i) = m_P(i, j);
}
if (reinit_P) { break; }
@@ -416,7 +402,7 @@ void BlockLocalPositionEstimator::update()
}
// do prediction
predict();
predict(imu);
// sensor corrections/ initializations
if (_gpsUpdated) {
@@ -545,6 +531,8 @@ bool BlockLocalPositionEstimator::landed()
return false;
}
_sub_land.update();
bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall);
return _sub_land.get().landed || disarmed_not_falling;
@@ -555,10 +543,10 @@ void BlockLocalPositionEstimator::publishLocalPos()
const Vector<float, n_x> &xLP = _xLowPass.getState();
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float evv = sqrtf(_P(X_vz, X_vz));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float epv = sqrtf(_P(X_z, X_z));
float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy));
float evv = sqrtf(m_P(X_vz, X_vz));
float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y));
float epv = sqrtf(m_P(X_z, X_z));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
@@ -679,9 +667,9 @@ void BlockLocalPositionEstimator::publishOdom()
}
// set the position variances
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = _P(X_vx, X_vx);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = _P(X_vy, X_vy);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = _P(X_vz, X_vz);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = m_P(X_vx, X_vx);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = m_P(X_vy, X_vy);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = m_P(X_vz, X_vz);
// unknown orientation covariances
// TODO: add orientation covariance to vehicle_attitude
@@ -695,9 +683,9 @@ void BlockLocalPositionEstimator::publishOdom()
}
// set the linear velocity variances
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = _P(X_vx, X_vx);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = _P(X_vy, X_vy);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = _P(X_vz, X_vz);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = m_P(X_vx, X_vx);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = m_P(X_vy, X_vy);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = m_P(X_vz, X_vz);
// unknown angular velocity covariances
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN;
@@ -723,21 +711,21 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
_pub_est_status.get().covariances[2] = NAN;
_pub_est_status.get().covariances[3] = NAN;
// linear velocity
_pub_est_status.get().covariances[4] = _P(X_vx, X_vx);
_pub_est_status.get().covariances[5] = _P(X_vy, X_vy);
_pub_est_status.get().covariances[6] = _P(X_vz, X_vz);
_pub_est_status.get().covariances[4] = m_P(X_vx, X_vx);
_pub_est_status.get().covariances[5] = m_P(X_vy, X_vy);
_pub_est_status.get().covariances[6] = m_P(X_vz, X_vz);
// position
_pub_est_status.get().covariances[7] = _P(X_x, X_x);
_pub_est_status.get().covariances[8] = _P(X_y, X_y);
_pub_est_status.get().covariances[9] = _P(X_z, X_z);
_pub_est_status.get().covariances[7] = m_P(X_x, X_x);
_pub_est_status.get().covariances[8] = m_P(X_y, X_y);
_pub_est_status.get().covariances[9] = m_P(X_z, X_z);
// gyro bias - not determined
_pub_est_status.get().covariances[10] = NAN;
_pub_est_status.get().covariances[11] = NAN;
_pub_est_status.get().covariances[12] = NAN;
// accel bias
_pub_est_status.get().covariances[13] = _P(X_bx, X_bx);
_pub_est_status.get().covariances[14] = _P(X_by, X_by);
_pub_est_status.get().covariances[15] = _P(X_bz, X_bz);
_pub_est_status.get().covariances[13] = m_P(X_bx, X_bx);
_pub_est_status.get().covariances[14] = m_P(X_by, X_by);
_pub_est_status.get().covariances[15] = m_P(X_bz, X_bz);
// mag - not determined
for (size_t i = 16; i <= 21; i++) {
@@ -745,7 +733,7 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
}
// replacing the hor wind cov with terrain altitude covariance
_pub_est_status.get().covariances[22] = _P(X_tz, X_tz);
_pub_est_status.get().covariances[22] = m_P(X_tz, X_tz);
_pub_est_status.get().covariances[23] = NAN;
_pub_est_status.get().n_states = n_x;
@@ -767,9 +755,9 @@ void BlockLocalPositionEstimator::publishGlobalPos()
float alt = -xLP(X_z) + _altOrigin;
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float epv = sqrtf(_P(X_z, X_z));
float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy));
float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y));
float epv = sqrtf(m_P(X_z, X_z));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
@@ -806,20 +794,20 @@ void BlockLocalPositionEstimator::publishGlobalPos()
void BlockLocalPositionEstimator::initP()
{
_P.setZero();
m_P.setZero();
// initialize to twice valid condition
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
m_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
m_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
m_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
m_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
m_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
// use vxy thresh for vz init as well
_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
m_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
// initialize bias uncertainty to small values to keep them stable
_P(X_bx, X_bx) = 1e-6;
_P(X_by, X_by) = 1e-6;
_P(X_bz, X_bz) = 1e-6;
_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
m_P(X_bx, X_bx) = 1e-6;
m_P(X_by, X_by) = 1e-6;
m_P(X_bz, X_bz) = 1e-6;
m_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
}
void BlockLocalPositionEstimator::initSS()
@@ -827,17 +815,17 @@ void BlockLocalPositionEstimator::initSS()
initP();
// dynamics matrix
_A.setZero();
m_A.setZero();
// derivative of position is velocity
_A(X_x, X_vx) = 1;
_A(X_y, X_vy) = 1;
_A(X_z, X_vz) = 1;
m_A(X_x, X_vx) = 1;
m_A(X_y, X_vy) = 1;
m_A(X_z, X_vz) = 1;
// input matrix
_B.setZero();
_B(X_vx, U_ax) = 1;
_B(X_vy, U_ay) = 1;
_B(X_vz, U_az) = 1;
m_B.setZero();
m_B(X_vx, U_ax) = 1;
m_B(X_vy, U_ay) = 1;
m_B(X_vz, U_az) = 1;
// update components that depend on current state
updateSSStates();
@@ -848,58 +836,58 @@ void BlockLocalPositionEstimator::updateSSStates()
{
// derivative of velocity is accelerometer acceleration
// (in input matrix) - bias (in body frame)
_A(X_vx, X_bx) = -_R_att(0, 0);
_A(X_vx, X_by) = -_R_att(0, 1);
_A(X_vx, X_bz) = -_R_att(0, 2);
m_A(X_vx, X_bx) = -_R_att(0, 0);
m_A(X_vx, X_by) = -_R_att(0, 1);
m_A(X_vx, X_bz) = -_R_att(0, 2);
_A(X_vy, X_bx) = -_R_att(1, 0);
_A(X_vy, X_by) = -_R_att(1, 1);
_A(X_vy, X_bz) = -_R_att(1, 2);
m_A(X_vy, X_bx) = -_R_att(1, 0);
m_A(X_vy, X_by) = -_R_att(1, 1);
m_A(X_vy, X_bz) = -_R_att(1, 2);
_A(X_vz, X_bx) = -_R_att(2, 0);
_A(X_vz, X_by) = -_R_att(2, 1);
_A(X_vz, X_bz) = -_R_att(2, 2);
m_A(X_vz, X_bx) = -_R_att(2, 0);
m_A(X_vz, X_by) = -_R_att(2, 1);
m_A(X_vz, X_bz) = -_R_att(2, 2);
}
void BlockLocalPositionEstimator::updateSSParams()
{
// input noise covariance matrix
_R.setZero();
_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();
m_R.setZero();
m_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
m_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
m_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();
// process noise power matrix
_Q.setZero();
m_Q.setZero();
float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get();
_Q(X_x, X_x) = pn_p_sq;
_Q(X_y, X_y) = pn_p_sq;
_Q(X_z, X_z) = pn_p_sq;
_Q(X_vx, X_vx) = pn_v_sq;
_Q(X_vy, X_vy) = pn_v_sq;
_Q(X_vz, X_vz) = pn_v_sq;
m_Q(X_x, X_x) = pn_p_sq;
m_Q(X_y, X_y) = pn_p_sq;
m_Q(X_z, X_z) = pn_p_sq;
m_Q(X_vx, X_vx) = pn_v_sq;
m_Q(X_vy, X_vy) = pn_v_sq;
m_Q(X_vz, X_vz) = pn_v_sq;
// technically, the noise is in the body frame,
// but the components are all the same, so
// ignoring for now
float pn_b_sq = _param_lpe_pn_b.get() * _param_lpe_pn_b.get();
_Q(X_bx, X_bx) = pn_b_sq;
_Q(X_by, X_by) = pn_b_sq;
_Q(X_bz, X_bz) = pn_b_sq;
m_Q(X_bx, X_bx) = pn_b_sq;
m_Q(X_by, X_by) = pn_b_sq;
m_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
float pn_t_noise_density =
_param_lpe_pn_t.get() +
(_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
void BlockLocalPositionEstimator::predict()
void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
{
// get acceleration
_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
Vector3f a(imu.accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += CONSTANTS_ONE_G; // add g
@@ -959,12 +947,12 @@ void BlockLocalPositionEstimator::predict()
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
Matrix<float, n_x, n_x> dP = (m_A * m_P + m_P * m_A.transpose() +
m_B * m_R * m_B.transpose() + m_Q) * getDt();
// covariance propagation logic
for (size_t i = 0; i < n_x; i++) {
if (_P(i, i) > P_MAX) {
if (m_P(i, i) > P_MAX) {
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
@@ -975,7 +963,7 @@ void BlockLocalPositionEstimator::predict()
}
}
_P += dP;
m_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
@@ -1002,3 +990,59 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
return OK;
}
int
BlockLocalPositionEstimator::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
BlockLocalPositionEstimator::task_spawn(int argc, char *argv[])
{
BlockLocalPositionEstimator *instance = new BlockLocalPositionEstimator();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int
BlockLocalPositionEstimator::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Attitude and position estimator using an Extended Kalman Filter.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[])
{
return BlockLocalPositionEstimator::main(argc, argv);
}

282
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@@ -1,15 +1,17 @@
#pragma once
#include <px4_platform_common/posix.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <controllib/blocks.hpp>
#include <mathlib/mathlib.h>
#include <px4_platform_common/posix.h>
#include <lib/controllib/blocks.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <matrix/Matrix.hpp>
// uORB Subscriptions
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@@ -36,6 +38,7 @@
using namespace matrix;
using namespace control;
using namespace time_literals;
static const float DELAY_MAX = 0.5f; // seconds
static const float HIST_STEP = 0.05f; // 20 hz
@@ -56,7 +59,8 @@ static const float BETA_TABLE[7] = {0,
19.6465647819,
};
class BlockLocalPositionEstimator : public control::SuperBlock, public ModuleParams
class BlockLocalPositionEstimator : public ModuleBase<BlockLocalPositionEstimator>, public ModuleParams,
public px4::WorkItem, public control::SuperBlock
{
// dynamics:
//
@@ -106,8 +110,21 @@ class BlockLocalPositionEstimator : public control::SuperBlock, public ModulePar
//
public:
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &) = delete;
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &) = delete;
BlockLocalPositionEstimator();
~BlockLocalPositionEstimator() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
private:
// constants
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
@@ -121,7 +138,6 @@ public:
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
enum {Y_target_x = 0, Y_target_y, n_y_target};
enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
enum {
FUSE_GPS = 1 << 0,
FUSE_FLOW = 1 << 1,
@@ -151,12 +167,7 @@ public:
EST_TZ = 1 << 2,
};
// public methods
BlockLocalPositionEstimator();
void update();
virtual ~BlockLocalPositionEstimator() = default;
private:
void Run() override;
// methods
// ----------------------------
@@ -171,7 +182,7 @@ private:
void updateSSParams();
// predict the next state
void predict();
void predict(const sensor_combined_s &imu);
// lidar
int lidarMeasure(Vector<float, n_y_lidar> &y);
@@ -248,25 +259,26 @@ private:
// ----------------------------
// subscriptions
uORB::SubscriptionPollable<actuator_armed_s> _sub_armed;
uORB::SubscriptionPollable<vehicle_land_detected_s> _sub_land;
uORB::SubscriptionPollable<vehicle_attitude_s> _sub_att;
uORB::SubscriptionPollable<vehicle_angular_velocity_s> _sub_angular_velocity;
uORB::SubscriptionPollable<optical_flow_s> _sub_flow;
uORB::SubscriptionPollable<sensor_combined_s> _sub_sensor;
uORB::SubscriptionPollable<parameter_update_s> _sub_param_update;
uORB::SubscriptionPollable<vehicle_gps_position_s> _sub_gps;
uORB::SubscriptionPollable<vehicle_odometry_s> _sub_visual_odom;
uORB::SubscriptionPollable<vehicle_odometry_s> _sub_mocap_odom;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist0;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist1;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist2;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist3;
uORB::SubscriptionPollable<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
uORB::SubscriptionPollable<distance_sensor_s> *_sub_lidar;
uORB::SubscriptionPollable<distance_sensor_s> *_sub_sonar;
uORB::SubscriptionPollable<landing_target_pose_s> _sub_landing_target_pose;
uORB::SubscriptionPollable<vehicle_air_data_s> _sub_airdata;
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionData<actuator_armed_s> _sub_armed{ORB_ID(actuator_armed)};
uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
uORB::SubscriptionData<vehicle_angular_velocity_s> _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionData<optical_flow_s> _sub_flow{ORB_ID(optical_flow)};
uORB::SubscriptionData<parameter_update_s> _sub_param_update{ORB_ID(parameter_update)};
uORB::SubscriptionData<vehicle_gps_position_s> _sub_gps{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_odometry_s> _sub_visual_odom{ORB_ID(vehicle_visual_odometry)};
uORB::SubscriptionData<vehicle_odometry_s> _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)};
uORB::SubscriptionData<distance_sensor_s> _sub_dist0{ORB_ID(distance_sensor), 0};
uORB::SubscriptionData<distance_sensor_s> _sub_dist1{ORB_ID(distance_sensor), 1};
uORB::SubscriptionData<distance_sensor_s> _sub_dist2{ORB_ID(distance_sensor), 2};
uORB::SubscriptionData<distance_sensor_s> _sub_dist3{ORB_ID(distance_sensor), 3};
uORB::SubscriptionData<distance_sensor_s> *_dist_subs[N_DIST_SUBS] {};
uORB::SubscriptionData<distance_sensor_s> *_sub_lidar{nullptr};
uORB::SubscriptionData<distance_sensor_s> *_sub_sonar{nullptr};
uORB::SubscriptionData<landing_target_pose_s> _sub_landing_target_pose{ORB_ID(landing_target_pose)};
uORB::SubscriptionData<vehicle_air_data_s> _sub_airdata{ORB_ID(vehicle_air_data)};
// publications
uORB::PublicationData<vehicle_local_position_s> _pub_lpos{ORB_ID(vehicle_local_position)};
@@ -278,6 +290,107 @@ private:
// map projection
struct map_projection_reference_s _map_ref;
// target mode paramters from landing_target_estimator module
enum TargetMode {
Target_Moving = 0,
Target_Stationary = 1
};
// flow gyro filter
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
// stats
BlockStats<float, n_y_baro> _baroStats;
BlockStats<float, n_y_sonar> _sonarStats;
BlockStats<float, n_y_lidar> _lidarStats;
BlockStats<float, 1> _flowQStats;
BlockStats<float, n_y_vision> _visionStats;
BlockStats<float, n_y_mocap> _mocapStats;
BlockStats<double, n_y_gps> _gpsStats;
uint16_t _landCount;
// low pass
BlockLowPassVector<float, n_x> _xLowPass;
BlockLowPass _aglLowPass;
// delay blocks
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
// misc
uint64_t _timeStamp;
uint64_t _time_origin;
uint64_t _timeStampLastBaro;
uint64_t _time_last_hist;
uint64_t _time_last_flow;
uint64_t _time_last_baro;
uint64_t _time_last_gps;
uint64_t _time_last_lidar;
uint64_t _time_last_sonar;
uint64_t _time_init_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
uint64_t _time_last_land;
uint64_t _time_last_target;
// reference altitudes
float _altOrigin;
bool _altOriginInitialized;
bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
float _baroAltOrigin;
float _gpsAltOrigin;
// status
bool _receivedGps;
bool _lastArmedState;
// masks
uint16_t _sensorTimeout;
uint16_t _sensorFault;
uint8_t _estimatorInitialized;
// sensor update flags
bool _flowUpdated;
bool _gpsUpdated;
bool _visionUpdated;
bool _mocapUpdated;
bool _lidarUpdated;
bool _sonarUpdated;
bool _landUpdated;
bool _baroUpdated;
// sensor validation flags
bool _vision_xy_valid;
bool _vision_z_valid;
bool _mocap_xy_valid;
bool _mocap_z_valid;
// sensor std deviations
float _vision_eph;
float _vision_epv;
float _mocap_eph;
float _mocap_epv;
// local to global coversion related variables
bool _is_global_cov_init;
uint64_t _global_ref_timestamp;
double _ref_lat;
double _ref_lon;
float _ref_alt;
// state space
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> m_P; // state covariance matrix
matrix::Dcm<float> _R_att;
Matrix<float, n_x, n_x> m_A; // dynamics matrix
Matrix<float, n_x, n_u> m_B; // input matrix
Matrix<float, n_u, n_u> m_R; // input covariance
Matrix<float, n_x, n_x> m_Q; // process noise covariance
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
@@ -346,105 +459,4 @@ private:
(ParamFloat<px4::params::LPE_LON>) _param_lpe_lon
)
// target mode paramters from landing_target_estimator module
enum TargetMode {
Target_Moving = 0,
Target_Stationary = 1
};
// flow gyro filter
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
// stats
BlockStats<float, n_y_baro> _baroStats;
BlockStats<float, n_y_sonar> _sonarStats;
BlockStats<float, n_y_lidar> _lidarStats;
BlockStats<float, 1> _flowQStats;
BlockStats<float, n_y_vision> _visionStats;
BlockStats<float, n_y_mocap> _mocapStats;
BlockStats<double, n_y_gps> _gpsStats;
uint16_t _landCount;
// low pass
BlockLowPassVector<float, n_x> _xLowPass;
BlockLowPass _aglLowPass;
// delay blocks
BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
// misc
px4_pollfd_struct_t _polls[3];
uint64_t _timeStamp;
uint64_t _time_origin;
uint64_t _timeStampLastBaro;
uint64_t _time_last_hist;
uint64_t _time_last_flow;
uint64_t _time_last_baro;
uint64_t _time_last_gps;
uint64_t _time_last_lidar;
uint64_t _time_last_sonar;
uint64_t _time_init_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
uint64_t _time_last_land;
uint64_t _time_last_target;
// reference altitudes
float _altOrigin;
bool _altOriginInitialized;
bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
float _baroAltOrigin;
float _gpsAltOrigin;
// status
bool _receivedGps;
bool _lastArmedState;
// masks
uint16_t _sensorTimeout;
uint16_t _sensorFault;
uint8_t _estimatorInitialized;
// sensor update flags
bool _flowUpdated;
bool _gpsUpdated;
bool _visionUpdated;
bool _mocapUpdated;
bool _lidarUpdated;
bool _sonarUpdated;
bool _landUpdated;
bool _baroUpdated;
// sensor validation flags
bool _vision_xy_valid;
bool _vision_z_valid;
bool _mocap_xy_valid;
bool _mocap_z_valid;
// sensor std deviations
float _vision_eph;
float _vision_epv;
float _mocap_eph;
float _mocap_epv;
// local to global coversion related variables
bool _is_global_cov_init;
uint64_t _global_ref_timestamp;
double _ref_lat;
double _ref_lon;
float _ref_alt;
// state space
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
matrix::Dcm<float> _R_att;
Matrix<float, n_x, n_x> _A; // dynamics matrix
Matrix<float, n_x, n_u> _B; // input matrix
Matrix<float, n_u, n_u> _R; // input covariance
Matrix<float, n_x, n_x> _Q; // process noise covariance
};

5
src/modules/local_position_estimator/CMakeLists.txt
View File

@@ -34,11 +34,9 @@ px4_add_module(
MODULE modules__local_position_estimator
MAIN local_position_estimator
COMPILE_FLAGS
-Wno-double-promotion # TODO: fix in Matrix library Vector::pow()
STACK_MAIN 5700
STACK_MAX 13000
STACK_MAX 3700
SRCS
local_position_estimator_main.cpp
BlockLocalPositionEstimator.cpp
sensors/flow.cpp
sensors/lidar.cpp
@@ -53,4 +51,5 @@ px4_add_module(
controllib
git_ecl
ecl_geo
px4_work_queue
)

140
src/modules/local_position_estimator/local_position_estimator_main.cpp
View File

@@ -1,140 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file local_position_estimator.cpp
* @author James Goppert <james.goppert@gmail.com>
* @author Mohammed Kabir
* @author Nuno Marques <n.marques21@hotmail.com>
*
* Local position estimator
*/
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/tasks.h>
#include "BlockLocalPositionEstimator.hpp"
extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]);
class LocalPositionEstimatorModule : public ModuleBase<LocalPositionEstimatorModule>
{
public:
~LocalPositionEstimatorModule() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static LocalPositionEstimatorModule *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
private:
BlockLocalPositionEstimator _estimator;
};
int LocalPositionEstimatorModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Attitude and position estimator using an Extended Kalman Filter.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int LocalPositionEstimatorModule::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int LocalPositionEstimatorModule::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("lp_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_ESTIMATOR,
7900,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
}
LocalPositionEstimatorModule *LocalPositionEstimatorModule::instantiate(int argc, char *argv[])
{
LocalPositionEstimatorModule *instance = new LocalPositionEstimatorModule();
if (instance == nullptr) {
PX4_ERR("alloc failed");
}
return instance;
}
void LocalPositionEstimatorModule::run()
{
while (!should_exit()) {
_estimator.update();
}
}
int local_position_estimator_main(int argc, char *argv[])
{
return LocalPositionEstimatorModule::main(argc, argv);
}

6
src/modules/local_position_estimator/sensors/baro.cpp
View File

@@ -68,7 +68,7 @@ void BlockLocalPositionEstimator::baroCorrect()
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
inv<float, n_y_baro>((C * m_P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
@@ -87,10 +87,10 @@ void BlockLocalPositionEstimator::baroCorrect()
}
// kalman filter correction always
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_baro> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::baroCheckTimeout()

6
src/modules/local_position_estimator/sensors/flow.cpp
View File

@@ -169,7 +169,7 @@ void BlockLocalPositionEstimator::flowCorrect()
Vector<float, 2> r = y - C * _x;
// residual covariance
Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose() + R;
Matrix<float, n_y_flow, n_y_flow> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().flow_innov[0] = r(0);
@@ -196,10 +196,10 @@ void BlockLocalPositionEstimator::flowCorrect()
if (!(_sensorFault & SENSOR_FLOW)) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
}

6
src/modules/local_position_estimator/sensors/gps.cpp
View File

@@ -186,7 +186,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
Vector<float, n_y_gps> r = y - C * x0;
// residual covariance
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 6; i++) {
@@ -217,10 +217,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
}
// kalman filter correction always for GPS
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_gps> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::gpsCheckTimeout()

6
src/modules/local_position_estimator/sensors/land.cpp
View File

@@ -59,7 +59,7 @@ void BlockLocalPositionEstimator::landCorrect()
R(Y_land_agl, Y_land_agl) = _param_lpe_land_z.get() * _param_lpe_land_z.get();
// residual
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * m_P * C.transpose()) + R);
Vector<float, n_y_land> r = y - C * _x;
_pub_innov.get().hagl_innov = r(Y_land_agl);
_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
@@ -85,10 +85,10 @@ void BlockLocalPositionEstimator::landCorrect()
}
// kalman filter correction always for land detector
Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_land> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::landCheckTimeout()

6
src/modules/local_position_estimator/sensors/landing_target.cpp
View File

@@ -82,7 +82,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
// residual covariance, (inverse)
Matrix<float, n_y_target, n_y_target> S_I =
inv<float, n_y_target>(C * _P * C.transpose() + R);
inv<float, n_y_target>(C * m_P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
@@ -103,10 +103,10 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
// kalman filter correction
Matrix<float, n_x, n_y_target> K =
_P * C.transpose() * S_I;
m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}

6
src/modules/local_position_estimator/sensors/lidar.cpp
View File

@@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
// residual
Vector<float, n_y_lidar> r = y - C * _x;
// residual covariance
Matrix<float, n_y_lidar, n_y_lidar> S = C * _P * C.transpose() + R;
Matrix<float, n_y_lidar, n_y_lidar> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().hagl_innov = r(0);
@@ -116,10 +116,10 @@ void BlockLocalPositionEstimator::lidarCorrect()
}
// kalman filter correction always
Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_lidar> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::lidarCheckTimeout()

9
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@@ -106,7 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", _mocap_eph, _mocap_epv);
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph,
(double)_mocap_epv);
return;
}
@@ -141,7 +142,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
// residual
Vector<float, n_y_mocap> r = y - C * _x;
// residual covariance
Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
Matrix<float, n_y_mocap, n_y_mocap> S = C * m_P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 3; i++) {
@@ -172,10 +173,10 @@ void BlockLocalPositionEstimator::mocapCorrect()
}
// kalman filter correction always
Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_mocap> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::mocapCheckTimeout()

6
src/modules/local_position_estimator/sensors/sonar.cpp
View File

@@ -109,7 +109,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
// residual
Vector<float, n_y_sonar> r = y - C * _x;
// residual covariance
Matrix<float, n_y_sonar, n_y_sonar> S = C * _P * C.transpose() + R;
Matrix<float, n_y_sonar, n_y_sonar> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().hagl_innov = r(0);
@@ -138,10 +138,10 @@ void BlockLocalPositionEstimator::sonarCorrect()
// kalman filter correction if no fault
if (!(_sensorFault & SENSOR_SONAR)) {
Matrix<float, n_x, n_y_sonar> K =
_P * C.transpose() * S_I;
m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
}

9
src/modules/local_position_estimator/sensors/vision.cpp
View File

@@ -111,7 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect()
Vector<float, n_y_vision> y;
if (visionMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", _vision_eph, _vision_epv);
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph,
(double)_vision_epv);
return;
}
@@ -158,7 +159,7 @@ void BlockLocalPositionEstimator::visionCorrect()
// residual
Matrix<float, n_y_vision, 1> r = y - C * x0;
// residual covariance
Matrix<float, n_y_vision, n_y_vision> S = C * _P * C.transpose() + R;
Matrix<float, n_y_vision, n_y_vision> S = C * m_P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 3; i++) {
@@ -190,10 +191,10 @@ void BlockLocalPositionEstimator::visionCorrect()
// kalman filter correction if no fault
if (!(_sensorFault & SENSOR_VISION)) {
Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_vision> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
}

2
src/modules/uORB/CMakeLists.txt
View File

@@ -48,8 +48,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
Subscription.hpp
SubscriptionCallback.hpp
SubscriptionInterval.hpp
SubscriptionPollable.cpp
SubscriptionPollable.hpp
uORB.cpp
uORB.h
uORBCommon.hpp

110
src/modules/uORB/SubscriptionPollable.cpp
View File

@@ -1,110 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SubscriptionPollable.cpp
*
*/
#include "SubscriptionPollable.hpp"
#include <px4_platform_common/defines.h>
namespace uORB
{
SubscriptionPollableBase::SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval,
unsigned instance) :
_meta(meta),
_instance(instance)
{
if (instance > 0) {
_handle = orb_subscribe_multi(_meta, instance);
} else {
_handle = orb_subscribe(_meta);
}
if (_handle < 0) {
PX4_ERR("%s sub failed", _meta->o_name);
}
if (interval > 0) {
orb_set_interval(_handle, interval);
}
}
bool SubscriptionPollableBase::updated()
{
bool isUpdated = false;
if (orb_check(_handle, &isUpdated) != PX4_OK) {
PX4_ERR("%s check failed", _meta->o_name);
}
return isUpdated;
}
bool SubscriptionPollableBase::update(void *data)
{
bool orb_updated = false;
if (updated()) {
if (orb_copy(_meta, _handle, data) != PX4_OK) {
PX4_ERR("%s copy failed", _meta->o_name);
} else {
orb_updated = true;
}
}
return orb_updated;
}
SubscriptionPollableBase::~SubscriptionPollableBase()
{
if (orb_unsubscribe(_handle) != PX4_OK) {
PX4_ERR("%s unsubscribe failed", _meta->o_name);
}
}
SubscriptionPollableNode::SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval,
unsigned instance,
List<SubscriptionPollableNode *> *list)
: SubscriptionPollableBase(meta, interval, instance)
{
if (list != nullptr) {
list->add(this);
}
}
} // namespace uORB

197
src/modules/uORB/SubscriptionPollable.hpp
View File

@@ -1,197 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SubscriptionPollable.hpp
*
*/
#pragma once
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
#include <px4_platform_common/defines.h>
namespace uORB
{
/**
* Base subscription wrapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT SubscriptionPollableBase
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param instance The instance for multi sub.
*/
SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
virtual ~SubscriptionPollableBase();
// no copy, assignment, move, move assignment
SubscriptionPollableBase(const SubscriptionPollableBase &) = delete;
SubscriptionPollableBase &operator=(const SubscriptionPollableBase &) = delete;
SubscriptionPollableBase(SubscriptionPollableBase &&) = delete;
SubscriptionPollableBase &operator=(SubscriptionPollableBase &&) = delete;
/**
* Check if there is a new update.
* */
bool updated();
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
bool update(void *data);
int getHandle() const { return _handle; }
const orb_metadata *getMeta() const { return _meta; }
unsigned getInstance() const { return _instance; }
protected:
const struct orb_metadata *_meta;
unsigned _instance;
int _handle{-1};
};
/**
* alias class name so it is clear that the base class
*/
typedef SubscriptionPollableBase SubscriptionPollableTiny;
/**
* The subscription base class as a list node.
*/
class __EXPORT SubscriptionPollableNode : public SubscriptionPollableBase, public ListNode<SubscriptionPollableNode *>
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param instance The instance for multi sub.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
*/
SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
List<SubscriptionPollableNode *> *list = nullptr);
virtual ~SubscriptionPollableNode() override = default;
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual bool update() = 0;
/**
* Like update(), but does not check first if there is data available
*/
virtual bool forcedUpdate() = 0;
};
/**
* SubscriptionPollable wrapper class
*/
template<class T>
class __EXPORT SubscriptionPollable : public SubscriptionPollableNode
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param list A list interface for adding to
* list during construction
*/
SubscriptionPollable(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
List<SubscriptionPollableNode *> *list = nullptr):
SubscriptionPollableNode(meta, interval, instance, list),
_data() // initialize data structure to zero
{
forcedUpdate();
}
~SubscriptionPollable() override = default;
// no copy, assignment, move, move assignment
SubscriptionPollable(const SubscriptionPollable &) = delete;
SubscriptionPollable &operator=(const SubscriptionPollable &) = delete;
SubscriptionPollable(SubscriptionPollable &&) = delete;
SubscriptionPollable &operator=(SubscriptionPollable &&) = delete;
/**
* Create an update function that uses the embedded struct.
*/
bool update() override
{
return SubscriptionPollableBase::update((void *)(&_data));
}
bool forcedUpdate() override
{
return orb_copy(_meta, _handle, &_data) == PX4_OK;
}
/*
* This function gets the T struct data
* */
const T &get() const
{
return _data;
}
private:
T _data;
};
} // namespace uORB