data_validator.h

Go to the documentation of this file.

/****************************************************************************
 *
 *   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 data_validator.h
 *
 * A data validation class to identify anomalies in data streams
 *
 * @author Lorenz Meier <lorenz@px4.io>
 */

#pragma once

#include <math.h>
#include <stdint.h>

class DataValidator
{
public:
    static const unsigned dimensions = 3;

    DataValidator() = default;
    ~DataValidator() = default;

    /**
     * Put an item into the validator.
     *
     * @param val       Item to put
     */
    void            put(uint64_t timestamp, float val, uint64_t error_count, int priority);

    /**
     * Put a 3D item into the validator.
     *
     * @param val       Item to put
     */
    void            put(uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority);

    /**
     * Get the next sibling in the group
     *
     * @return      the next sibling
     */
    DataValidator       *sibling() { return _sibling; }

    /**
     * Set the sibling to the next node in the group
     *
     */
    void            setSibling(DataValidator *new_sibling) { _sibling = new_sibling; }

    /**
     * Get the confidence of this validator
     * @return      the confidence between 0 and 1
     */
    float           confidence(uint64_t timestamp);

    /**
     * Get the error count of this validator
     * @return      the error count
     */
    uint64_t        error_count() const { return _error_count; }

    /**
     * Get the values of this validator
     * @return      the stored value
     */
    float           *value() { return _value; }

    /**
     * Get the used status of this validator
     * @return      true if this validator ever saw data
     */
    bool            used() const { return (_time_last > 0); }

    /**
     * Get the priority of this validator
     * @return      the stored priority
     */
    int         priority() const { return _priority; }

    /**
     * Get the error state of this validator
     * @return      the bitmask with the error status
     */
    uint32_t        state() const { return _error_mask; }

    /**
     * Reset the error state of this validator
     */
    void            reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }

    /**
     * Get the RMS values of this validator
     * @return      the stored RMS
     */
    float           *rms() { return _rms; }

    /**
     * Get the vibration offset
     * @return      the stored vibration offset
     */
    float           *vibration_offset() { return _vibe; }

    /**
     * Print the validator value
     *
     */
    void            print();

    /**
     * Set the timeout value
     *
     * @param timeout_interval_us The timeout interval in microseconds
     */
    void            set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }

    /**
     * Set the equal count threshold
     *
     * @param threshold The number of equal values before considering the sensor stale
     */
    void            set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; }

    /**
     * Get the timeout value
     *
     * @return The timeout interval in microseconds
     */
    uint32_t        get_timeout() const { return _timeout_interval; }

    /**
     * Data validator error states
     */
    static constexpr uint32_t ERROR_FLAG_NO_ERROR       = (0x00000000U);
    static constexpr uint32_t ERROR_FLAG_NO_DATA        = (0x00000001U);
    static constexpr uint32_t ERROR_FLAG_STALE_DATA     = (0x00000001U << 1);
    static constexpr uint32_t ERROR_FLAG_TIMEOUT        = (0x00000001U << 2);
    static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT  = (0x00000001U << 3);
    static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY    = (0x00000001U << 4);

private:
    uint32_t _error_mask{ERROR_FLAG_NO_ERROR};  /**< sensor error state */

    uint32_t _timeout_interval{20000};      /**< interval in which the datastream times out in us */

    uint64_t _time_last{0};             /**< last timestamp */
    uint64_t _event_count{0};           /**< total data counter */
    uint64_t _error_count{0};           /**< error count */

    int _error_density{0};              /**< ratio between successful reads and errors */

    int _priority{0};               /**< sensor nominal priority */

    float _mean[dimensions] {};         /**< mean of value */
    float _lp[dimensions] {};           /**< low pass value */
    float _M2[dimensions] {};           /**< RMS component value */
    float _rms[dimensions] {};          /**< root mean square error */
    float _value[dimensions] {};            /**< last value */
    float _vibe[dimensions] {};         /**< vibration level, in sensor unit */

    unsigned _value_equal_count{0};         /**< equal values in a row */
    unsigned _value_equal_count_threshold{VALUE_EQUAL_COUNT_DEFAULT};   /**< when to consider an equal count as a problem */

    DataValidator *_sibling{nullptr};       /**< sibling in the group */

    static const constexpr unsigned NORETURN_ERRCOUNT = 10000;  /**< if the error count reaches this value, return sensor as invalid */
    static const constexpr float ERROR_DENSITY_WINDOW = 100.0f;     /**< window in measurement counts for errors */
    static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = 100;    /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */

    /* we don't want this class to be copied */
    DataValidator(const DataValidator &) = delete;
    DataValidator operator=(const DataValidator &) = delete;
};