sonar.cpp

Go to the documentation of this file.

#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>

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

void BlockLocalPositionEstimator::sonarInit()
{
    // measure
    Vector<float, n_y_sonar> y;

    if (_sonarStats.getCount() == 0) {
        _time_init_sonar = _timeStamp;
    }

    if (sonarMeasure(y) != OK) {
        return;
    }

    // if finished
    if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) {
        if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) {
            mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min");
            _sonarStats.reset();

        } else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) {
            mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout ");
            _sonarStats.reset();

        } else {
            PX4_INFO("[lpe] sonar init "
                 "mean %d cm std %d cm",
                 int(100 * _sonarStats.getMean()(0)),
                 int(100 * _sonarStats.getStdDev()(0)));
            _sensorTimeout &= ~SENSOR_SONAR;
            _sensorFault &= ~SENSOR_SONAR;
        }
    }
}

int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
{
    // measure
    float d = _sub_sonar->get().current_distance;
    float eps = 0.01f;  // 1 cm
    float min_dist = _sub_sonar->get().min_distance + eps;
    float max_dist = _sub_sonar->get().max_distance - eps;

    // prevent driver from setting min dist below eps
    if (min_dist < eps) {
        min_dist = eps;
    }

    // check for bad data
    if (d > max_dist || d < min_dist) {
        return -1;
    }

    // update stats
    _sonarStats.update(Scalarf(d));
    _time_last_sonar = _timeStamp;
    y.setZero();
    matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
    y(0) = (d + _param_lpe_snr_off_z.get()) *
           cosf(euler.phi()) *
           cosf(euler.theta());
    return OK;
}

void BlockLocalPositionEstimator::sonarCorrect()
{
    // measure
    Vector<float, n_y_sonar> y;

    if (sonarMeasure(y) != OK) { return; }

    // do not use sonar if lidar is active and not faulty or timed out
    if (_lidarUpdated
        && !(_sensorFault & SENSOR_LIDAR)
        && !(_sensorTimeout & SENSOR_LIDAR)) { return; }

    // calculate covariance
    float cov = _sub_sonar->get().variance;

    if (cov < 1.0e-3f) {
        // use sensor value if reasoanble
        cov = _param_lpe_snr_z.get() * _param_lpe_snr_z.get();
    }

    // sonar measurement matrix and noise matrix
    Matrix<float, n_y_sonar, n_x> C;
    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.

    // covariance matrix
    SquareMatrix<float, n_y_sonar> R;
    R.setZero();
    R(0, 0) = cov;

    // residual
    Vector<float, n_y_sonar> r = y - C * _x;
    // residual covariance
    Matrix<float, n_y_sonar, n_y_sonar> S = C * m_P * C.transpose() + R;

    // publish innovations
    _pub_innov.get().hagl_innov = r(0);
    _pub_innov.get().hagl_innov_var = S(0, 0);

    // residual covariance, (inverse)
    Matrix<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);

    // fault detection
    float beta = (r.transpose()  * (S_I * r))(0, 0);

    if (beta > BETA_TABLE[n_y_sonar]) {
        if (!(_sensorFault & SENSOR_SONAR)) {
            _sensorFault |= SENSOR_SONAR;
            mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault,  beta %5.2f", double(beta));
        }

        // abort correction
        return;

    } else if (_sensorFault & SENSOR_SONAR) {
        _sensorFault &= ~SENSOR_SONAR;
        //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK");
    }

    // kalman filter correction if no fault
    if (!(_sensorFault & SENSOR_SONAR)) {
        Matrix<float, n_x, n_y_sonar> K =
            m_P * C.transpose() * S_I;
        Vector<float, n_x> dx = K * r;
        _x += dx;
        m_P -= K * C * m_P;
    }
}

void BlockLocalPositionEstimator::sonarCheckTimeout()
{
    if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) {
        if (!(_sensorTimeout & SENSOR_SONAR)) {
            _sensorTimeout |= SENSOR_SONAR;
            _sonarStats.reset();
            mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
        }
    }
}