diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 81ea4a947d..31a86c5914 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -9,14 +9,14 @@ orb_advert_t mavlink_log_pub = nullptr; // required standard deviation of estimate for estimator to publish data -static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m -static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m -static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m -static const float P_MAX = 1.0e6f; // max allowed value in state covariance -static const float LAND_RATE = 10.0f; // rate of land detector correction +static const float P_MAX = 1.0e6f; // max allowed value in state covariance +static const float LAND_RATE = 10.0f; // rate of land detector correction -static const char *msg_label = "[lpe] "; // rate of land detector correction +static const char *msg_label = "[lpe] "; // rate of land detector correction BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // this block has no parent, and has name LPE @@ -192,8 +192,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : } BlockLocalPositionEstimator::~BlockLocalPositionEstimator() -{ -} +{} Vector BlockLocalPositionEstimator::dynamics( float t, @@ -205,7 +204,6 @@ Vector BlockLocalPositionEstimator::dyn void BlockLocalPositionEstimator::update() { - // wait for a sensor update, check for exit condition every 100 ms int ret = px4_poll(_polls, 3, 100); @@ -224,7 +222,6 @@ void BlockLocalPositionEstimator::update() bool armedState = _sub_armed.get().armed; if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) { - // detect distance sensors for (int i = 0; i < N_DIST_SUBS; i++) { uORB::Subscription *s = _dist_subs[i]; @@ -260,22 +257,22 @@ void BlockLocalPositionEstimator::update() // if (!_lastArmedState && armedState) { - // // we just armed, we are at origin on the ground - // _x(X_x) = 0; - // _x(X_y) = 0; - // // reset Z or not? _x(X_z) = 0; + // // we just armed, we are at origin on the ground + // _x(X_x) = 0; + // _x(X_y) = 0; + // // reset Z or not? _x(X_z) = 0; - // // we aren't moving, all velocities are zero - // _x(X_vx) = 0; - // _x(X_vy) = 0; - // _x(X_vz) = 0; + // // we aren't moving, all velocities are zero + // _x(X_vx) = 0; + // _x(X_vy) = 0; + // _x(X_vz) = 0; - // // assume we are on the ground, so terrain alt is local alt - // _x(X_tz) = _x(X_z); + // // assume we are on the ground, so terrain alt is local alt + // _x(X_tz) = _x(X_z); - // // reset lowpass filter as well - // _xLowPass.setState(_x); - // _aglLowPass.setState(0); + // // reset lowpass filter as well + // _xLowPass.setState(_x); + // _aglLowPass.setState(0); // } _lastArmedState = armedState; @@ -288,7 +285,7 @@ void BlockLocalPositionEstimator::update() int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative; if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) { - uint64_t baro_timestamp = _sub_sensor.get().timestamp + \ + uint64_t baro_timestamp = _sub_sensor.get().timestamp + \ _sub_sensor.get().baro_timestamp_relative; if (baro_timestamp != _timeStampLastBaro) { @@ -305,7 +302,7 @@ void BlockLocalPositionEstimator::update() bool lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated(); bool sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated(); bool landUpdated = landed() - && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate + && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate // get new data updateSubscriptions(); @@ -319,7 +316,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)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) { + if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) { vxy_stddev_ok = true; } @@ -386,7 +383,6 @@ void BlockLocalPositionEstimator::update() mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m", double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin)); - } // reinitialize x if necessary @@ -599,19 +595,19 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z; _pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY; _pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z; - _pub_lpos.get().x = xLP(X_x); // north - _pub_lpos.get().y = xLP(X_y); // east + _pub_lpos.get().x = xLP(X_x); // north + _pub_lpos.get().y = xLP(X_y); // east if (_fusion.get() & FUSE_PUB_AGL_Z) { - _pub_lpos.get().z = -_aglLowPass.getState(); // agl + _pub_lpos.get().z = -_aglLowPass.getState(); // agl } else { - _pub_lpos.get().z = xLP(X_z); // down + _pub_lpos.get().z = xLP(X_z); // down } - _pub_lpos.get().vx = xLP(X_vx); // north - _pub_lpos.get().vy = xLP(X_vy); // east - _pub_lpos.get().vz = xLP(X_vz); // down + _pub_lpos.get().vx = xLP(X_vx); // north + _pub_lpos.get().vy = xLP(X_vy); // east + _pub_lpos.get().vz = xLP(X_vz); // down // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity _pub_lpos.get().z_deriv = xLP(X_vz); @@ -624,7 +620,7 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; _pub_lpos.get().ref_alt = _altOrigin; _pub_lpos.get().dist_bottom = _aglLowPass.getState(); - _pub_lpos.get().dist_bottom_rate = - xLP(X_vz); + _pub_lpos.get().dist_bottom_rate = -xLP(X_vz); // we estimate agl even when we don't have terrain info // if you are in terrain following mode this is important // so that if terrain estimation fails there isn't a @@ -801,7 +797,6 @@ void BlockLocalPositionEstimator::updateSSParams() _pn_t_noise_density.get() + (_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; - } void BlockLocalPositionEstimator::predict() @@ -813,7 +808,7 @@ void BlockLocalPositionEstimator::predict() Vector3f a(_sub_sensor.get().accelerometer_m_s2); // note, bias is removed in dynamics function _u = _R_att * a; - _u(U_az) += 9.81f; // add g + _u(U_az) += 9.81f; // add g // update state space based on new states updateSSStates(); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index e485bf73c7..4bc7ae1b77 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -32,10 +32,10 @@ using namespace matrix; using namespace control; -static const float DELAY_MAX = 0.5f; // seconds -static const float HIST_STEP = 0.05f; // 20 hz +static const float DELAY_MAX = 0.5f; // seconds +static const float HIST_STEP = 0.05f; // 20 hz static const float BIAS_MAX = 1e-1f; -static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; +static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; static const size_t N_DIST_SUBS = 4; // for fault detection @@ -73,31 +73,31 @@ class BlockLocalPositionEstimator : public control::SuperBlock // // // input: -// ax, ay, az (acceleration NED) +// ax, ay, az (acceleration NED) // // states: -// px, py, pz , ( position NED, m) -// vx, vy, vz ( vel NED, m/s), -// bx, by, bz ( accel bias, m/s^2) -// tz (terrain altitude, ASL, m) +// px, py, pz , ( position NED, m) +// vx, vy, vz ( vel NED, m/s), +// bx, by, bz ( accel bias, m/s^2) +// tz (terrain altitude, ASL, m) // // measurements: // -// sonar: pz (measured d*cos(phi)*cos(theta)) +// sonar: pz (measured d*cos(phi)*cos(theta)) // -// baro: pz +// baro: pz // -// flow: vx, vy (flow is in body x, y frame) +// flow: vx, vy (flow is in body x, y frame) // -// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) +// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) // -// lidar: pz (actual measured d*cos(phi)*cos(theta)) +// lidar: pz (actual measured d*cos(phi)*cos(theta)) // -// vision: px, py, pz, vx, vy, vz +// vision: px, py, pz, vx, vy, vz // -// mocap: px, py, pz +// mocap: px, py, pz // -// land (detects when landed)): pz (always measures agl = 0) +// land (detects when landed)): pz (always measures agl = 0) // public: @@ -220,7 +220,10 @@ private: void checkTimeouts(); // misc - inline float agl() { return _x(X_tz) - _x(X_z); } + inline float agl() + { + return _x(X_tz) - _x(X_z); + } bool landed(); int getDelayPeriods(float delay, uint8_t *periods); @@ -317,7 +320,7 @@ private: BlockParamFloat _t_max_grade; // init origin - BlockParamInt _fake_origin; + BlockParamInt _fake_origin; BlockParamFloat _init_origin_lat; BlockParamFloat _init_origin_lon; @@ -375,15 +378,15 @@ private: uint8_t _estimatorInitialized; // state space - Vector _x; // state vector - Vector _u; // input vector - Matrix _P; // state covariance matrix + Vector _x; // state vector + Vector _u; // input vector + Matrix _P; // state covariance matrix matrix::Dcm _R_att; Vector3f _eul; - Matrix _A; // dynamics matrix - Matrix _B; // input matrix - Matrix _R; // input covariance - Matrix _Q; // process noise covariance + 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/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index f74c93f381..2948f673cb 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -6,8 +6,8 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize -static const uint32_t REQ_BARO_INIT_COUNT = 100; -static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s +static const uint32_t REQ_BARO_INIT_COUNT = 100; +static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s void BlockLocalPositionEstimator::baroInit() { @@ -59,7 +59,7 @@ void BlockLocalPositionEstimator::baroCorrect() // baro measurement matrix Matrix C; C.setZero(); - C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. Matrix R; R.setZero(); diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 7613e42344..80588e18fd 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -7,8 +7,8 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize -static const uint32_t REQ_FLOW_INIT_COUNT = 10; -static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s +static const uint32_t REQ_FLOW_INIT_COUNT = 10; +static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s // minimum flow altitude static const float flow_min_agl = 0.3; @@ -89,8 +89,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) // compute velocities in body frame using ground distance // note that the integral rates in the optical_flow uORB topic are RH rotations about body axes Vector3f delta_b( - +(flow_y_rad - gyro_y_rad)*d, - -(flow_x_rad - gyro_x_rad)*d, + +(flow_y_rad - gyro_y_rad) * d, + -(flow_x_rad - gyro_x_rad) * d, 0); // rotation of flow from body to nav frame @@ -201,9 +201,7 @@ void BlockLocalPositionEstimator::flowCorrect() Vector dx = K * r; _x += dx; _P -= K * C * _P; - } - } void BlockLocalPositionEstimator::flowCheckTimeout() diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 3f951b5a43..63cd63ac6d 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -6,8 +6,8 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize -static const uint32_t REQ_GPS_INIT_COUNT = 10; -static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s +static const uint32_t REQ_GPS_INIT_COUNT = 10; +static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s void BlockLocalPositionEstimator::gpsInit() { @@ -188,7 +188,7 @@ void BlockLocalPositionEstimator::gpsCorrect() Matrix S = C * _P * C.transpose() + R; // publish innovations - for (int i = 0; i < 6; i ++) { + for (int i = 0; i < 6; i++) { _pub_innov.get().vel_pos_innov[i] = r(i); _pub_innov.get().vel_pos_innov_var[i] = S(i, i); } @@ -205,8 +205,8 @@ void BlockLocalPositionEstimator::gpsCorrect() if (beta / BETA_TABLE[n_y_gps] > beta_thresh) { if (!(_sensorFault & SENSOR_GPS)) { mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", - double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), - double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5))); + double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)), + double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5))); _sensorFault |= SENSOR_GPS; } diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp index 5a0fd47fab..c62ca518f3 100644 --- a/src/modules/local_position_estimator/sensors/land.cpp +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -7,8 +7,8 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize // -static const uint32_t REQ_LAND_INIT_COUNT = 1; -static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s +static const uint32_t REQ_LAND_INIT_COUNT = 1; +static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s void BlockLocalPositionEstimator::landInit() { @@ -48,8 +48,8 @@ void BlockLocalPositionEstimator::landCorrect() // y = -(z - tz) C(Y_land_vx, X_vx) = 1; C(Y_land_vy, X_vy) = 1; - C(Y_land_agl, X_z) = -1; // measured altitude, negative down dir. - C(Y_land_agl, X_tz) = 1; // measured altitude, negative down dir. + C(Y_land_agl, X_z) = -1;// measured altitude, negative down dir. + C(Y_land_agl, X_tz) = 1;// measured altitude, negative down dir. // use parameter covariance SquareMatrix R; @@ -101,4 +101,3 @@ void BlockLocalPositionEstimator::landCheckTimeout() } } } - diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index 72a7e32240..f455cded0b 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -7,8 +7,8 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize // -static const uint32_t REQ_LIDAR_INIT_COUNT = 10; -static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s +static const uint32_t REQ_LIDAR_INIT_COUNT = 10; +static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s void BlockLocalPositionEstimator::lidarInit() { @@ -34,7 +34,7 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector &y) { // measure float d = _sub_lidar->get().current_distance; - float eps = 0.01f; // 1 cm + float eps = 0.01f; // 1 cm float min_dist = _sub_lidar->get().min_distance + eps; float max_dist = _sub_lidar->get().max_distance - eps; @@ -70,8 +70,8 @@ void BlockLocalPositionEstimator::lidarCorrect() C.setZero(); // y = -(z - tz) // TODO could add trig to make this an EKF correction - C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir. - C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir. + C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir. // use parameter covariance unless sensor provides reasonable value SquareMatrix R; diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index c5ad80e8ec..248fad388f 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -6,8 +6,8 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize -static const uint32_t REQ_MOCAP_INIT_COUNT = 20; -static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s +static const uint32_t REQ_MOCAP_INIT_COUNT = 20; +static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s void BlockLocalPositionEstimator::mocapInit() { @@ -67,7 +67,7 @@ void BlockLocalPositionEstimator::mocapCorrect() // noise matrix Matrix R; R.setZero(); - float mocap_p_var = _mocap_p_stddev.get()* \ + float mocap_p_var = _mocap_p_stddev.get() * \ _mocap_p_stddev.get(); R(Y_mocap_x, Y_mocap_x) = mocap_p_var; R(Y_mocap_y, Y_mocap_y) = mocap_p_var; @@ -79,11 +79,12 @@ void BlockLocalPositionEstimator::mocapCorrect() Matrix S = C * _P * C.transpose() + R; // publish innovations - for (int i = 0; i < 3; i ++) { + for (int i = 0; i < 3; i++) { _pub_innov.get().vel_pos_innov[i] = r(i); _pub_innov.get().vel_pos_innov_var[i] = S(i, i); } - for (int i = 3; i < 6; i ++) { + + for (int i = 3; i < 6; i++) { _pub_innov.get().vel_pos_innov[i] = 0; _pub_innov.get().vel_pos_innov_var[i] = 1; } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 245edb0fa1..337e35b27a 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -6,9 +6,9 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize -static const int REQ_SONAR_INIT_COUNT = 10; -static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s -static const float SONAR_MAX_INIT_STD = 0.3f; // meters +static const int REQ_SONAR_INIT_COUNT = 10; +static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s +static const float SONAR_MAX_INIT_STD = 0.3f; // meters void BlockLocalPositionEstimator::sonarInit() { @@ -48,7 +48,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector &y) { // measure float d = _sub_sonar->get().current_distance; - float eps = 0.01f; // 1 cm + float eps = 0.01f; // 1 cm float min_dist = _sub_sonar->get().min_distance + eps; float max_dist = _sub_sonar->get().max_distance - eps; @@ -95,8 +95,8 @@ void BlockLocalPositionEstimator::sonarCorrect() C.setZero(); // y = -(z - tz) // TODO could add trig to make this an EKF correction - C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir. - C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir. + C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir. // covariance matrix SquareMatrix R; diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 922b2ba791..9f3d99ee2d 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -7,12 +7,12 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor to initialize. // This is a vision based position measurement so we assume // as soon as we get one measurement it is initialized. -static const uint32_t REQ_VISION_INIT_COUNT = 1; +static const uint32_t REQ_VISION_INIT_COUNT = 1; // We don't want to deinitialize it because // this will throw away a correction before it starts using the data so we // set the timeout to 0.5 seconds -static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s +static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s void BlockLocalPositionEstimator::visionInit() { @@ -102,7 +102,7 @@ void BlockLocalPositionEstimator::visionCorrect() // vision delayed x uint8_t i_hist = 0; - float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds + float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds if (vision_delay < 0.0f) { vision_delay = 0.0f; }