Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: change namespace of MultiCopter and FixedWing params 

this stops the libraries knowing anything about AP_Vehicle
This commit is contained in:
Peter Barker 2022-09-30 09:10:41 +10:00 committed by Andrew Tridgell
parent 573604582e
commit d438cd9ed2
2 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_TECS/AP_TECS.cpp
View File

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

11
libraries/AP_TECS/AP_TECS.h
View File

@ -20,13 +20,14 @@
#include <AP_Math/AP_Math.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Vehicle/AP_FixedWing.h>
#include <Filter/DerivativeFilter.h>
#include <Filter/AverageFilter.h>
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;