2017-04-27 18:44:03 -03:00
|
|
|
/* Variometer class by Samuel Tabor
|
|
|
|
|
|
|
|
Manages the estimation of aircraft total energy, drag and vertical air velocity.
|
|
|
|
*/
|
|
|
|
#include "Variometer.h"
|
|
|
|
|
2019-06-27 00:05:06 -03:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
|
2020-05-05 01:41:17 -03:00
|
|
|
Variometer::Variometer(const AP_Vehicle::FixedWing &parms) :
|
2019-03-29 11:08:52 -03:00
|
|
|
_aparm(parms)
|
2017-04-27 18:44:03 -03:00
|
|
|
{
|
2019-06-08 09:51:00 -03:00
|
|
|
_climb_filter = LowPassFilter<float>(1.0/60.0);
|
2019-06-28 10:29:04 -03:00
|
|
|
|
|
|
|
_vdot_filter2 = LowPassFilter<float>(1.0f/60.0f);
|
2017-04-27 18:44:03 -03:00
|
|
|
}
|
|
|
|
|
2019-06-23 10:47:29 -03:00
|
|
|
void Variometer::update(const float polar_K, const float polar_Cd0, const float polar_B)
|
2017-04-27 18:44:03 -03:00
|
|
|
{
|
2020-05-05 01:41:17 -03:00
|
|
|
const AP_AHRS &_ahrs = AP::ahrs();
|
|
|
|
|
2017-04-29 07:40:24 -03:00
|
|
|
_ahrs.get_relative_position_D_home(alt);
|
|
|
|
alt = -alt;
|
2017-04-27 18:44:03 -03:00
|
|
|
|
2019-03-26 12:29:53 -03:00
|
|
|
float aspd = 0;
|
|
|
|
if (!_ahrs.airspeed_estimate(aspd)) {
|
2017-04-27 18:44:03 -03:00
|
|
|
aspd = _aparm.airspeed_cruise_cm / 100.0f;
|
|
|
|
}
|
2019-03-26 12:29:53 -03:00
|
|
|
_aspd_filt = _sp_filter.apply(aspd);
|
|
|
|
|
2019-06-22 13:02:35 -03:00
|
|
|
// Constrained airspeed.
|
2019-06-23 11:12:45 -03:00
|
|
|
const float minV = sqrtf(polar_K/1.5);
|
2019-06-23 04:35:33 -03:00
|
|
|
_aspd_filt_constrained = _aspd_filt>minV ? _aspd_filt : minV;
|
|
|
|
|
2019-06-22 13:02:35 -03:00
|
|
|
|
2020-01-11 11:14:06 -04:00
|
|
|
tau = calculate_circling_time_constant();
|
2019-06-28 10:29:04 -03:00
|
|
|
|
|
|
|
float dt = (float)(AP_HAL::micros64() - _prev_update_time)/1e6;
|
|
|
|
|
|
|
|
// Logic borrowed from AP_TECS.cpp
|
|
|
|
// Update and average speed rate of change
|
|
|
|
// Get DCM
|
|
|
|
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
|
|
|
|
// Calculate speed rate of change
|
|
|
|
float temp = rotMat.c.x * GRAVITY_MSS + AP::ins().get_accel().x;
|
|
|
|
// take 5 point moving average
|
|
|
|
float dsp = _vdot_filter.apply(temp);
|
|
|
|
|
|
|
|
// Now we need to high-pass this signal to remove bias.
|
|
|
|
_vdot_filter2.set_cutoff_frequency(1/(20*tau));
|
|
|
|
float dsp_bias = _vdot_filter2.apply(temp, dt);
|
|
|
|
|
|
|
|
float dsp_cor = dsp - dsp_bias;
|
|
|
|
|
|
|
|
|
2019-06-23 04:35:33 -03:00
|
|
|
Vector3f velned;
|
|
|
|
if (_ahrs.get_velocity_NED(velned)) {
|
|
|
|
// if possible use the EKF vertical velocity
|
|
|
|
raw_climb_rate = -velned.z;
|
|
|
|
}
|
2019-06-28 10:29:04 -03:00
|
|
|
|
|
|
|
_climb_filter.set_cutoff_frequency(1/(3*tau));
|
|
|
|
smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt);
|
2019-06-22 13:02:35 -03:00
|
|
|
|
2019-03-26 12:29:53 -03:00
|
|
|
// Compute still-air sinkrate
|
2019-06-23 04:35:33 -03:00
|
|
|
float roll = _ahrs.roll;
|
2019-06-23 04:52:38 -03:00
|
|
|
float sinkrate = calculate_aircraft_sinkrate(roll, polar_K, polar_Cd0, polar_B);
|
2019-03-26 12:29:53 -03:00
|
|
|
|
2019-06-28 10:29:04 -03:00
|
|
|
reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate;
|
2019-03-26 12:29:53 -03:00
|
|
|
|
|
|
|
|
|
|
|
filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise
|
|
|
|
displayed_reading = TE_FILT_DISPLAYED * reading + (1 - TE_FILT_DISPLAYED) * displayed_reading;
|
|
|
|
|
|
|
|
_prev_update_time = AP_HAL::micros64();
|
|
|
|
|
2019-06-23 11:13:08 -03:00
|
|
|
float expected_roll = atanf(powf(_aspd_filt_constrained,2)/(GRAVITY_MSS*_aparm.loiter_radius));
|
|
|
|
_expected_thermalling_sink = calculate_aircraft_sinkrate(expected_roll, polar_K, polar_Cd0, polar_B);
|
|
|
|
|
2020-04-21 21:22:42 -03:00
|
|
|
// @LoggerMessage: VAR
|
|
|
|
// @Vehicles: Plane
|
|
|
|
// @Description: Variometer data
|
|
|
|
// @Field: TimeUS: Time since system startup
|
|
|
|
// @Field: aspd_raw: always zero
|
|
|
|
// @Field: aspd_filt: filtered and constrained airspeed
|
|
|
|
// @Field: alt: AHRS altitude
|
|
|
|
// @Field: roll: AHRS roll
|
|
|
|
// @Field: raw: estimated air vertical speed
|
|
|
|
// @Field: filt: low-pass filtered air vertical speed
|
|
|
|
// @Field: cl: raw climb rate
|
|
|
|
// @Field: fc: filtered climb rate
|
|
|
|
// @Field: exs: expected sink rate relative to air in thermalling turn
|
|
|
|
// @Field: dsp: average acceleration along X axis
|
|
|
|
// @Field: dspb: detected bias in average acceleration along X axis
|
2019-06-28 10:29:04 -03:00
|
|
|
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc,exs,dsp,dspb", "Qfffffffffff",
|
2019-03-26 12:29:53 -03:00
|
|
|
AP_HAL::micros64(),
|
|
|
|
(double)0.0,
|
2019-06-23 04:35:33 -03:00
|
|
|
(double)_aspd_filt_constrained,
|
2019-03-26 12:29:53 -03:00
|
|
|
(double)alt,
|
|
|
|
(double)roll,
|
|
|
|
(double)reading,
|
2019-06-08 09:51:00 -03:00
|
|
|
(double)filtered_reading,
|
|
|
|
(double)raw_climb_rate,
|
2019-06-23 11:13:08 -03:00
|
|
|
(double)smoothed_climb_rate,
|
2019-06-27 13:26:55 -03:00
|
|
|
(double)_expected_thermalling_sink,
|
2019-06-28 10:29:04 -03:00
|
|
|
(double)dsp,
|
|
|
|
(double)dsp_bias);
|
2017-04-27 18:44:03 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2019-06-23 04:52:38 -03:00
|
|
|
float Variometer::calculate_aircraft_sinkrate(float phi,
|
|
|
|
const float polar_K,
|
|
|
|
const float polar_CD0,
|
|
|
|
const float polar_B)
|
2017-04-27 18:44:03 -03:00
|
|
|
{
|
|
|
|
// Remove aircraft sink rate
|
|
|
|
float CL0; // CL0 = 2*W/(rho*S*V^2)
|
|
|
|
float C1; // C1 = CD0/CL0
|
|
|
|
float C2; // C2 = CDi0/CL0 = B*CL0
|
2019-06-23 04:52:38 -03:00
|
|
|
CL0 = polar_K / (_aspd_filt_constrained * _aspd_filt_constrained);
|
2019-06-22 13:02:35 -03:00
|
|
|
|
2017-04-27 18:44:03 -03:00
|
|
|
C1 = polar_CD0 / CL0; // constant describing expected angle to overcome zero-lift drag
|
|
|
|
C2 = polar_B * CL0; // constant describing expected angle to overcome lift induced drag at zero bank
|
|
|
|
|
2019-06-23 04:52:38 -03:00
|
|
|
float cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi)
|
|
|
|
|
|
|
|
return _aspd_filt_constrained * (C1 + C2 / (cosphi * cosphi));
|
2017-04-27 18:44:03 -03:00
|
|
|
}
|
2019-06-23 04:35:33 -03:00
|
|
|
|
|
|
|
float Variometer::calculate_circling_time_constant()
|
|
|
|
{
|
|
|
|
// Calculate a time constant to use to filter quantities over a full thermal orbit.
|
|
|
|
// This is used for rejecting variation in e.g. climb rate, or estimated climb rate
|
|
|
|
// potential, as the aircraft orbits the thermal.
|
|
|
|
// Use the time to circle - variations at the circling frequency then have a gain of 25%
|
|
|
|
// and the response to a step input will reach 64% of final value in three orbits.
|
2019-06-28 10:29:04 -03:00
|
|
|
return _aparm.loiter_radius*2*M_PI/_aspd_filt_constrained;
|
2019-06-23 04:35:33 -03:00
|
|
|
}
|