diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 0fabd56553..b5d75069bb 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include diff --git a/src/lib/controllib/block/Block.cpp b/src/lib/controllib/block/Block.cpp index cfb10da0c8..32662d6cf1 100644 --- a/src/lib/controllib/block/Block.cpp +++ b/src/lib/controllib/block/Block.cpp @@ -42,8 +42,6 @@ #include -#include - 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; template class List; diff --git a/src/lib/controllib/block/Block.hpp b/src/lib/controllib/block/Block.hpp index 2f00878d1d..512303ac7f 100644 --- a/src/lib/controllib/block/Block.hpp +++ b/src/lib/controllib/block/Block.hpp @@ -40,15 +40,14 @@ #pragma once #include -#include #include +#include 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 &getSubscriptions() { return _subscriptions; } List &getParams() { return _params; } const char *_name; SuperBlock *_parent; float _dt{0.0f}; - List _subscriptions; List _params; }; @@ -119,17 +115,9 @@ public: if (getChildren().getHead() != nullptr) { updateChildParams(); } } - void updateSubscriptions() override - { - Block::updateSubscriptions(); - - if (getChildren().getHead() != nullptr) { updateChildSubscriptions(); } - } - protected: List &getChildren() { return _children; } void updateChildParams(); - void updateChildSubscriptions(); List _children; }; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 0451119b17..2dfb544c80 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -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 BlockLocalPositionEstimator::dynamics( float t, const Vector &x, const Vector &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 *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 &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(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 dP = (_A * _P + _P * _A.transpose() + - _B * _R * _B.transpose() + _Q) * getDt(); + Matrix 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); +} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 9fc6a9b7d1..9c0dfc9c25 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -1,15 +1,17 @@ #pragma once -#include #include +#include #include -#include -#include +#include +#include #include +#include #include // uORB Subscriptions -#include +#include +#include #include #include #include @@ -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, 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 &y); @@ -248,25 +259,26 @@ private: // ---------------------------- // subscriptions - uORB::SubscriptionPollable _sub_armed; - uORB::SubscriptionPollable _sub_land; - uORB::SubscriptionPollable _sub_att; - uORB::SubscriptionPollable _sub_angular_velocity; - uORB::SubscriptionPollable _sub_flow; - uORB::SubscriptionPollable _sub_sensor; - uORB::SubscriptionPollable _sub_param_update; - uORB::SubscriptionPollable _sub_gps; - uORB::SubscriptionPollable _sub_visual_odom; - uORB::SubscriptionPollable _sub_mocap_odom; - uORB::SubscriptionPollable _sub_dist0; - uORB::SubscriptionPollable _sub_dist1; - uORB::SubscriptionPollable _sub_dist2; - uORB::SubscriptionPollable _sub_dist3; - uORB::SubscriptionPollable *_dist_subs[N_DIST_SUBS]; - uORB::SubscriptionPollable *_sub_lidar; - uORB::SubscriptionPollable *_sub_sonar; - uORB::SubscriptionPollable _sub_landing_target_pose; - uORB::SubscriptionPollable _sub_airdata; + uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; + + uORB::SubscriptionData _sub_armed{ORB_ID(actuator_armed)}; + uORB::SubscriptionData _sub_land{ORB_ID(vehicle_land_detected)}; + uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionData _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)}; + uORB::SubscriptionData _sub_flow{ORB_ID(optical_flow)}; + uORB::SubscriptionData _sub_param_update{ORB_ID(parameter_update)}; + uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData _sub_visual_odom{ORB_ID(vehicle_visual_odometry)}; + uORB::SubscriptionData _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)}; + uORB::SubscriptionData _sub_dist0{ORB_ID(distance_sensor), 0}; + uORB::SubscriptionData _sub_dist1{ORB_ID(distance_sensor), 1}; + uORB::SubscriptionData _sub_dist2{ORB_ID(distance_sensor), 2}; + uORB::SubscriptionData _sub_dist3{ORB_ID(distance_sensor), 3}; + uORB::SubscriptionData *_dist_subs[N_DIST_SUBS] {}; + uORB::SubscriptionData *_sub_lidar{nullptr}; + uORB::SubscriptionData *_sub_sonar{nullptr}; + uORB::SubscriptionData _sub_landing_target_pose{ORB_ID(landing_target_pose)}; + uORB::SubscriptionData _sub_airdata{ORB_ID(vehicle_air_data)}; // publications uORB::PublicationData _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 _baroStats; + BlockStats _sonarStats; + BlockStats _lidarStats; + BlockStats _flowQStats; + BlockStats _visionStats; + BlockStats _mocapStats; + BlockStats _gpsStats; + uint16_t _landCount; + + // low pass + BlockLowPassVector _xLowPass; + BlockLowPass _aglLowPass; + + // delay blocks + BlockDelay _xDelay; + BlockDelay _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 _x; // state vector + Vector _u; // input vector + Matrix m_P; // state covariance matrix + + matrix::Dcm _R_att; + + Matrix m_A; // dynamics matrix + Matrix m_B; // input matrix + Matrix m_R; // input covariance + Matrix m_Q; // process noise covariance + DEFINE_PARAMETERS( (ParamInt) _param_sys_autostart, /**< example parameter */ @@ -346,105 +459,4 @@ private: (ParamFloat) _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 _baroStats; - BlockStats _sonarStats; - BlockStats _lidarStats; - BlockStats _flowQStats; - BlockStats _visionStats; - BlockStats _mocapStats; - BlockStats _gpsStats; - uint16_t _landCount; - - // low pass - BlockLowPassVector _xLowPass; - BlockLowPass _aglLowPass; - - // delay blocks - BlockDelay _xDelay; - BlockDelay _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 _x; // state vector - Vector _u; // input vector - Matrix _P; // state covariance matrix - - matrix::Dcm _R_att; - - Matrix _A; // dynamics matrix - Matrix _B; // input matrix - Matrix _R; // input covariance - Matrix _Q; // process noise covariance }; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index f87fb1d1fd..a54534e6d0 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -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 ) diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp deleted file mode 100644 index 958899a69e..0000000000 --- a/src/modules/local_position_estimator/local_position_estimator_main.cpp +++ /dev/null @@ -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 - * @author Mohammed Kabir - * @author Nuno Marques - * - * Local position estimator - */ - -#include -#include -#include - -#include "BlockLocalPositionEstimator.hpp" - -extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]); - -class LocalPositionEstimatorModule : public ModuleBase -{ -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); -} diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index 805f869092..ecd08be57b 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -68,7 +68,7 @@ void BlockLocalPositionEstimator::baroCorrect() // residual Matrix S_I = - inv((C * _P * C.transpose()) + R); + inv((C * m_P * C.transpose()) + R); Vector r = y - (C * _x); // fault detection @@ -87,10 +87,10 @@ void BlockLocalPositionEstimator::baroCorrect() } // kalman filter correction always - Matrix K = _P * C.transpose() * S_I; + Matrix K = m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } void BlockLocalPositionEstimator::baroCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 60b0c03ff9..6ba01d3645 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -169,7 +169,7 @@ void BlockLocalPositionEstimator::flowCorrect() Vector r = y - C * _x; // residual covariance - Matrix S = C * _P * C.transpose() + R; + Matrix 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 K = - _P * C.transpose() * S_I; + m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index fe3cc9fe2d..a5afa8d6a3 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -186,7 +186,7 @@ void BlockLocalPositionEstimator::gpsCorrect() Vector r = y - C * x0; // residual covariance - Matrix S = C * _P * C.transpose() + R; + Matrix 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 K = _P * C.transpose() * S_I; + Matrix K = m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } void BlockLocalPositionEstimator::gpsCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp index 8e67311192..e307408509 100644 --- a/src/modules/local_position_estimator/sensors/land.cpp +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -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 S_I = inv((C * _P * C.transpose()) + R); + Matrix S_I = inv((C * m_P * C.transpose()) + R); Vector 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 K = _P * C.transpose() * S_I; + Matrix K = m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } void BlockLocalPositionEstimator::landCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/modules/local_position_estimator/sensors/landing_target.cpp index 0c389ed686..37dcb90429 100644 --- a/src/modules/local_position_estimator/sensors/landing_target.cpp +++ b/src/modules/local_position_estimator/sensors/landing_target.cpp @@ -82,7 +82,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect() // residual covariance, (inverse) Matrix S_I = - inv(C * _P * C.transpose() + R); + inv(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 K = - _P * C.transpose() * S_I; + m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index 126c943601..e0d9240851 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::lidarCorrect() // residual Vector r = y - C * _x; // residual covariance - Matrix S = C * _P * C.transpose() + R; + Matrix 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 K = _P * C.transpose() * S_I; + Matrix K = m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } void BlockLocalPositionEstimator::lidarCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index cdbef158a8..cb445bbf9f 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -106,7 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect() Vector 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 r = y - C * _x; // residual covariance - Matrix S = C * _P * C.transpose() + R; + Matrix 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 K = _P * C.transpose() * S_I; + Matrix K = m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } void BlockLocalPositionEstimator::mocapCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index ecc8b063c1..47489a6fa2 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -109,7 +109,7 @@ void BlockLocalPositionEstimator::sonarCorrect() // residual Vector r = y - C * _x; // residual covariance - Matrix S = C * _P * C.transpose() + R; + Matrix 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 K = - _P * C.transpose() * S_I; + m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } } diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 14604e99e2..9ba4e76a20 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -111,7 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect() Vector 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 r = y - C * x0; // residual covariance - Matrix S = C * _P * C.transpose() + R; + Matrix 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 K = _P * C.transpose() * S_I; + Matrix K = m_P * C.transpose() * S_I; Vector dx = K * r; _x += dx; - _P -= K * C * _P; + m_P -= K * C * m_P; } } diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 365127e73e..a07767e8a7 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -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 diff --git a/src/modules/uORB/SubscriptionPollable.cpp b/src/modules/uORB/SubscriptionPollable.cpp deleted file mode 100644 index 22beb7c0ff..0000000000 --- a/src/modules/uORB/SubscriptionPollable.cpp +++ /dev/null @@ -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 - -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 *list) - : SubscriptionPollableBase(meta, interval, instance) -{ - if (list != nullptr) { - list->add(this); - } -} - -} // namespace uORB diff --git a/src/modules/uORB/SubscriptionPollable.hpp b/src/modules/uORB/SubscriptionPollable.hpp deleted file mode 100644 index 2e8d5d3be3..0000000000 --- a/src/modules/uORB/SubscriptionPollable.hpp +++ /dev/null @@ -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 -#include -#include -#include - -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 -{ -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 *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 __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 *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