Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Avoid calculations in lat/lng.

This commit is contained in:
Samuel Tabor 2019-03-29 14:10:11 +00:00 committed by Andrew Tridgell
parent 0cccc8dc29
commit d7abd296ef
2 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -136,7 +136,7 @@ SoaringController::SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, co
void SoaringController::get_target(Location &wp)
{
wp = _prev_update_location;
_ahrs.get_position(wp);
wp.offset(_ekf.X[2], _ekf.X[3]);
}
@ -219,6 +219,12 @@ void SoaringController::init_thermalling()
const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE, INITIAL_RADIUS_COVARIANCE, INITIAL_POSITION_COVARIANCE, INITIAL_POSITION_COVARIANCE};
const MatrixN<float,4> p{init_p};
Vector3f position;
if (!_ahrs.get_relative_position_NED_home(position)) {
return;
}
// New state vector filter will be reset. Thermal location is placed in front of a/c
const float init_xr[4] = {INITIAL_THERMAL_STRENGTH,
INITIAL_THERMAL_RADIUS,
@ -229,7 +235,7 @@ void SoaringController::init_thermalling()
// Also reset covariance matrix p so filter is not affected by previous data
_ekf.reset(xr, p, q, r);
_ahrs.get_position(_prev_update_location);
_prev_update_location = position;
_prev_update_time = AP_HAL::micros64();
_thermal_start_time_us = AP_HAL::micros64();
}
@ -243,9 +249,9 @@ void SoaringController::init_cruising()
}
}
void SoaringController::get_wind_corrected_drift(const Location *current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy)
void SoaringController::get_wind_corrected_drift(Vector3f current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy)
{
const Vector2f diff = _prev_update_location.get_distance_NE(*current_loc); // get distances from previous update
Vector3f diff = current_loc - _prev_update_location; // get distances from previous update
*dx = diff.x;
*dy = diff.y;
@ -263,15 +269,21 @@ void SoaringController::get_altitude_wrt_home(float *alt)
}
void SoaringController::update_thermalling()
{
struct Location current_loc;
_ahrs.get_position(current_loc);
Vector3f current_position;
if (!_ahrs.get_relative_position_NED_home(current_position)) {
return;
}
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);
get_wind_corrected_drift(current_position, &wind, &dx_w, &dy_w, &dx, &dy);
struct Location current_loc;
_ahrs.get_position(current_loc);
// 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",
@ -292,7 +304,7 @@ void SoaringController::update_thermalling()
//log_data();
_ekf.update(_vario.reading,dx, dy); // update the filter
_prev_update_location = current_loc; // save for next time
_prev_update_location = current_position; // save for next time
_prev_update_time = AP_HAL::micros64();
}

4
libraries/AP_Soaring/AP_Soaring.h
View File

@ -32,7 +32,7 @@ class SoaringController {
const AP_Vehicle::FixedWing &_aparm;
// store aircraft location at last update
struct Location _prev_update_location;
Vector3f _prev_update_location;
// store time thermal was entered for hysteresis
unsigned long _thermal_start_time_us;
@ -46,7 +46,7 @@ class SoaringController {
bool _throttle_suppressed;
float McCready(float alt);
void get_wind_corrected_drift(const Location *current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy);
void get_wind_corrected_drift(Vector3f current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy);
void get_altitude_wrt_home(float *alt);
protected: