Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: added AP_Soar library, formerly known as SoaringController

Browse Source
This commit is contained in:
Andrey Kolobov 2017-02-27 10:13:46 +11:00 committed by Andrew Tridgell
parent ddfe55849e
commit ea534f0445
4 changed files with 606 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

386
libraries/AP_Soaring/AP_Soaring.cpp Normal file
View File

@ -0,0 +1,386 @@
#include "AP_Soaring.h"
#include <GCS_MAVLink/GCS.h>
#include <stdint.h>
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<alt_min)) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ALERT, "Out of allowable altitude range, beginning cruise. Alt = %f\n", (double)_alt);
return true;
}
return false;
}
bool SoaringController::check_init_thermal_criteria()
{
if (soar_active && (AP_HAL::micros64() - _thermal_start_time_us) < ((unsigned)min_thermal_s * 1e6)) {
return true;
}
return false;
}
void SoaringController::init_thermalling()
{
// Calc filter matrices - so that changes to parameters can be updated by switching in and out of thermal mode
float r = powf(thermal_r, 2);
float cov_q1 = powf(thermal_q1, 2); // State covariance
float cov_q2 = powf(thermal_q2, 2); // State covariance
const float init_q[4] = {cov_q1, cov_q2, cov_q2, cov_q2};
const MatrixN<4> 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<float,4> 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(&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_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(&current_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;
}

113
libraries/AP_Soaring/AP_Soaring.h Normal file
View File

@ -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 <AP_AHRS/AP_AHRS.h>
#include <AP_Param/AP_Param.h>
#include <DataFlash/DataFlash.h>
#include <AP_Math/AP_Math.h>
#include "ExtendedKalmanFilter.h"
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#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();
};

80
libraries/AP_Soaring/ExtendedKalmanFilter.cpp Normal file
View File

@ -0,0 +1,80 @@
#include "ExtendedKalmanFilter.h"
#include "AP_Math/matrixN.h"
float ExtendedKalmanFilter::measurementpredandjacobian(VectorN<float,N> &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<float,N> &x, const MatrixN<N> &p, const MatrixN<N> q, float r)
{
P = p;
X = x;
Q = q;
R = r;
}
void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
{
MatrixN<N> tempM;
VectorN<float,N> tempV;
VectorN<float,N> H;
VectorN<float,N> P12;
VectorN<float,N> 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();
}

27
libraries/AP_Soaring/ExtendedKalmanFilter.h Normal file
View File

@ -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 <AP_Math/matrixN.h>
#define N 4
class ExtendedKalmanFilter {
public:
ExtendedKalmanFilter(void) {}
VectorN<float,N> X;
MatrixN<N> P;
MatrixN<N> Q;
float R;
void reset(const VectorN<float,N> &x, const MatrixN<N> &p, const MatrixN<N> q, float r);
void update(float z, float Vx, float Vy);
private:
float measurementpredandjacobian(VectorN<float,N> &A);
};