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

AP_Vehicle,AP_SpdHgtControl,AP_TECS: move enum FlightStages to AP_Vehicle::FixedWing

Browse Source
This commit is contained in:
Tom Pittenger 2016-12-13 18:19:56 -08:00
parent 90b2458a27
commit 8019cedf5b
4 changed files with 32 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_SpdHgtControl/AP_SpdHgtControl.h
View File

@ -13,6 +13,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <DataFlash/DataFlash.h>
#include <AP_Vehicle/AP_Vehicle.h>
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,

32
libraries/AP_TECS/AP_TECS.cpp
View File

@ -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

4
libraries/AP_TECS/AP_TECS.h
View File

@ -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;

12
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -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
};
};
/*