diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h index 269b21120b..aa9840cee9 100644 --- a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h @@ -13,6 +13,7 @@ #include #include #include +#include class AP_SpdHgtControl { public: @@ -21,25 +22,12 @@ public: // Should be called at 50Hz or faster virtual void update_50hz(void) = 0; - /** - stages of flight so the altitude controller can choose to - prioritise height or speed - */ - enum FlightStage { - FLIGHT_TAKEOFF = 1, - FLIGHT_VTOL = 2, - FLIGHT_NORMAL = 3, - FLIGHT_LAND_APPROACH = 4, - FLIGHT_LAND_PREFLARE = 5, - FLIGHT_LAND_FINAL = 6, - FLIGHT_LAND_ABORT = 7 - }; // Update of the pitch and throttle demands // Should be called at 10Hz or faster virtual void update_pitch_throttle( int32_t hgt_dem_cm, int32_t EAS_dem_cm, - enum FlightStage flight_stage, + enum AP_Vehicle::FixedWing::FlightStage flight_stage, bool is_doing_auto_land, float distance_beyond_land_wp, int32_t ptchMinCO_cd, diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 0ddc815c9c..b9d8585674 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -470,7 +470,7 @@ void AP_TECS::_update_height_demand(void) // in final landing stage force height rate demand to the // configured sink rate and adjust the demanded height to // be kinematically consistent with the height rate. - if (_flight_stage == FLIGHT_LAND_FINAL) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) { _integSEB_state = 0; if (_flare_counter == 0) { _hgt_rate_dem = _climb_rate; @@ -525,11 +525,11 @@ void AP_TECS::_detect_underspeed(void) _flags.underspeed = false; } - if (_flight_stage == AP_TECS::FLIGHT_VTOL) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) { _flags.underspeed = false; } else if (((_TAS_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f) && - _flight_stage != AP_TECS::FLIGHT_LAND_FINAL) || + _flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) || ((_height < _hgt_dem_adj) && _flags.underspeed)) { _flags.underspeed = true; @@ -658,7 +658,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_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { if (!_flags.reached_speed_takeoff) { // ensure we run at full throttle until we reach the target airspeed @@ -682,7 +682,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) float AP_TECS::_get_i_gain(void) { float i_gain = _integGain; - if (_flight_stage == FLIGHT_TAKEOFF) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) { if (!is_zero(_integGain_takeoff)) { i_gain = _integGain_takeoff; } @@ -768,7 +768,7 @@ void AP_TECS::_update_pitch(void) float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); if (!_ahrs.airspeed_sensor_enabled()) { SKE_weighting = 0.0f; - } else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { + } else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { SKE_weighting = 2.0f; } else if (_flags.is_doing_auto_land) { if (_spdWeightLand < 0) { @@ -806,7 +806,7 @@ void AP_TECS::_update_pitch(void) float integSEB_delta = integSEB_input * _DT; #if 0 - if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) { ::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n", _hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem), SEB_dem, SEBdot_dem, SEB_error, SEBdot_error); @@ -825,14 +825,14 @@ void AP_TECS::_update_pitch(void) float temp = SEB_error + SEBdot_dem * timeConstant(); float pitch_damp = _ptchDamp; - if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) { pitch_damp = _landDamp; } else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) { pitch_damp = _land_pitch_damp; } temp += SEBdot_error * pitch_damp; - if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { temp += _PITCHminf * gainInv; } float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp; @@ -895,7 +895,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _DT = 0.1f; // when first starting TECS, use a // small time constant } - else if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) + else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { _PITCHminf = 0.000174533f * ptchMinCO_cd; _hgt_dem_adj_last = hgt_afe; @@ -907,7 +907,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _flags.badDescent = false; } - if (_flight_stage != AP_TECS::FLIGHT_TAKEOFF && _flight_stage != AP_TECS::FLIGHT_LAND_ABORT) { + if (_flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && _flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { // reset takeoff speed flag when not in takeoff _flags.reached_speed_takeoff = false; } @@ -924,7 +924,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 FlightStage flight_stage, + enum AP_Vehicle::FixedWing::FlightStage flight_stage, bool is_doing_auto_land, float distance_beyond_land_wp, int32_t ptchMinCO_cd, @@ -949,7 +949,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_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) { + (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT)) { _THRmaxf = aparm.takeoff_throttle_max * 0.01f; } else { _THRmaxf = aparm.throttle_max * 0.01f; @@ -979,7 +979,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _pitch_max_limit = 90; } - if (flight_stage == FLIGHT_LAND_FINAL) { + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) { // in flare use min pitch from LAND_PITCH_CD _PITCHminf = MAX(_PITCHminf, _landing.get_pitch_cd() * 0.01f); @@ -990,7 +990,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // and allow zero throttle _THRminf = 0; - } else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) { + } else if ((flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH || flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) { // constrain the pitch in landing as we get close to the flare // point. Use a simple linear limit from 15 meters after the // landing point @@ -1011,7 +1011,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } } - if (flight_stage == FLIGHT_TAKEOFF || flight_stage == FLIGHT_LAND_ABORT) { + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { 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 58a4fba091..90d187abc4 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -44,7 +44,7 @@ public: // Update the control loop calculations void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, - enum FlightStage flight_stage, + enum AP_Vehicle::FixedWing::FlightStage flight_stage, bool is_doing_auto_land, float distance_beyond_land_wp, int32_t ptchMinCO_cd, @@ -262,7 +262,7 @@ private: uint32_t _underspeed_start_ms; // auto mode flightstage - enum FlightStage _flight_stage; + enum AP_Vehicle::FixedWing::FlightStage _flight_stage; // pitch demand before limiting float _pitch_dem_unc; diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index be065d09fb..caa97ebe0b 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -57,6 +57,18 @@ public: float height_estimate; float last_distance; }; + + + // stages of flight + enum FlightStage { + FLIGHT_TAKEOFF = 1, + FLIGHT_VTOL = 2, + FLIGHT_NORMAL = 3, + FLIGHT_LAND_APPROACH = 4, + FLIGHT_LAND_PREFLARE = 5, + FLIGHT_LAND_FINAL = 6, + FLIGHT_LAND_ABORT = 7 + }; }; /*