aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Soaring: Add SOAR_THML_BANK parameter and associated logic.

Browse Source
This commit is contained in:
Samuel Tabor 2021-01-20 21:07:31 +00:00 committed by Tom Pittenger
parent 4fbba17506
commit 4ec1e55833
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -131,6 +131,13 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("MAX_RADIUS", 17, SoaringController, max_radius, -1), AP_GROUPINFO("MAX_RADIUS", 17, SoaringController, max_radius, -1),
// @Param: THML_BANK
// @DisplayName: Thermalling bank angle
// @Description: This parameter sets the bank angle to use when thermalling. Typically 30 - 45 degrees works well.
// @Range: 20 50
// @User: Advanced
AP_GROUPINFO("THML_BANK", 18, SoaringController, thermal_bank, 30.0),
AP_GROUPEND AP_GROUPEND
}; };
@ -312,7 +319,7 @@ void SoaringController::update_thermalling()
_ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y); _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y);
_thermalability = (_ekf.X[0]*expf(-powf(_aparm.loiter_radius / _ekf.X[1], 2))) - _vario.get_exp_thermalling_sink(); _thermalability = (_ekf.X[0]*expf(-powf(get_thermalling_radius()/_ekf.X[1], 2))) - _vario.get_exp_thermalling_sink();
_prev_update_time = AP_HAL::micros64(); _prev_update_time = AP_HAL::micros64();
@ -472,4 +479,13 @@ bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp)
} }
} }
float SoaringController::get_thermalling_radius() const
{
// Thermalling radius is controlled by parameter SOAR_THML_BANK and true target airspeed.
const float target_aspd = _spdHgt.get_target_airspeed() * AP::ahrs().get_EAS2TAS();
const float radius = (target_aspd*target_aspd) / (GRAVITY_MSS * tanf(thermal_bank*DEG_TO_RAD));
return radius;
}
#endif // HAL_SOARING_ENABLED #endif // HAL_SOARING_ENABLED

3
libraries/AP_Soaring/AP_Soaring.h
View File

@ -75,6 +75,7 @@ protected:
AP_Float alt_min; AP_Float alt_min;
AP_Float alt_cutoff; AP_Float alt_cutoff;
AP_Float max_drift; AP_Float max_drift;
AP_Float thermal_bank;
public: public:
SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms); SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
@ -134,6 +135,8 @@ public:
float get_circling_time() const {return _vario.tau;} float get_circling_time() const {return _vario.tau;}
float get_thermalling_radius() const;
private: private:
// slow down messages if they are the same. During loiter we could smap the same message. Only show new messages during loiters // 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; LoiterStatus _cruise_criteria_msg_last;