mirror of https://github.com/ArduPilot/ardupilot
87 lines
2.2 KiB
C++
87 lines
2.2 KiB
C++
|
|
/* Variometer class by Samuel Tabor
|
|
|
|
Manages the estimation of aircraft total energy, drag and vertical air velocity.
|
|
*/
|
|
#pragma once
|
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <Filter/AverageFilter.h>
|
|
#include <AP_Vehicle/AP_FixedWing.h>
|
|
|
|
class Variometer {
|
|
|
|
const AP_FixedWing &_aparm;
|
|
|
|
// store time of last update
|
|
uint64_t _prev_update_time;
|
|
|
|
float _raw_climb_rate;
|
|
|
|
float _aspd_filt_constrained;
|
|
|
|
float _expected_thermalling_sink;
|
|
|
|
// declares a 5point average filter using floats
|
|
AverageFilterFloat_Size5 _vdot_filter;
|
|
|
|
AverageFilterFloat_Size5 _sp_filter;
|
|
|
|
/*
|
|
low pass filters for various purposes.
|
|
*/
|
|
// Climb rate filter for monitoring progress in thermal.
|
|
LowPassFilter<float> _climb_filter{1/60.0};
|
|
|
|
// Fast filter for mavlink/audio vario output.
|
|
LowPassFilter<float> _audio_filter{1/0.71};
|
|
|
|
// Slower filter for deciding to enter THERMAL mode.
|
|
LowPassFilter<float> _trigger_filter{1/4.06};
|
|
|
|
// Longitudinal acceleration bias filter.
|
|
LowPassFilter<float> _vdotbias_filter{1/60.0};
|
|
|
|
// Speed to fly vario filter.
|
|
LowPassFilter<float> _stf_filter{1/20.0};
|
|
|
|
public:
|
|
struct PolarParams {
|
|
AP_Float K;
|
|
AP_Float CD0;
|
|
AP_Float B;
|
|
};
|
|
|
|
Variometer(const AP_FixedWing &parms, const PolarParams &polarParams);
|
|
|
|
float alt;
|
|
float reading;
|
|
float tau;
|
|
|
|
void update(const float thermal_bank);
|
|
float calculate_aircraft_sinkrate(float phi) const;
|
|
|
|
void reset_climb_filter(float value) { _climb_filter.reset(value);}
|
|
|
|
void reset_trigger_filter(float value) { _trigger_filter.reset(value);}
|
|
|
|
float get_airspeed(void) const {return _aspd_filt_constrained;};
|
|
|
|
float get_displayed_value(void) const {return _audio_filter.get();};
|
|
|
|
float get_filtered_climb(void) const {return _climb_filter.get();};
|
|
|
|
float get_trigger_value(void) const {return _trigger_filter.get();};
|
|
|
|
float get_stf_value(void) const {return _stf_filter.get();};
|
|
|
|
float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;};
|
|
|
|
float calculate_circling_time_constant(const float thermal_bank) const;
|
|
|
|
private:
|
|
const PolarParams &_polarParams;
|
|
};
|
|
|