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

AP_TECS: Add flags to indicate gliding flight, and use these with AP_Soaring.

Browse Source
This commit is contained in:
Samuel Tabor 2018-10-18 13:51:21 +01:00 committed by Andrew Tridgell
parent dc8c062fbe
commit 684ee11fc3
9 changed files with 75 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/ArduPlane.cpp
View File

@ -577,13 +577,6 @@ void Plane::update_alt()
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
}
bool soaring_active = false;
#if SOARING_ENABLED == ENABLED
if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
soaring_active = true;
}
#endif
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
@ -592,8 +585,7 @@ void Plane::update_alt()
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor,
soaring_active);
aerodynamic_load_factor);
}
}

3
ArduPlane/mode_loiter.cpp
View File

@ -10,7 +10,8 @@ bool ModeLoiter::_enter()
plane.loiter_angle_reset();
#if SOARING_ENABLED == ENABLED
if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) {
if (plane.g2.soaring_controller.is_active() &&
(plane.g2.soaring_controller.suppress_throttle() || plane.aparm.throttle_max==0)) {
plane.g2.soaring_controller.init_thermalling();
plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
}

10
ArduPlane/servos.cpp
View File

@ -470,16 +470,6 @@ void Plane::set_servos_controlled(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle));
}
#if SOARING_ENABLED == ENABLED
// suppress throttle when soaring is active
if ((control_mode == &mode_fbwb || control_mode == &mode_cruise ||
control_mode == &mode_auto || control_mode == &mode_loiter) &&
g2.soaring_controller.is_active() &&
g2.soaring_controller.get_throttle_suppressed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
#endif
}
/*

12
ArduPlane/soaring.cpp
View File

@ -10,6 +10,8 @@
void Plane::update_soaring() {
if (!g2.soaring_controller.is_active()) {
// This also sets the TECS gliding_requested to false.
g2.soaring_controller.set_throttle_suppressed(false);
return;
}
@ -22,18 +24,18 @@ void Plane::update_soaring() {
break;
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
if (!g2.soaring_controller.suppress_throttle()) {
if (!g2.soaring_controller.suppress_throttle() && aparm.throttle_max > 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
set_mode(mode_rtl, ModeReason::SOARING_FBW_B_WITH_MOTOR_RUNNING);
}
break;
case Mode::Number::LOITER:
// Do nothing. We will switch back to auto/rtl before enabling throttle.
// Never use throttle in LOITER with soaring active.
g2.soaring_controller.set_throttle_suppressed(true);
break;
default:
// This does not affect the throttle since suppressed is only checked in the above three modes.
// It ensures that the soaring always starts with throttle suppressed though.
g2.soaring_controller.set_throttle_suppressed(true);
// In any other mode allow throttle.
g2.soaring_controller.set_throttle_suppressed(false);
break;
}

18
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -153,12 +153,14 @@ bool SoaringController::suppress_throttle()
if (_throttle_suppressed && (alt < alt_min)) {
// Time to throttle up
_throttle_suppressed = false;
set_throttle_suppressed(false);
} else if ((!_throttle_suppressed) && (alt > alt_cutoff)) {
// Start glide
_throttle_suppressed = true;
set_throttle_suppressed(true);
// Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide.
_spdHgt.reset_pitch_I();
_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.
@ -275,10 +277,10 @@ void SoaringController::init_thermalling()
void SoaringController::init_cruising()
{
if (is_active() && suppress_throttle()) {
if (is_active()) {
_cruise_start_time_us = AP_HAL::micros64();
// Start glide. Will be updated on the next loop.
_throttle_suppressed = true;
set_throttle_suppressed(true);
}
}
@ -344,3 +346,11 @@ bool SoaringController::is_active() const
// active when above 1700
return RC_Channels::get_radio_in(soar_active_ch-1) >= 1700;
}
void SoaringController::set_throttle_suppressed(bool suppressed)
{
_throttle_suppressed = suppressed;
// Let the TECS know.
_spdHgt.set_gliding_requested_flag(suppressed);
}

7
libraries/AP_Soaring/AP_Soaring.h
View File

@ -93,14 +93,13 @@ public:
void update_thermalling();
void update_cruising();
bool is_active() const;
void set_throttle_suppressed(bool suppressed);
bool get_throttle_suppressed() const
{
return _throttle_suppressed;
}
void set_throttle_suppressed(bool suppressed)
{
_throttle_suppressed = suppressed;
}
float get_vario_reading() const
{
return _vario.displayed_reading;

9
libraries/AP_SpdHgtControl/AP_SpdHgtControl.h
View File

@ -31,8 +31,7 @@ public:
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor,
bool soaring_active) = 0;
float load_factor) = 0;
// demanded throttle in percentage
// should return 0 to 100
@ -66,6 +65,12 @@ public:
// reset on next loop
virtual void reset(void) = 0;
// set gliding requested flag
virtual void set_gliding_requested_flag(bool gliding_requested) = 0;
// set propulsion failed flag
virtual void set_propulsion_failed_flag(bool propulsion_failed) = 0;
// add new controllers to this enum. Users can then
// select which controller to use by setting the
// SPDHGT_CONTROLLER parameter

24
libraries/AP_TECS/AP_TECS.cpp
View File

@ -641,6 +641,10 @@ void AP_TECS::_update_throttle_with_airspeed(void)
{
_throttle_dem = 1.0f;
}
else if (_flags.is_gliding)
{
_throttle_dem = 0.0f;
}
else
{
// Calculate gain scaler from specific energy error to throttle
@ -755,6 +759,11 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
_throttle_dem = nomThr;
}
if (_flags.is_gliding)
{
_throttle_dem = 0.0f;
}
// Calculate additional throttle for turn drag compensation including throttle nudging
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
@ -780,7 +789,7 @@ void AP_TECS::_detect_bad_descent(void)
// 2) Specific total energy error > 0
// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
float STEdot = _SPEdot + _SKEdot;
if ((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f)))
if (((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f))) && !_flags.is_gliding)
{
_flags.badDescent = true;
}
@ -807,7 +816,7 @@ void AP_TECS::_update_pitch(void)
// height. This is needed as the usual relationship of speed
// and height is broken by the VTOL motors
SKE_weighting = 0.0f;
} else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
} else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND || _flags.is_gliding) {
SKE_weighting = 2.0f;
} else if (_flags.is_doing_auto_land) {
if (_spdWeightLand < 0) {
@ -902,7 +911,9 @@ void AP_TECS::_update_pitch(void)
// Add a feedforward term from demanded airspeed to pitch.
_pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
if (_flags.is_gliding) {
_pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
}
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
@ -980,14 +991,14 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor,
bool soaring_active)
float load_factor)
{
// Calculate time in seconds since last update
uint64_t now = AP_HAL::micros64();
_DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
_update_pitch_throttle_last_usec = now;
_flags.is_gliding = _flags.gliding_requested || _flags.propulsion_failed || aparm.throttle_max==0;
_flags.is_doing_auto_land = (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
_distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage;
@ -1141,8 +1152,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// Detect bad descent due to demanded airspeed being too high
_detect_bad_descent();
// when soaring is active we never trigger a bad descent
if (soaring_active || (_options & OPTION_GLIDER_ONLY)) {
if (_options & OPTION_GLIDER_ONLY) {
_flags.badDescent = false;
}

26
libraries/AP_TECS/AP_TECS.h
View File

@ -52,8 +52,7 @@ public:
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor,
bool soaring_active) override;
float load_factor) override;
// demanded throttle in percentage
// should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0
@ -107,6 +106,17 @@ public:
_path_proportion = constrain_float(path_proportion, 0.0f, 1.0f);
}
// set soaring flag
void set_gliding_requested_flag(bool gliding_requested) override {
_flags.gliding_requested = gliding_requested;
}
// set propulsion failed flag
void set_propulsion_failed_flag(bool propulsion_failed) override {
_flags.propulsion_failed = propulsion_failed;
}
// set pitch max limit in degrees
void set_pitch_max_limit(int8_t pitch_limit) {
_pitch_max_limit = pitch_limit;
@ -272,6 +282,18 @@ private:
// true when we have reached target speed in takeoff
bool reached_speed_takeoff:1;
// true if the soaring feature has requested gliding flight
bool gliding_requested:1;
// true when we are in gliding flight, in one of three situations;
// - THR_MAX=0
// - gliding has been requested e.g. by soaring feature
// - engine failure detected (detection not implemented currently)
bool is_gliding:1;
// true if a propulsion failure is detected.
bool propulsion_failed:1;
};
union {
struct flags _flags;