From ea534f0445fde8442a1bf07fa209511521d04cb8 Mon Sep 17 00:00:00 2001 From: Andrey Kolobov Date: Mon, 27 Feb 2017 10:13:46 +1100 Subject: [PATCH] AP_Soaring: added AP_Soar library, formerly known as SoaringController --- libraries/AP_Soaring/AP_Soaring.cpp | 386 ++++++++++++++++++ libraries/AP_Soaring/AP_Soaring.h | 113 +++++ libraries/AP_Soaring/ExtendedKalmanFilter.cpp | 80 ++++ libraries/AP_Soaring/ExtendedKalmanFilter.h | 27 ++ 4 files changed, 606 insertions(+) create mode 100644 libraries/AP_Soaring/AP_Soaring.cpp create mode 100644 libraries/AP_Soaring/AP_Soaring.h create mode 100644 libraries/AP_Soaring/ExtendedKalmanFilter.cpp create mode 100644 libraries/AP_Soaring/ExtendedKalmanFilter.h diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp new file mode 100644 index 0000000000..4552f2acbb --- /dev/null +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -0,0 +1,386 @@ +#include "AP_Soaring.h" +#include +#include +extern const AP_HAL::HAL& hal; + + +// ArduSoar parameters +const AP_Param::GroupInfo SoaringController::var_info[] = { + // @Param: ENABLE + // @DisplayName: Is the soaring mode enabled or not + // @Description: Toggles the soaring mode on and off + // @Values: 0:Disable,1:Enable + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, SoaringController, soar_active, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: VSPEED + // @DisplayName: Vertical v-speed + // @Description: Rate of climb to trigger themalling speed + // @Units: m/s + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("VSPEED", 2, SoaringController, thermal_vspeed, 0.7f), + + // @Param: Q1 + // @DisplayName: Process noise + // @Description: Standard deviation of noise in process for strength + // @Units: + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("Q1", 3, SoaringController, thermal_q1, 0.001f), + + // @Param: Q2 + // @DisplayName: Process noise + // @Description: Standard deviation of noise in process for position and radius + // @Units: + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("Q2", 4, SoaringController, thermal_q2, 0.03f), + + // @Param: R + // @DisplayName: Measurement noise + // @Description: Standard deviation of noise in measurement + // @Units: + // @Range: 0 10 + // @User: Advanced + + AP_GROUPINFO("R", 5, SoaringController, thermal_r, 0.45f), + + // @Param: DIST_AHEAD + // @DisplayName: Distance to thermal center + // @Description: Initial guess of the distance to the thermal center + // @Units: metres + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("DIST_AHEAD", 6, SoaringController, thermal_distance_ahead, 5.0f), + + // @Param: MIN_THML_S + // @DisplayName: Minimum thermalling time + // @Description: Minimum number of seconds to spend thermalling + // @Units: seconds + // @Range: 0 32768 + // @User: Advanced + AP_GROUPINFO("MIN_THML_S", 7, SoaringController, min_thermal_s, 20), + + // @Param: MIN_CRSE_S + // @DisplayName: Minimum cruising time + // @Description: Minimum number of seconds to spend cruising + // @Units: seconds + // @Range: 0 32768 + // @User: Advanced + AP_GROUPINFO("MIN_CRSE_S", 8, SoaringController, min_cruise_s, 30), + + // @Param: POLAR_CD0 + // @DisplayName: Zero lift drag coef. + // @Description: Zero lift drag coefficient + // @Units: + // @Range: 0 0.5 + // @User: Advanced + AP_GROUPINFO("POLAR_CD0", 9, SoaringController, polar_CD0, 0.027), + + // @Param: POLAR_B + // @DisplayName: Induced drag coeffient + // @Description: Induced drag coeffient + // @Units: + // @Range: 0 0.5 + // @User: Advanced + AP_GROUPINFO("POLAR_B", 10, SoaringController, polar_B, 0.031), + + // @Param: POLAR_K + // @DisplayName: Cl factor + // @Description: Cl factor 2*m*g/(rho*S) + // @Units: m*m/s/s + // @Range: 0 0.5 + // @User: Advanced + AP_GROUPINFO("POLAR_K", 11, SoaringController, polar_K, 25.6), + + // @Param: ALT_MAX + // @DisplayName: Maximum soaring altitude, relative to the home location + // @Description: Don't thermal any higher than this. + // @Units: meters + // @Range: 0 1000.0 + // @User: Advanced + AP_GROUPINFO("ALT_MAX", 12, SoaringController, alt_max, 350.0), + + // @Param: ALT_MIN + // @DisplayName: Minimum soaring altitude, relative to the home location + // @Description: Don't get any lower than this. + // @Units: meters + // @Range: 0 1000.0 + // @User: Advanced + AP_GROUPINFO("ALT_MIN", 13, SoaringController, alt_min, 50.0), + + // @Param: ALT_CUTOFF + // @DisplayName: Maximum power altitude, relative to the home location + // @Description: Cut off throttle at this alt. + // @Units: meters + // @Range: 0 1000.0 + // @User: Advanced + AP_GROUPINFO("ALT_CUTOFF", 14, SoaringController, alt_cutoff, 250.0), + + AP_GROUPEND +}; + +SoaringController::SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms) : + _ahrs(ahrs), + _spdHgt(spdHgt), + _aparm(parms), + _new_data(false), + _loiter_rad(parms.loiter_radius), + _throttle_suppressed(true) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void SoaringController::get_target(Location &wp) +{ + wp = _prev_update_location; + location_offset(wp, _ekf.X[2], _ekf.X[3]); +} + +bool SoaringController::suppress_throttle() +{ + float alt = 0; + get_altitude_wrt_home(&alt); + + if (_throttle_suppressed && (alt < alt_min)) { + // Time to throttle up + _throttle_suppressed = false; + } else if ((!_throttle_suppressed) && (alt > alt_cutoff)) { + // Start glide + _throttle_suppressed = true; + // Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide. + _spdHgt.reset_pitch_I(); + _cruise_start_time_us = AP_HAL::micros64(); + // Reset the filtered vario rate - it is currently elevated due to the climb rate and would otherwise take a while to fall again, + // leading to false positives. + _filtered_vario_reading = 0; + } + + return _throttle_suppressed; +} + +bool SoaringController::check_thermal_criteria() +{ + return (soar_active + && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6)) + && _filtered_vario_reading > thermal_vspeed + && _alt < alt_max + && _alt > alt_min); +} + +bool SoaringController::check_cruise_criteria() +{ + float thermalability = (_ekf.X[0]*expf(-powf(_loiter_rad / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK; + + if (soar_active && (AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6) && thermalability < McCready(_alt)) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Thermal weak, recommend quitting: W %f R %f th %f alt %f Mc %f\n", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (double)_alt, (double)McCready(_alt)); + return true; + } else if (soar_active && (_alt>alt_max || _alt q{init_q}; + const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE, INITIAL_RADIUS_COVARIANCE, INITIAL_POSITION_COVARIANCE, INITIAL_POSITION_COVARIANCE}; + const MatrixN<4> p{init_p}; + + // 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, + thermal_distance_ahead * cosf(_ahrs.yaw), + thermal_distance_ahead * sinf(_ahrs.yaw)}; + const VectorN xr{init_xr}; + + // 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_time = AP_HAL::micros64(); + _thermal_start_time_us = AP_HAL::micros64(); +} + +void SoaringController::init_cruising() +{ + if (is_active() && suppress_throttle()) { + _cruise_start_time_us = AP_HAL::micros64(); + // Start glide. Will be updated on the next loop. + _throttle_suppressed = true; + } +} + +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) +{ + Vector2f diff = location_diff(_prev_update_location, *current_loc); // get distances from previous update + *dx = diff.x; + *dy = diff.y; + + // Wind correction + *wind_drift_x = wind->x * (AP_HAL::micros64() - _prev_update_time) * 1e-6; + *wind_drift_y = wind->y * (AP_HAL::micros64() - _prev_update_time) * 1e-6; + *dx -= *wind_drift_x; + *dy -= *wind_drift_y; +} + +void SoaringController::get_altitude_wrt_home(float *alt) +{ + _ahrs.get_relative_position_D_home(*alt); + *alt *= -1.0f; +} +void SoaringController::update_thermalling() +{ + struct Location current_loc; + _ahrs.get_position(current_loc); + + if (_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); + +#if (0) + // Print32_t filter info for debugging + int32_t i; + for (i = 0; i < 4; i++) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "%e ", (double)_ekf.P[i][i]); + } + for (i = 0; i < 4; i++) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "%e ", (double)_ekf.X[i]); + } +#endif + + // write log - save the data. + DataFlash_Class::instance()->Log_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], + (double)current_loc.lat, + (double)current_loc.lng, + (double)_alt, + (double)dx_w, + (double)dy_w); + + //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(); + _new_data = false; + } +} + +void SoaringController::update_cruising() +{ + // Reserved for future tasks that need to run continuously while in FBWB or AUTO mode, + // for example, calculation of optimal airspeed and flap angle. +} + +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); + } +} + +float SoaringController::correct_netto_rate(float climb_rate, float phi, float aspd) +{ + // Remove aircraft sink rate + float CL0; // CL0 = 2*W/(rho*S*V^2) + float C1; // C1 = CD0/CL0 + float C2; // C2 = CDi0/CL0 = B*CL0 + float netto_rate; + float cosphi; + CL0 = polar_K / (aspd * aspd); + C1 = polar_CD0 / CL0; // constant describing expected angle to overcome zero-lift drag + C2 = polar_B * CL0; // constant describing expected angle to overcome lift induced drag at zero bank + + cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi) + netto_rate = climb_rate + aspd * (C1 + C2 / (cosphi * cosphi)); // effect of aircraft drag removed + + // Remove acceleration effect - needs to be tested. + //float temp_netto = netto_rate; + //float dVdt = SpdHgt_Controller->get_VXdot(); + //netto_rate = netto_rate + aspd*dVdt/GRAVITY_MSS; + //GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "%f %f %f %f\n",temp_netto,dVdt,netto_rate,barometer.get_altitude()); + return netto_rate; +} + +float SoaringController::McCready(float alt) +{ + // A method shell to be filled in later + return thermal_vspeed; +} + +bool SoaringController::is_active() +{ + return soar_active; +} diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h new file mode 100644 index 0000000000..0956e8e410 --- /dev/null +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -0,0 +1,113 @@ +/* + Soaring Controller class by Samuel Tabor + + Provides a layer between the thermal centring algorithm and the main + code for managing navigation targets, data logging, tuning parameters, + algorithm inputs and eventually other soaring strategies such as + speed-to-fly. AP_TECS libary used for reference. +*/ + +#pragma once + +#include +#include +#include +#include +#include "ExtendedKalmanFilter.h" +#include + +#define EXPECTED_THERMALLING_SINK 0.7 +#define INITIAL_THERMAL_STRENGTH 2.0 +#define INITIAL_THERMAL_RADIUS 30.0 //150.0 +#define INITIAL_STRENGTH_COVARIANCE 0.0049 +#define INITIAL_RADIUS_COVARIANCE 2500.0 +#define INITIAL_POSITION_COVARIANCE 300.0 +#define ASPD_FILT 0.05 +#define TE_FILT 0.03 +#define TE_FILT_DISPLAYED 0.15 + +class SoaringController { + ExtendedKalmanFilter _ekf{}; + AP_AHRS &_ahrs; + AP_SpdHgtControl &_spdHgt; + const AP_Vehicle::FixedWing &_aparm; + + // store aircraft location at last update + struct Location _prev_update_location; + + // store time thermal was entered for hysteresis + unsigned long _thermal_start_time_us; + + // store time cruise was entered for hysteresis + unsigned long _cruise_start_time_us; + + // store time of last update + unsigned long _prev_update_time; + + // store time of last update + unsigned long _prev_vario_update_time; + + float _vario_reading; + float _filtered_vario_reading; + float _last_alt; + float _alt; + float _last_aspd; + float _last_roll; + float _last_total_E; + bool _new_data; + float _loiter_rad; + bool _throttle_suppressed; + + float _displayed_vario_reading; + float _aspd_filt; + float correct_netto_rate(float climb_rate, float phi, float aspd); + 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_altitude_wrt_home(float *alt); + +protected: + AP_Int8 soar_active; + AP_Float thermal_vspeed; + AP_Float thermal_q1; + AP_Float thermal_q2; + AP_Float thermal_r; + AP_Float thermal_distance_ahead; + AP_Int16 min_thermal_s; + AP_Int16 min_cruise_s; + AP_Float polar_CD0; + AP_Float polar_B; + AP_Float polar_K; + AP_Float alt_max; + AP_Float alt_min; + AP_Float alt_cutoff; + +public: + SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms); + + // this supports the TECS_* user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + void get_target(Location & wp); + bool suppress_throttle(); + bool check_thermal_criteria(); + bool check_cruise_criteria(); + bool check_init_thermal_criteria(); + void init_thermalling(); + void init_cruising(); + void update_thermalling(); + void update_cruising(); + bool is_active(); + bool get_throttle_suppressed() + { + return _throttle_suppressed; + } + void set_throttle_suppressed(bool suppressed) + { + _throttle_suppressed = suppressed; + } + float get_vario_reading() + { + return _displayed_vario_reading; + } + + void update_vario(); +}; diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.cpp b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp new file mode 100644 index 0000000000..6cd6a78bb7 --- /dev/null +++ b/libraries/AP_Soaring/ExtendedKalmanFilter.cpp @@ -0,0 +1,80 @@ +#include "ExtendedKalmanFilter.h" +#include "AP_Math/matrixN.h" + + +float ExtendedKalmanFilter::measurementpredandjacobian(VectorN &A) +{ + // This function computes the Jacobian using equations from + // analytical derivation of Gaussian updraft distribution + // This expression gets used lots + float expon = expf(- (powf(X[2], 2) + powf(X[3], 2)) / powf(X[1], 2)); + // Expected measurement + float w = X[0] * expon; + + // Elements of the Jacobian + A[0] = expon; + A[1] = 2 * X[0] * ((powf(X[2],2) + powf(X[3],2)) / powf(X[1],3)) * expon; + A[2] = -2 * (X[0] * X[2] / powf(X[1],2)) * expon; + A[3] = A[2] * X[3] / X[2]; + return w; +} + + +void ExtendedKalmanFilter::reset(const VectorN &x, const MatrixN &p, const MatrixN q, float r) +{ + P = p; + X = x; + Q = q; + R = r; +} + + +void ExtendedKalmanFilter::update(float z, float Vx, float Vy) +{ + MatrixN tempM; + VectorN tempV; + VectorN H; + VectorN P12; + VectorN K; + + // LINE 28 + // Estimate new state from old. + X[2] -= Vx; + X[3] -= Vy; + + // LINE 33 + // Update the covariance matrix + // P = A*ekf.P*A'+ekf.Q; + // We know A is identity so + // P = ekf.P+ekf.Q; + P += Q; + + // What measurement do we expect to receive in the estimated + // state + // LINE 37 + // [z1,H] = ekf.jacobian_h(x1); + float z1 = measurementpredandjacobian(H); + + // LINE 40 + // P12 = P * H'; + P.mult(H, P12); //cross covariance + + // LINE 41 + // Calculate the KALMAN GAIN + // K = P12 * inv(H*P12 + ekf.R); %Kalman filter gain + K = P12 * 1.0 / (H * P12 + R); + + // Correct the state estimate using the measurement residual. + // LINE 44 + // X = x1 + K * (z - z1); + X += K * (z - z1); + + // Correct the covariance too. + // LINE 46 + // NB should be altered to reflect Stengel + // P = P_predict - K * P12'; + tempM.mult(K, P12); + P -= tempM; + + P.force_symmetry(); +} diff --git a/libraries/AP_Soaring/ExtendedKalmanFilter.h b/libraries/AP_Soaring/ExtendedKalmanFilter.h new file mode 100644 index 0000000000..a9f1b7776d --- /dev/null +++ b/libraries/AP_Soaring/ExtendedKalmanFilter.h @@ -0,0 +1,27 @@ +/* +Extended Kalman Filter class by Sam Tabor, 2013. +* http://diydrones.com/forum/topics/autonomous-soaring +* Set up for identifying thermals of Gaussian form, but could be adapted to other +* purposes by adapting the equations for the jacobians. +*/ + +#pragma once + +#include + +#define N 4 + +class ExtendedKalmanFilter { +public: + ExtendedKalmanFilter(void) {} + + VectorN X; + MatrixN P; + MatrixN Q; + float R; + void reset(const VectorN &x, const MatrixN &p, const MatrixN q, float r); + void update(float z, float Vx, float Vy); + +private: + float measurementpredandjacobian(VectorN &A); +};