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;
_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(&current_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(&current_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()

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) :
_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(),

1
libraries/AP_Soaring/Variometer.h
View File

@ -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);