From 684ee11fc3d2754574ef249cfdbe3ffc3245b8cf Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Thu, 18 Oct 2018 13:51:21 +0100 Subject: [PATCH] AP_TECS: Add flags to indicate gliding flight, and use these with AP_Soaring. --- ArduPlane/ArduPlane.cpp | 10 +------ ArduPlane/mode_loiter.cpp | 3 ++- ArduPlane/servos.cpp | 10 ------- ArduPlane/soaring.cpp | 12 +++++---- libraries/AP_Soaring/AP_Soaring.cpp | 18 ++++++++++--- libraries/AP_Soaring/AP_Soaring.h | 7 +++-- libraries/AP_SpdHgtControl/AP_SpdHgtControl.h | 9 +++++-- libraries/AP_TECS/AP_TECS.cpp | 24 ++++++++++++----- libraries/AP_TECS/AP_TECS.h | 26 +++++++++++++++++-- 9 files changed, 75 insertions(+), 44 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 09c9e59bbf..44e53bbf6d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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); } } diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 13decff8db..f926eba41f 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -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 } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 40f6af7da1..0431f815ed 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -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 } /* diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 4feff10568..23a39e6ddc 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -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; } diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index adaf9bac1e..84c1f20b18 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -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); +} \ No newline at end of file diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 3f94ae2aa6..872764c8c7 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -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; diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h index 995a76fb3d..e9c3e3e74a 100644 --- a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h @@ -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 diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 3ac9b57d1a..241d294e93 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -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; } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 9f0890cd64..d44cd17638 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -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;