void SoaringController::update_vario()
{
Location current_loc;
_ahrs.get_position(current_loc);
get_altitude_wrt_home(&_alt);
if (fabsf(_alt - _last_alt) > 0.0001f) { // if no change in altitude then there will be no update of ekf buffer
// Both filtered total energy rates and unfiltered are computed for the thermal switching logic and the EKF
float aspd = 0;
float roll = _ahrs.roll;
if (!_ahrs.airspeed_estimate(&aspd)) {
aspd = _aparm.airspeed_cruise_cm / 100.0f;
}
_aspd_filt = ASPD_FILT * aspd + (1 - ASPD_FILT) * _aspd_filt;
float total_E = _alt + 0.5 *_aspd_filt * _aspd_filt / GRAVITY_MSS; // Work out total energy
float sinkrate = correct_netto_rate(0.0f, (roll + _last_roll) / 2, _aspd_filt); // Compute still-air sinkrate
_vario_reading = (total_E - _last_total_E) / ((AP_HAL::micros64() - _prev_vario_update_time) * 1e-6) + sinkrate; // Unfiltered netto rate
_filtered_vario_reading = TE_FILT * _vario_reading + (1 - TE_FILT) * _filtered_vario_reading; // Apply low pass timeconst filter for noise
_displayed_vario_reading = TE_FILT_DISPLAYED * _vario_reading + (1 - TE_FILT_DISPLAYED) * _displayed_vario_reading;
float dx = 0;
float dy = 0;
float dx_w = 0;
float dy_w = 0;
Vector3f wind = _ahrs.wind_estimate();
get_wind_corrected_drift(¤t_loc, &wind, &dx_w, &dy_w, &dx, &dy);
_last_alt = _alt; // Store variables
_last_roll = roll;
_last_aspd = aspd;
_last_total_E = total_E;
_prev_vario_update_time = AP_HAL::micros64();
_new_data = true;
DataFlash_Class::instance()->Log_Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,wx,wy,dx,dy", "Qffffffffff",
AP_HAL::micros64(),
(double)aspd,
(double)_aspd_filt,
(double)_alt,
(double)roll,
(double)_vario_reading,
(double)_filtered_vario_reading,
(double)wind.x,
(double)wind.y,
(double)dx,
(double)dy);
}
}
void Variometer::update(const float polar_K, const float polar_B, const float polar_Cd0)
{
_ahrs.get_relative_position_D_home(alt);
alt = -alt;
if (fabsf(alt - _last_alt) > 0.0001f) { // if no change in altitude then there will be no update of ekf buffer
// Both filtered total energy rates and unfiltered are computed for the thermal switching logic and the EKF
float aspd = 0;
float roll = _ahrs.roll;
if (!_ahrs.airspeed_estimate(&aspd)) {
aspd = _aparm.airspeed_cruise_cm / 100.0f;
}
_aspd_filt = ASPD_FILT * aspd + (1 - ASPD_FILT) * _aspd_filt;
float total_E = alt + 0.5f *_aspd_filt * _aspd_filt / GRAVITY_MSS; // Work out total energy
float sinkrate = correct_netto_rate(0.0f, (roll + _last_roll) / 2, _aspd_filt, polar_K, polar_Cd0, polar_B); // Compute still-air sinkrate
reading = (total_E - _last_total_E) / ((AP_HAL::micros64() - _prev_update_time) * 1e-6) + sinkrate; // Unfiltered netto rate
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;
_last_alt = alt; // Store variables
_last_roll = roll;
_last_aspd = aspd;
_last_total_E = total_E;
_prev_update_time = AP_HAL::micros64();
new_data = true;
DataFlash_Class::instance()->Log_Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff",
AP_HAL::micros64(),
(double)aspd,
(double)_aspd_filt,
(double)alt,
(double)roll,
(double)reading,
(double)filtered_reading);
}
}
