mirror of https://github.com/ArduPilot/ardupilot
AP_TECS: change namespace of MultiCopter and FixedWing params
this stops the libraries knowing anything about AP_Vehicle
This commit is contained in:
parent
573604582e
commit
d438cd9ed2
|
@ -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
|
// 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
|
// 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) &&
|
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;
|
const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == ThrClipStatus::MIN;
|
||||||
if (max_climb_condition && _hgt_dem > _hgt_dem_prev) {
|
if (max_climb_condition && _hgt_dem > _hgt_dem_prev) {
|
||||||
_hgt_dem = _hgt_dem_prev;
|
_hgt_dem = _hgt_dem_prev;
|
||||||
|
@ -552,7 +552,7 @@ void AP_TECS::_detect_underspeed(void)
|
||||||
_flags.underspeed = false;
|
_flags.underspeed = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) {
|
if (_flight_stage == AP_FixedWing::FlightStage::VTOL) {
|
||||||
_flags.underspeed = false;
|
_flags.underspeed = false;
|
||||||
} else if (((_TAS_state < _TASmin * 0.9f) &&
|
} else if (((_TAS_state < _TASmin * 0.9f) &&
|
||||||
(_throttle_dem >= _THRmaxf * 0.95f) &&
|
(_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_max = 0.5f * _TASmax * _TASmax - _SKE_dem;
|
||||||
float SPE_err_min = 0.5f * _TASmin * _TASmin - _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
|
when we are in a VTOL state then we ignore potential energy
|
||||||
errors as we have vertical motors that interfere with the
|
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
|
// Calculate integrator state, constraining state
|
||||||
// Set integrator to a max throttle value during climbout
|
// Set integrator to a max throttle value during climbout
|
||||||
_integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
|
_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) {
|
if (!_flags.reached_speed_takeoff) {
|
||||||
// ensure we run at full throttle until we reach the target airspeed
|
// ensure we run at full throttle until we reach the target airspeed
|
||||||
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
|
_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 AP_TECS::_get_i_gain(void)
|
||||||
{
|
{
|
||||||
float i_gain = _integGain;
|
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)) {
|
if (!is_zero(_integGain_takeoff)) {
|
||||||
i_gain = _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);
|
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
|
||||||
if (!(_ahrs.airspeed_sensor_enabled()|| _use_synthetic_airspeed)) {
|
if (!(_ahrs.airspeed_sensor_enabled()|| _use_synthetic_airspeed)) {
|
||||||
SKE_weighting = 0.0f;
|
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
|
// if we are in VTOL mode then control pitch without regard to
|
||||||
// speed. Speed is also taken care of independently of
|
// speed. Speed is also taken care of independently of
|
||||||
// height. This is needed as the usual relationship of speed
|
// height. This is needed as the usual relationship of speed
|
||||||
// and height is broken by the VTOL motors
|
// and height is broken by the VTOL motors
|
||||||
SKE_weighting = 0.0f;
|
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;
|
SKE_weighting = 2.0f;
|
||||||
} else if (_flags.is_doing_auto_land) {
|
} else if (_flags.is_doing_auto_land) {
|
||||||
if (_spdWeightLand < 0) {
|
if (_spdWeightLand < 0) {
|
||||||
|
@ -875,7 +875,7 @@ void AP_TECS::_update_pitch(void)
|
||||||
}
|
}
|
||||||
temp += SEBdot_error * pitch_damp;
|
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;
|
temp += _PITCHminf * gainInv;
|
||||||
}
|
}
|
||||||
float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp;
|
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
|
_DT = 0.1f; // when first starting TECS, use a
|
||||||
// small time constant
|
// small time constant
|
||||||
_need_reset = false;
|
_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;
|
_PITCHminf = 0.000174533f * ptchMinCO_cd;
|
||||||
_hgt_dem_adj_last = hgt_afe;
|
_hgt_dem_adj_last = hgt_afe;
|
||||||
_hgt_dem_adj = _hgt_dem_adj_last;
|
_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;
|
_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
|
// reset takeoff speed flag when not in takeoff
|
||||||
_flags.reached_speed_takeoff = false;
|
_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,
|
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
|
||||||
int32_t EAS_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,
|
float distance_beyond_land_wp,
|
||||||
int32_t ptchMinCO_cd,
|
int32_t ptchMinCO_cd,
|
||||||
int16_t throttle_nudge,
|
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;
|
_update_pitch_throttle_last_usec = now;
|
||||||
|
|
||||||
_flags.is_gliding = _flags.gliding_requested || _flags.propulsion_failed || aparm.throttle_max==0;
|
_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;
|
_distance_beyond_land_wp = distance_beyond_land_wp;
|
||||||
_flight_stage = flight_stage;
|
_flight_stage = flight_stage;
|
||||||
|
|
||||||
|
@ -1000,7 +1001,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
|
||||||
_update_speed(load_factor);
|
_update_speed(load_factor);
|
||||||
|
|
||||||
if (aparm.takeoff_throttle_max != 0 &&
|
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;
|
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
|
||||||
} else {
|
} else {
|
||||||
_THRmaxf = aparm.throttle_max * 0.01f;
|
_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) {
|
if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
|
||||||
// we have reached our target speed in takeoff, allow for
|
// we have reached our target speed in takeoff, allow for
|
||||||
// normal throttle control
|
// normal throttle control
|
||||||
|
|
|
@ -20,13 +20,14 @@
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
#include <AP_Param/AP_Param.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>
|
#include <Filter/AverageFilter.h>
|
||||||
|
|
||||||
class AP_Landing;
|
class AP_Landing;
|
||||||
class AP_TECS {
|
class AP_TECS {
|
||||||
public:
|
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)
|
: _ahrs(ahrs)
|
||||||
, aparm(parms)
|
, aparm(parms)
|
||||||
, _landing(landing)
|
, _landing(landing)
|
||||||
|
@ -46,7 +47,7 @@ public:
|
||||||
// Update the control loop calculations
|
// Update the control loop calculations
|
||||||
void update_pitch_throttle(int32_t hgt_dem_cm,
|
void update_pitch_throttle(int32_t hgt_dem_cm,
|
||||||
int32_t EAS_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,
|
float distance_beyond_land_wp,
|
||||||
int32_t ptchMinCO_cd,
|
int32_t ptchMinCO_cd,
|
||||||
int16_t throttle_nudge,
|
int16_t throttle_nudge,
|
||||||
|
@ -157,7 +158,7 @@ private:
|
||||||
// reference to the AHRS object
|
// reference to the AHRS object
|
||||||
AP_AHRS &_ahrs;
|
AP_AHRS &_ahrs;
|
||||||
|
|
||||||
const AP_Vehicle::FixedWing &aparm;
|
const AP_FixedWing &aparm;
|
||||||
|
|
||||||
// reference to const AP_Landing to access it's params
|
// reference to const AP_Landing to access it's params
|
||||||
const AP_Landing &_landing;
|
const AP_Landing &_landing;
|
||||||
|
@ -316,7 +317,7 @@ private:
|
||||||
uint32_t _underspeed_start_ms;
|
uint32_t _underspeed_start_ms;
|
||||||
|
|
||||||
// auto mode flightstage
|
// auto mode flightstage
|
||||||
enum AP_Vehicle::FixedWing::FlightStage _flight_stage;
|
enum AP_FixedWing::FlightStage _flight_stage;
|
||||||
|
|
||||||
// pitch demand before limiting
|
// pitch demand before limiting
|
||||||
float _pitch_dem_unc;
|
float _pitch_dem_unc;
|
||||||
|
|
Loading…
Reference in New Issue