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

AP_Soaring: Separate the vario functionality into a seperate class from the soaring/thermalling functionality.

This commit is contained in:
samuelctabor 2017-04-27 22:44:03 +01:00 committed by Andrew Tridgell
parent c9e140a946
commit 4cbff71689
4 changed files with 156 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -132,9 +132,9 @@ SoaringController::SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, co
_ahrs(ahrs),
_spdHgt(spdHgt),
_aparm(parms),
_new_data(false),
_loiter_rad(parms.loiter_radius),
_throttle_suppressed(true)
_throttle_suppressed(true),
_vario(ahrs,spdHgt,parms)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -161,7 +161,7 @@ bool SoaringController::suppress_throttle()
_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;
_vario.filtered_reading = 0;
}
return _throttle_suppressed;
@ -171,20 +171,21 @@ 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);
&& _vario.filtered_reading > thermal_vspeed
&& _vario.alt < alt_max
&& _vario.alt > alt_min);
}
bool SoaringController::check_cruise_criteria()
{
float thermalability = (_ekf.X[0]*expf(-powf(_loiter_rad / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK;
float alt = _vario.alt;
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));
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);
} 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;
}
@ -259,7 +260,7 @@ void SoaringController::update_thermalling()
struct Location current_loc;
_ahrs.get_position(current_loc);
if (_new_data) {
if (_vario.new_data) {
float dx = 0;
float dy = 0;
float dx_w = 0;
@ -281,7 +282,7 @@ void SoaringController::update_thermalling()
// 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)_vario.reading,
(double)dx,
(double)dy,
(double)_ekf.X[0],
@ -290,16 +291,16 @@ void SoaringController::update_thermalling()
(double)_ekf.X[3],
current_loc.lat,
current_loc.lng,
(double)_alt,
(double)_vario.alt,
(double)dx_w,
(double)dy_w);
//log_data();
_ekf.update(_vario_reading,dx, dy); // update the filter
_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;
_vario.new_data = false;
}
}
@ -311,75 +312,9 @@ void SoaringController::update_cruising()
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);
}
_vario.update(polar_K, polar_CD0, polar_B);
}
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)
{

22
libraries/AP_Soaring/AP_Soaring.h
View File

@ -14,6 +14,7 @@
#include <DataFlash/DataFlash.h>
#include <AP_Math/AP_Math.h>
#include "ExtendedKalmanFilter.h"
#include "Variometer.h"
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#define EXPECTED_THERMALLING_SINK 0.7
@ -22,15 +23,14 @@
#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;
Variometer _vario;
// store aircraft location at last update
struct Location _prev_update_location;
@ -44,23 +44,9 @@ class SoaringController {
// 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);
@ -107,7 +93,7 @@ public:
}
float get_vario_reading()
{
return _displayed_vario_reading;
return _vario.displayed_reading;
}
void update_vario();

88
libraries/AP_Soaring/Variometer.cpp Normal file
View File

@ -0,0 +1,88 @@
/* Variometer class by Samuel Tabor
Manages the estimation of aircraft total energy, drag and vertical air velocity.
*/
#include "Variometer.h"
Variometer::Variometer(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms) :
_ahrs(ahrs),
_spdHgt(spdHgt),
_aparm(parms),
new_data(false)
{
}
void Variometer::update(const float polar_K, const float polar_B, const float polar_Cd0)
{
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, polar_K, polar_Cd0, polar_B); // Compute still-air sinkrate
reading = (total_E - _last_total_E) / ((AP_HAL::micros64() - _prev_update_time) * 1e-6) + sinkrate; // Unfiltered netto rate
filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise
displayed_reading = TE_FILT_DISPLAYED * reading + (1 - TE_FILT_DISPLAYED) * displayed_reading;
_last_alt = alt; // Store variables
_last_roll = roll;
_last_aspd = aspd;
_last_total_E = total_E;
_prev_update_time = AP_HAL::micros64();
new_data = true;
DataFlash_Class::instance()->Log_Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffffffff",
AP_HAL::micros64(),
(double)aspd,
(double)_aspd_filt,
(double)alt,
(double)roll,
(double)reading,
(double)filtered_reading);
}
}
float Variometer::correct_netto_rate(float climb_rate,
float phi,
float aspd,
const float polar_K,
const float polar_CD0,
const float polar_B)
{
// 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;
}
void Variometer::get_altitude_wrt_home(float *alt)
{
_ahrs.get_relative_position_D_home(*alt);
*alt *= -1.0f;
}

47
libraries/AP_Soaring/Variometer.h Normal file
View File

@ -0,0 +1,47 @@
/* Variometer class by Samuel Tabor
Manages the estimation of aircraft total energy, drag and vertical air velocity.
*/
#pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Param/AP_Param.h>
#include <DataFlash/DataFlash.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#define ASPD_FILT 0.05
#define TE_FILT 0.03
#define TE_FILT_DISPLAYED 0.15
class Variometer {
AP_AHRS &_ahrs;
AP_SpdHgtControl &_spdHgt;
const AP_Vehicle::FixedWing &_aparm;
// store time of last update
unsigned long _prev_update_time;
float _last_alt;
float _aspd_filt;
float _last_aspd;
float _last_roll;
float _last_total_E;
void get_altitude_wrt_home(float *alt);
public:
Variometer(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
float alt;
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);
};