From d438cd9ed20e6b2e516c26414203f47127338e8b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 30 Sep 2022 09:10:41 +1000 Subject: [PATCH] AP_TECS: change namespace of MultiCopter and FixedWing params this stops the libraries knowing anything about AP_Vehicle --- libraries/AP_TECS/AP_TECS.cpp | 29 +++++++++++++++-------------- libraries/AP_TECS/AP_TECS.h | 11 ++++++----- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index ade5870c35..6fd8ad3c95 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -512,7 +512,7 @@ void AP_TECS::_update_height_demand(void) // Don't allow height demand to get too far ahead of the vehicles current height // if vehicle is unable to follow the demanded climb or descent const bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf || _thr_clip_status == ThrClipStatus::MAX) && - !(_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); + !(_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == ThrClipStatus::MIN; if (max_climb_condition && _hgt_dem > _hgt_dem_prev) { _hgt_dem = _hgt_dem_prev; @@ -552,7 +552,7 @@ void AP_TECS::_detect_underspeed(void) _flags.underspeed = false; } - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) { + if (_flight_stage == AP_FixedWing::FlightStage::VTOL) { _flags.underspeed = false; } else if (((_TAS_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f) && @@ -617,7 +617,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem; float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem; - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) { + if (_flight_stage == AP_FixedWing::FlightStage::VTOL) { /* when we are in a VTOL state then we ignore potential energy errors as we have vertical motors that interfere with the @@ -677,7 +677,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) // Calculate integrator state, constraining state // Set integrator to a max throttle value during climbout _integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr; - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { if (!_flags.reached_speed_takeoff) { // ensure we run at full throttle until we reach the target airspeed _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state); @@ -725,7 +725,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) float AP_TECS::_get_i_gain(void) { float i_gain = _integGain; - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { if (!is_zero(_integGain_takeoff)) { i_gain = _integGain_takeoff; } @@ -808,13 +808,13 @@ void AP_TECS::_update_pitch(void) float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); if (!(_ahrs.airspeed_sensor_enabled()|| _use_synthetic_airspeed)) { SKE_weighting = 0.0f; - } else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) { + } else if (_flight_stage == AP_FixedWing::FlightStage::VTOL) { // if we are in VTOL mode then control pitch without regard to // speed. Speed is also taken care of independently of // 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 || _flags.is_gliding) { + } else if ( _flags.underspeed || _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING || _flags.is_gliding) { SKE_weighting = 2.0f; } else if (_flags.is_doing_auto_land) { if (_spdWeightLand < 0) { @@ -875,7 +875,7 @@ void AP_TECS::_update_pitch(void) } temp += SEBdot_error * pitch_damp; - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { temp += _PITCHminf * gainInv; } float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp; @@ -948,7 +948,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _DT = 0.1f; // when first starting TECS, use a // small time constant _need_reset = false; - } else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + } + else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { _PITCHminf = 0.000174533f * ptchMinCO_cd; _hgt_dem_adj_last = hgt_afe; _hgt_dem_adj = _hgt_dem_adj_last; @@ -958,7 +959,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _flags.badDescent = false; } - if (_flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && _flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { // reset takeoff speed flag when not in takeoff _flags.reached_speed_takeoff = false; } @@ -975,7 +976,7 @@ void AP_TECS::_update_STE_rate_lim(void) void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, - enum AP_Vehicle::FixedWing::FlightStage flight_stage, + enum AP_FixedWing::FlightStage flight_stage, float distance_beyond_land_wp, int32_t ptchMinCO_cd, int16_t throttle_nudge, @@ -988,7 +989,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _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); + _flags.is_doing_auto_land = (flight_stage == AP_FixedWing::FlightStage::LAND); _distance_beyond_land_wp = distance_beyond_land_wp; _flight_stage = flight_stage; @@ -1000,7 +1001,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _update_speed(load_factor); if (aparm.takeoff_throttle_max != 0 && - (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { + (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { _THRmaxf = aparm.takeoff_throttle_max * 0.01f; } else { _THRmaxf = aparm.throttle_max * 0.01f; @@ -1093,7 +1094,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } } - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) { // we have reached our target speed in takeoff, allow for // normal throttle control diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index ab12617c84..b2f284a51f 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -20,13 +20,14 @@ #include #include #include -#include +#include +#include #include class AP_Landing; class AP_TECS { public: - AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, const AP_Landing &landing, const uint32_t log_bitmask) + AP_TECS(AP_AHRS &ahrs, const AP_FixedWing &parms, const AP_Landing &landing, const uint32_t log_bitmask) : _ahrs(ahrs) , aparm(parms) , _landing(landing) @@ -46,7 +47,7 @@ public: // Update the control loop calculations void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, - enum AP_Vehicle::FixedWing::FlightStage flight_stage, + enum AP_FixedWing::FlightStage flight_stage, float distance_beyond_land_wp, int32_t ptchMinCO_cd, int16_t throttle_nudge, @@ -157,7 +158,7 @@ private: // reference to the AHRS object AP_AHRS &_ahrs; - const AP_Vehicle::FixedWing &aparm; + const AP_FixedWing &aparm; // reference to const AP_Landing to access it's params const AP_Landing &_landing; @@ -316,7 +317,7 @@ private: uint32_t _underspeed_start_ms; // auto mode flightstage - enum AP_Vehicle::FixedWing::FlightStage _flight_stage; + enum AP_FixedWing::FlightStage _flight_stage; // pitch demand before limiting float _pitch_dem_unc;