2017-02-26 19:13:46 -04:00
|
|
|
/*
|
|
|
|
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 <AP_Math/AP_Math.h>
|
|
|
|
#include "ExtendedKalmanFilter.h"
|
2017-04-27 18:44:03 -03:00
|
|
|
#include "Variometer.h"
|
2017-02-26 19:13:46 -04:00
|
|
|
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
|
|
|
|
|
|
|
|
#define INITIAL_THERMAL_STRENGTH 2.0
|
2019-07-07 09:10:22 -03:00
|
|
|
#define INITIAL_THERMAL_RADIUS 80.0
|
2017-02-26 19:13:46 -04:00
|
|
|
#define INITIAL_STRENGTH_COVARIANCE 0.0049
|
2019-07-07 09:10:22 -03:00
|
|
|
#define INITIAL_RADIUS_COVARIANCE 400.0
|
|
|
|
#define INITIAL_POSITION_COVARIANCE 400.0
|
2017-04-27 18:44:03 -03:00
|
|
|
|
2017-02-26 19:13:46 -04:00
|
|
|
|
|
|
|
class SoaringController {
|
|
|
|
ExtendedKalmanFilter _ekf{};
|
|
|
|
AP_SpdHgtControl &_spdHgt;
|
2017-04-27 18:44:03 -03:00
|
|
|
Variometer _vario;
|
2019-05-16 07:39:56 -03:00
|
|
|
const AP_Vehicle::FixedWing &_aparm;
|
2017-02-26 19:13:46 -04:00
|
|
|
|
|
|
|
// store aircraft location at last update
|
2019-03-29 11:10:11 -03:00
|
|
|
Vector3f _prev_update_location;
|
2017-02-26 19:13:46 -04:00
|
|
|
|
|
|
|
// store time thermal was entered for hysteresis
|
2019-08-07 19:33:06 -03:00
|
|
|
uint64_t _thermal_start_time_us;
|
2017-02-26 19:13:46 -04:00
|
|
|
|
2019-06-08 22:10:35 -03:00
|
|
|
// store position thermal was entered as a backup check
|
|
|
|
Vector3f _thermal_start_pos;
|
2019-06-07 21:19:42 -03:00
|
|
|
|
2017-02-26 19:13:46 -04:00
|
|
|
// store time cruise was entered for hysteresis
|
2019-08-07 19:33:06 -03:00
|
|
|
uint64_t _cruise_start_time_us;
|
2017-02-26 19:13:46 -04:00
|
|
|
|
|
|
|
// store time of last update
|
2019-08-07 19:33:06 -03:00
|
|
|
uint64_t _prev_update_time;
|
2017-02-26 19:13:46 -04:00
|
|
|
|
|
|
|
bool _throttle_suppressed;
|
|
|
|
|
|
|
|
float McCready(float alt);
|
|
|
|
|
2019-06-22 13:02:35 -03:00
|
|
|
float _thermalability;
|
|
|
|
float _expected_sink;
|
|
|
|
|
2020-01-11 11:14:28 -04:00
|
|
|
LowPassFilter<float> _position_x_filter;
|
|
|
|
LowPassFilter<float> _position_y_filter;
|
|
|
|
|
2017-02-26 19:13:46 -04:00
|
|
|
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;
|
2019-06-08 22:10:35 -03:00
|
|
|
AP_Float max_drift;
|
|
|
|
|
2017-02-26 19:13:46 -04:00
|
|
|
public:
|
2020-05-05 01:41:17 -03:00
|
|
|
SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
|
2017-02-26 19:13:46 -04:00
|
|
|
|
2020-04-05 08:38:19 -03:00
|
|
|
enum class LoiterStatus {
|
|
|
|
DISABLED,
|
2019-06-05 15:23:29 -03:00
|
|
|
ALT_TOO_HIGH,
|
|
|
|
ALT_TOO_LOW,
|
|
|
|
THERMAL_WEAK,
|
2019-06-07 21:19:42 -03:00
|
|
|
ALT_LOST,
|
2019-06-09 07:01:16 -03:00
|
|
|
DRIFT_EXCEEDED,
|
2020-04-05 08:38:19 -03:00
|
|
|
GOOD_TO_KEEP_LOITERING
|
|
|
|
};
|
2019-06-05 15:23:29 -03:00
|
|
|
|
2020-04-05 08:38:19 -03:00
|
|
|
enum class ActiveStatus {
|
2020-07-13 05:46:51 -03:00
|
|
|
SOARING_DISABLED,
|
2020-04-05 08:38:19 -03:00
|
|
|
MANUAL_MODE_CHANGE,
|
|
|
|
AUTO_MODE_CHANGE
|
|
|
|
};
|
2019-08-18 12:02:15 -03:00
|
|
|
|
2019-06-17 09:15:53 -03:00
|
|
|
AP_Float max_radius;
|
|
|
|
|
2017-02-26 19:13:46 -04:00
|
|
|
// 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();
|
2019-06-22 14:51:17 -03:00
|
|
|
LoiterStatus check_cruise_criteria(Vector2f prev_wp, Vector2f next_wp);
|
2017-02-26 19:13:46 -04:00
|
|
|
void init_thermalling();
|
|
|
|
void init_cruising();
|
|
|
|
void update_thermalling();
|
|
|
|
void update_cruising();
|
2018-10-18 09:51:21 -03:00
|
|
|
void set_throttle_suppressed(bool suppressed);
|
|
|
|
|
2017-03-03 05:16:40 -04:00
|
|
|
bool get_throttle_suppressed() const
|
2017-02-26 19:13:46 -04:00
|
|
|
{
|
|
|
|
return _throttle_suppressed;
|
|
|
|
}
|
2018-10-18 09:51:21 -03:00
|
|
|
|
2018-01-28 19:03:36 -04:00
|
|
|
float get_vario_reading() const
|
2017-02-26 19:13:46 -04:00
|
|
|
{
|
2017-04-27 18:44:03 -03:00
|
|
|
return _vario.displayed_reading;
|
2017-02-26 19:13:46 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void update_vario();
|
2019-06-07 12:07:06 -03:00
|
|
|
|
2019-06-22 14:51:17 -03:00
|
|
|
bool check_drift(Vector2f prev_wp, Vector2f next_wp);
|
|
|
|
|
2020-04-05 08:38:19 -03:00
|
|
|
void update_active_state();
|
2019-08-18 12:02:15 -03:00
|
|
|
|
2020-04-05 08:38:19 -03:00
|
|
|
bool is_active() const {return _last_update_status>=SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;};
|
2019-07-15 07:27:29 -03:00
|
|
|
|
2020-07-13 05:46:51 -03:00
|
|
|
void set_pilot_desired_state(ActiveStatus pilot_desired_state) {_pilot_desired_state = pilot_desired_state;};
|
|
|
|
|
2019-06-07 12:07:06 -03:00
|
|
|
private:
|
|
|
|
// slow down messages if they are the same. During loiter we could smap the same message. Only show new messages during loiters
|
|
|
|
LoiterStatus _cruise_criteria_msg_last;
|
2020-04-05 08:38:19 -03:00
|
|
|
|
|
|
|
ActiveStatus _last_update_status;
|
|
|
|
|
2020-07-13 05:46:51 -03:00
|
|
|
ActiveStatus _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE;
|
|
|
|
|
2020-04-05 08:38:19 -03:00
|
|
|
ActiveStatus active_state() const;
|
2017-02-26 19:13:46 -04:00
|
|
|
};
|