forked from Archive/PX4-Autopilot
154 lines
6.0 KiB
C++
154 lines
6.0 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2020 Estimation and Control Library (ECL). 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 ECL 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 sensor_range_finder.hpp
|
|
* Range finder class containing all the required checks
|
|
*
|
|
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
|
|
*
|
|
*/
|
|
#pragma once
|
|
|
|
#include "Sensor.hpp"
|
|
#include <matrix/math.hpp>
|
|
|
|
namespace estimator
|
|
{
|
|
namespace sensor
|
|
{
|
|
|
|
class SensorRangeFinder : public Sensor
|
|
{
|
|
public:
|
|
SensorRangeFinder() = default;
|
|
~SensorRangeFinder() override = default;
|
|
|
|
void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
|
|
bool isHealthy() const override { return _is_sample_valid; }
|
|
bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
|
|
|
|
void setSample(const rangeSample &sample) {
|
|
_sample = sample;
|
|
_is_sample_ready = true;
|
|
}
|
|
|
|
// This is required because of the ring buffer
|
|
// TODO: move the ring buffer here
|
|
rangeSample* getSampleAddress() { return &_sample; }
|
|
|
|
void setPitchOffset(float new_pitch_offset)
|
|
{
|
|
if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
|
|
_sin_pitch_offset = sinf(new_pitch_offset);
|
|
_cos_pitch_offset = cosf(new_pitch_offset);
|
|
_pitch_offset_rad = new_pitch_offset;
|
|
}
|
|
}
|
|
|
|
void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; }
|
|
|
|
void setLimits(float min_distance, float max_distance) {
|
|
_rng_valid_min_val = min_distance;
|
|
_rng_valid_max_val = max_distance;
|
|
}
|
|
|
|
float getCosTilt() const { return _cos_tilt_rng_to_earth; }
|
|
|
|
void setRange(float rng) { _sample.rng = rng; }
|
|
float getRange() const { return _sample.rng; }
|
|
|
|
float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
|
|
|
|
void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
|
|
void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
|
|
|
|
float getValidMinVal() const { return _rng_valid_min_val; }
|
|
float getValidMaxVal() const { return _rng_valid_max_val; }
|
|
|
|
private:
|
|
void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
|
|
|
|
void updateValidity(uint64_t current_time_us);
|
|
void updateDtDataLpf(uint64_t current_time_us);
|
|
bool isSampleOutOfDate(uint64_t current_time_us) const;
|
|
bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
|
|
bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
|
|
bool isDataInRange() const;
|
|
void updateStuckCheck();
|
|
|
|
rangeSample _sample{};
|
|
|
|
bool _is_sample_ready{}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
|
|
bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid
|
|
uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec)
|
|
|
|
/*
|
|
* Stuck check
|
|
*/
|
|
bool _is_stuck{};
|
|
float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m)
|
|
float _stuck_min_val{}; ///< minimum value for new rng measurement when being stuck
|
|
float _stuck_max_val{}; ///< maximum value for new rng measurement when being stuck
|
|
|
|
/*
|
|
* Data regularity check
|
|
*/
|
|
static constexpr float _dt_update{0.01f}; ///< delta time since last ekf update TODO: this should be a parameter
|
|
float _dt_data_lpf{}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
|
|
|
|
/*
|
|
* Tilt check
|
|
*/
|
|
float _cos_tilt_rng_to_earth{}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame
|
|
float _range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
|
|
float _pitch_offset_rad{3.14f}; ///< range finder tilt rotation about the Y body axis
|
|
float _sin_pitch_offset{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis
|
|
float _cos_pitch_offset{-1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis
|
|
|
|
/*
|
|
* Range check
|
|
*/
|
|
float _rng_valid_min_val{}; ///< minimum distance that the rangefinder can measure (m)
|
|
float _rng_valid_max_val{}; ///< maximum distance that the rangefinder can measure (m)
|
|
|
|
/*
|
|
* Quality check
|
|
*/
|
|
uint64_t _time_bad_quality_us{}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis)
|
|
uint64_t _quality_hyst_us{1000000}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
|
|
};
|
|
|
|
} // namespace sensor
|
|
} // namespace estimator
|