Line data    Source code

       1             : /* Variometer class by Samuel Tabor
       2             : 
       3             : Manages the estimation of aircraft total energy, drag and vertical air velocity.
       4             : */
       5             : #include "Variometer.h"
       6             : 
       7             : #include <AP_AHRS/AP_AHRS.h>
       8             : #include <AP_Logger/AP_Logger.h>
       9             : 
      10         692 : Variometer::Variometer(const AP_FixedWing &parms, const PolarParams &polarParams) :
      11         692 :     _aparm(parms),
      12         692 :     _polarParams(polarParams)
      13             : {
      14         692 : }
      15             : 
      16       68767 : void Variometer::update(const float thermal_bank)
      17             : {
      18       68767 :     const AP_AHRS &_ahrs = AP::ahrs();
      19             : 
      20       68767 :     _ahrs.get_relative_position_D_home(alt);
      21       68767 :     alt = -alt;
      22             : 
      23       68767 :     float aspd = 0;
      24       68767 :     if (!_ahrs.airspeed_EAS(aspd)) {
      25         280 :         aspd = _aparm.airspeed_cruise;
      26             :     }
      27             : 
      28       68767 :     float aspd_filt = _sp_filter.apply(aspd);
      29             : 
      30             :     // Constrained airspeed.
      31       68767 :     const float minV = sqrtf(_polarParams.K/1.5);
      32       68767 :     _aspd_filt_constrained = aspd_filt>minV ? aspd_filt : minV;
      33             : 
      34       68767 :     tau = calculate_circling_time_constant(radians(thermal_bank));
      35             : 
      36       68767 :     float dt = (float)(AP_HAL::micros64() - _prev_update_time)/1e6;
      37             : 
      38             :     // Logic borrowed from AP_TECS.cpp
      39             :     // Update and average speed rate of change
      40             :     // Get DCM
      41       68767 :     const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
      42             :     // Calculate speed rate of change
      43       68767 :     float temp = rotMat.c.x * GRAVITY_MSS + AP::ins().get_accel().x;
      44             :     // take 5 point moving average
      45       68767 :     float dsp = _vdot_filter.apply(temp);
      46             : 
      47             :     // Now we need to high-pass this signal to remove bias.
      48       68767 :     _vdotbias_filter.set_cutoff_frequency(1/(20*tau));
      49       68767 :     float dsp_bias = _vdotbias_filter.apply(temp, dt);
      50             :     
      51       68767 :     float dsp_cor = dsp - dsp_bias;
      52             : 
      53             : 
      54       68767 :     Vector3f velned;
      55             : 
      56       68767 :     float raw_climb_rate = 0.0f;
      57       68767 :     if (_ahrs.get_velocity_NED(velned)) {
      58             :         // if possible use the EKF vertical velocity
      59       67178 :         raw_climb_rate = -velned.z;
      60             :     }
      61             :     
      62       68767 :     _climb_filter.set_cutoff_frequency(1/(3*tau));
      63       68767 :     float smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt);
      64             : 
      65             :     // Compute still-air sinkrate
      66       68767 :     float roll = _ahrs.get_roll_rad();
      67       68767 :     float sinkrate = calculate_aircraft_sinkrate(roll);
      68             : 
      69       68767 :     reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate;
      70             :     
      71             :     // Update filters.
      72             : 
      73       68767 :     float filtered_reading = _trigger_filter.apply(reading, dt);
      74             : 
      75       68767 :     _audio_filter.apply(reading, dt);
      76             : 
      77       68767 :     _stf_filter.apply(reading, dt);
      78             : 
      79       68767 :     _prev_update_time = AP_HAL::micros64();
      80             : 
      81       68767 :     _expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank));
      82             : 
      83             : #if HAL_LOGGING_ENABLED
      84             : // @LoggerMessage: VAR
      85             : // @Vehicles: Plane
      86             : // @Description: Variometer data
      87             : // @Field: TimeUS: Time since system startup
      88             : // @Field: aspd_raw: always zero
      89             : // @Field: aspd_filt: filtered and constrained airspeed
      90             : // @Field: alt: AHRS altitude
      91             : // @Field: roll: AHRS roll
      92             : // @Field: raw: estimated air vertical speed
      93             : // @Field: filt: low-pass filtered air vertical speed
      94             : // @Field: cl: raw climb rate
      95             : // @Field: fc: filtered climb rate
      96             : // @Field: exs: expected sink rate relative to air in thermalling turn
      97             : // @Field: dsp: average acceleration along X axis
      98             : // @Field: dspb: detected bias in average acceleration along X axis
      99      137534 :     AP::logger().WriteStreaming("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc,exs,dsp,dspb", "Qfffffffffff",
     100             :                        AP_HAL::micros64(),
     101             :                        (double)0.0,
     102       68767 :                        (double)_aspd_filt_constrained,
     103       68767 :                        (double)alt,
     104             :                        (double)roll,
     105       68767 :                        (double)reading,
     106             :                        (double)filtered_reading,
     107       68767 :                        (double)_raw_climb_rate,
     108             :                        (double)smoothed_climb_rate,
     109       68767 :                        (double)_expected_thermalling_sink,
     110             :                        (double)dsp,
     111             :                        (double)dsp_bias);
     112             : #else
     113             :     (void)filtered_reading;
     114             :     (void)smoothed_climb_rate;
     115             : #endif
     116       68767 : }
     117             : 
     118      137534 : float Variometer::calculate_aircraft_sinkrate(float phi) const
     119             : {
     120             :     // Remove aircraft sink rate
     121             :     float CL0;  // CL0 = 2*W/(rho*S*V^2)
     122             :     float C1;   // C1 = CD0/CL0
     123             :     float C2;   // C2 = CDi0/CL0 = B*CL0
     124      137534 :     CL0 = _polarParams.K / (_aspd_filt_constrained * _aspd_filt_constrained);
     125             : 
     126      137534 :     C1 = _polarParams.CD0 / CL0;  // constant describing expected angle to overcome zero-lift drag
     127      137534 :     C2 = _polarParams.B * CL0;    // constant describing expected angle to overcome lift induced drag at zero bank
     128             : 
     129      137534 :     float cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi)
     130             :     
     131      137534 :     return _aspd_filt_constrained * (C1 + C2 / (cosphi * cosphi));
     132             : }
     133             : 
     134       68767 : float Variometer::calculate_circling_time_constant(float thermal_bank) const
     135             : {
     136             :     // Calculate a time constant to use to filter quantities over a full thermal orbit.
     137             :     // This is used for rejecting variation in e.g. climb rate, or estimated climb rate
     138             :     // potential, as the aircraft orbits the thermal.
     139             :     // Use the time to circle - variations at the circling frequency then have a gain of 25%
     140             :     // and the response to a step input will reach 64% of final value in three orbits.
     141       68767 :     return 2*M_PI*_aspd_filt_constrained/(GRAVITY_MSS*tanf(thermal_bank));
     142             : }