diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 61d5c5d943..8713a954e7 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -266,48 +266,34 @@ void SoaringController::update_thermalling() struct Location current_loc; _ahrs.get_position(current_loc); - if (_vario.new_data) { - 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); + 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); -#if (0) - // Print32_t filter info for debugging - int32_t i; - for (i = 0; i < 4; i++) { - gcs().send_text(MAV_SEVERITY_INFO, "%e ", (double)_ekf.P[i][i]); - } - for (i = 0; i < 4; i++) { - gcs().send_text(MAV_SEVERITY_INFO, "%e ", (double)_ekf.X[i]); - } -#endif + // write log - save the data. + AP::logger().Write("SOAR", "TimeUS,nettorate,dx,dy,x0,x1,x2,x3,lat,lng,alt,dx_w,dy_w", "QfffffffLLfff", + AP_HAL::micros64(), + (double)_vario.reading, + (double)dx, + (double)dy, + (double)_ekf.X[0], + (double)_ekf.X[1], + (double)_ekf.X[2], + (double)_ekf.X[3], + current_loc.lat, + current_loc.lng, + (double)_vario.alt, + (double)dx_w, + (double)dy_w); - // write log - save the data. - AP::logger().Write("SOAR", "TimeUS,nettorate,dx,dy,x0,x1,x2,x3,lat,lng,alt,dx_w,dy_w", "QfffffffLLfff", - AP_HAL::micros64(), - (double)_vario.reading, - (double)dx, - (double)dy, - (double)_ekf.X[0], - (double)_ekf.X[1], - (double)_ekf.X[2], - (double)_ekf.X[3], - current_loc.lat, - current_loc.lng, - (double)_vario.alt, - (double)dx_w, - (double)dy_w); + //log_data(); + _ekf.update(_vario.reading,dx, dy); // update the filter - //log_data(); - _ekf.update(_vario.reading,dx, dy); // update the filter - - _prev_update_location = current_loc; // save for next time - _prev_update_time = AP_HAL::micros64(); - _vario.new_data = false; - } + _prev_update_location = current_loc; // save for next time + _prev_update_time = AP_HAL::micros64(); } void SoaringController::update_cruising() diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index 25b7e99957..6a6825c0b7 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -8,8 +8,7 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity. Variometer::Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) : _ahrs(ahrs), - _aparm(parms), - new_data(false) + _aparm(parms) { } @@ -52,7 +51,6 @@ void Variometer::update(const float polar_K, const float polar_B, const float po displayed_reading = TE_FILT_DISPLAYED * reading + (1 - TE_FILT_DISPLAYED) * displayed_reading; _prev_update_time = AP_HAL::micros64(); - new_data = true; AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff", AP_HAL::micros64(), diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h index 70b9c152e8..4aa3482078 100644 --- a/libraries/AP_Soaring/Variometer.h +++ b/libraries/AP_Soaring/Variometer.h @@ -40,7 +40,6 @@ public: float reading; float filtered_reading; float displayed_reading; - bool new_data; void update(const float polar_K, const float polar_CD0, const float polar_B); float correct_netto_rate(float climb_rate, float phi, float aspd, const float polar_K, const float polar_CD0, const float polar_B);