AP_Soaring: Override airspeed and flap angle.

This commit is contained in:
Samuel Tabor 2020-07-10 15:14:23 +01:00 committed by Andrew Tridgell
parent 3ab9dda6f5
commit c44fed34d4
4 changed files with 72 additions and 0 deletions

View File

@ -194,6 +194,27 @@ void Plane::calc_airspeed_errors()
#endif // OFFBOARD_GUIDED == ENABLED
#if HAL_SOARING_ENABLED
} else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
if (control_mode == &mode_thermal) {
float arspd = g2.soaring_controller.get_thermalling_target_airspeed();
if (arspd > 0) {
target_airspeed_cm = arspd * 100;
} else {
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
} else if (control_mode == &mode_auto) {
float arspd = g2.soaring_controller.get_cruising_target_airspeed();
if (arspd > 0) {
target_airspeed_cm = arspd * 100;
} else {
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
}
#endif
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm();

View File

@ -629,6 +629,12 @@ void Plane::set_servos_flaps(void)
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
#if HAL_SOARING_ENABLED
if (control_mode == &mode_thermal) {
auto_flap_percent = g2.soaring_controller.get_thermalling_flap();
}
#endif
/*
special flap levels for takeoff and landing. This works
better than speed based flaps as it leads to less

View File

@ -139,6 +139,29 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @Units: deg
AP_GROUPINFO("THML_BANK", 18, SoaringController, thermal_bank, 30.0),
// 19 reserved for POLAR_LEARN.
// @Param: THML_ARSPD
// @DisplayName: Specific setting for airspeed when thermalling.
// @Description: If non-zero this airspeed will be used when thermalling.
// @Range: 5 50
// @User: Advanced
AP_GROUPINFO("THML_ARSPD", 20, SoaringController, soar_thermal_airspeed, 0),
// @Param: CRSE_ARSPD
// @DisplayName: Specific setting for airspeed when cruising.
// @Description: If non-zero this airspeed will be used when cruising.
// @Range: 5 50
// @User: Advanced
AP_GROUPINFO("CRSE_ARSPD", 21, SoaringController, soar_cruise_airspeed, 0),
// @Param: THML_FLAP
// @DisplayName: Flap percent to be used during thermalling flight.
// @Description: This sets the flap when in LOITER with soaring active. Overrides the usual auto flap behaviour.
// @Range: 0 100
// @User: Advanced
AP_GROUPINFO("THML_FLAP", 22, SoaringController, soar_thermal_flap, 0),
AP_GROUPEND
};
@ -469,4 +492,14 @@ float SoaringController::get_thermalling_radius() const
return radius;
}
float SoaringController::get_thermalling_target_airspeed()
{
return soar_thermal_airspeed;
}
float SoaringController::get_cruising_target_airspeed()
{
return soar_cruise_airspeed;
}
#endif // HAL_SOARING_ENABLED

View File

@ -74,6 +74,9 @@ protected:
AP_Float alt_cutoff;
AP_Float max_drift;
AP_Float thermal_bank;
AP_Float soar_thermal_airspeed;
AP_Float soar_cruise_airspeed;
AP_Float soar_thermal_flap;
public:
SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms);
@ -135,6 +138,15 @@ public:
float get_thermalling_radius() const;
float get_thermalling_target_airspeed();
float get_cruising_target_airspeed();
float get_thermalling_flap() const
{
return soar_thermal_flap;
}
private:
ActiveStatus _last_update_status;