aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Soaring: Remove redundant check for new vario data.

Browse Source
This commit is contained in:
Samuel Tabor 2019-03-29 14:08:52 +00:00 committed by Andrew Tridgell
parent 64460be0d0
commit c135b00b7b
3 changed files with 26 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -266,48 +266,34 @@ void SoaringController::update_thermalling()
struct Location current_loc; struct Location current_loc;
_ahrs.get_position(current_loc); _ahrs.get_position(current_loc);
if (_vario.new_data) { float dx = 0;
float dx = 0; float dy = 0;
float dy = 0; float dx_w = 0;
float dx_w = 0; float dy_w = 0;
float dy_w = 0; Vector3f wind = _ahrs.wind_estimate();
Vector3f wind = _ahrs.wind_estimate(); get_wind_corrected_drift(&current_loc, &wind, &dx_w, &dy_w, &dx, &dy);
get_wind_corrected_drift(&current_loc, &wind, &dx_w, &dy_w, &dx, &dy);
#if (0) // write log - save the data.
// Print32_t filter info for debugging AP::logger().Write("SOAR", "TimeUS,nettorate,dx,dy,x0,x1,x2,x3,lat,lng,alt,dx_w,dy_w", "QfffffffLLfff",
int32_t i; AP_HAL::micros64(),
for (i = 0; i < 4; i++) { (double)_vario.reading,
gcs().send_text(MAV_SEVERITY_INFO, "%e ", (double)_ekf.P[i][i]); (double)dx,
} (double)dy,
for (i = 0; i < 4; i++) { (double)_ekf.X[0],
gcs().send_text(MAV_SEVERITY_INFO, "%e ", (double)_ekf.X[i]); (double)_ekf.X[1],
} (double)_ekf.X[2],
#endif (double)_ekf.X[3],
current_loc.lat,
current_loc.lng,
(double)_vario.alt,
(double)dx_w,
(double)dy_w);
// write log - save the data. //log_data();
AP::logger().Write("SOAR", "TimeUS,nettorate,dx,dy,x0,x1,x2,x3,lat,lng,alt,dx_w,dy_w", "QfffffffLLfff", _ekf.update(_vario.reading,dx, dy); // update the filter
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(); _prev_update_location = current_loc; // save for next time
_ekf.update(_vario.reading,dx, dy); // update the filter _prev_update_time = AP_HAL::micros64();
_prev_update_location = current_loc; // save for next time
_prev_update_time = AP_HAL::micros64();
_vario.new_data = false;
}
} }
void SoaringController::update_cruising() void SoaringController::update_cruising()

4
libraries/AP_Soaring/Variometer.cpp
View File

@ -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) : Variometer::Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
_ahrs(ahrs), _ahrs(ahrs),
_aparm(parms), _aparm(parms)
new_data(false)
{ {
} }
@ -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; displayed_reading = TE_FILT_DISPLAYED * reading + (1 - TE_FILT_DISPLAYED) * displayed_reading;
_prev_update_time = AP_HAL::micros64(); _prev_update_time = AP_HAL::micros64();
new_data = true;
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff", AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff",
AP_HAL::micros64(), AP_HAL::micros64(),

1
libraries/AP_Soaring/Variometer.h
View File

@ -40,7 +40,6 @@ public:
float reading; float reading;
float filtered_reading; float filtered_reading;
float displayed_reading; float displayed_reading;
bool new_data;
void update(const float polar_K, const float polar_CD0, const float polar_B); 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); float correct_netto_rate(float climb_rate, float phi, float aspd, const float polar_K, const float polar_CD0, const float polar_B);