mirror of https://github.com/ArduPilot/ardupilot
115 lines
3.2 KiB
C++
115 lines
3.2 KiB
C++
/*
|
|
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_Int8 soar_active_ch;
|
|
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() const;
|
|
bool get_throttle_suppressed() const
|
|
{
|
|
return _throttle_suppressed;
|
|
}
|
|
void set_throttle_suppressed(bool suppressed)
|
|
{
|
|
_throttle_suppressed = suppressed;
|
|
}
|
|
float get_vario_reading()
|
|
{
|
|
return _displayed_vario_reading;
|
|
}
|
|
|
|
void update_vario();
|
|
};
|