Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed-wing speed modes #18605

Closed
wants to merge 14 commits into from
Closed

Fixed-wing speed modes #18605

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
e76734f
TECS: directly publish tecs_status from within TECS instance
sfuhrer Jul 16, 2021
99e1e9d
TECS: add flight_phase field in tecs_status (Level, Climb, Descend)
sfuhrer Nov 8, 2021
faf06ea
FW Pos Control: add wind_mode to adapt airspeed setpoint based on wind
sfuhrer Nov 8, 2021
09e415c
FW Position Control: add logic for speed modes Eco and Dash
sfuhrer Nov 9, 2021
af394e6
FW Position Control: simplify underspeed disabling logic for tecs
sfuhrer Nov 9, 2021
9f1f834
FW Position Control: centralise airspeed setpoint adaptions for auto …
sfuhrer Nov 9, 2021
8c42b71
FW Position Control: add reset function for global variables used in …
sfuhrer Nov 9, 2021
024777e
FW Position Control: add airspeed offset based on wind in Eco mode
sfuhrer Nov 9, 2021
55809e8
FW Position Control: add Eco mode in TECS and separate Eco climbrate
sfuhrer Nov 9, 2021
810c69d
FW Position Control: Clean up speed mode params
sfuhrer Nov 11, 2021
4fab9c8
FW Position Control: remove global variable _current_speed_mode and i…
sfuhrer Nov 11, 2021
b0b8055
FW Position Control: bit of clean up after review
sfuhrer Nov 11, 2021
57e6dcc
FW Position Control: improve Eco parameter description
sfuhrer Nov 12, 2021
1e80478
TECS: fix publishing of tecs_status
sfuhrer Nov 25, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions msg/tecs_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ uint8 TECS_MODE_LAND = 3
uint8 TECS_MODE_LAND_THROTTLELIM = 4
uint8 TECS_MODE_BAD_DESCENT = 5
uint8 TECS_MODE_CLIMBOUT = 6
uint8 TECS_MODE_ECO = 7


float32 altitude_sp
Expand Down Expand Up @@ -42,3 +43,9 @@ float32 throttle_sp
float32 pitch_sp_rad

uint8 mode

uint8 flight_phase

uint8 TECS_FLIGHT_PHASE_LEVEL = 0 # In level flight (no altitude setpoint change)
uint8 TECS_FLIGHT_PHASE_CLIMB = 1 # Climb is triggered (altitude setpoint is increasing)
uint8 TECS_FLIGHT_PHASE_DESCEND = 2 # Descend is triggered (altitude setpoint is descreasing)
123 changes: 120 additions & 3 deletions src/lib/tecs/TECS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,12 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil
* @author Paul Riseborough
*/


TECS::TECS()
{
_tecs_status_pub.advertise();
}

/*
* This function implements a complementary filter to estimate the climb rate when
* inertial nav data is not available. It also calculates a true airspeed derivative
Expand Down Expand Up @@ -227,7 +233,7 @@ void TECS::_update_energy_estimates()
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
// Calculate the specific energy balance error
_SEB_error = SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
_SEB_error = get_SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);

// Calculate specific energy rate demands in units of (m**2/sec**3)
_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
Expand Down Expand Up @@ -452,6 +458,38 @@ void TECS::_updateTrajectoryGenerationConstraints()
_velocity_control_traj_generator.setMaxVelDown(_max_climb_rate); // different convention for FW than for MC
}

void TECS::_updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint)
{
const bool input_is_height_rate = PX4_ISFINITE(height_rate_setpoint);

// update flight phase state (are we flying level, climbing or descending)
if (input_is_height_rate) {

if (PX4_ISFINITE(_velocity_control_traj_generator.getCurrentPosition())) {
// we have a valid altitude setpoint which means that we are flying level
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;

} else if (_velocity_control_traj_generator.getCurrentVelocity() > FLT_EPSILON) {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;

} else {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
}

} else {
// stay in flight phase level if only small altitude changes are demanded (<10m)
if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() > 10.0f) {
Copy link
Contributor Author

@sfuhrer sfuhrer Nov 9, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Instead of hard-coding the limits (10m atm), we could link it to FW_ALT_ERR_U and FW_ALT_ERR_O.

_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;

} else if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() < -10.0f) {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;

} else {
_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
}
}
}

void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
float target_sinkrate, float altitude_amsl)
{
Expand Down Expand Up @@ -554,7 +592,7 @@ void TECS::_update_STE_rate_lim()
void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
float target_climbrate, float target_sinkrate, float hgt_rate_sp)
float hgt_rate_sp, bool eco_mode_enabled)
{
// Calculate the time since last update (seconds)
uint64_t now = hrt_absolute_time();
Expand Down Expand Up @@ -591,10 +629,16 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
// Detect an uncommanded descent caused by an unachievable airspeed demand
_detect_uncommanded_descent();

_eco_mode_enabled = eco_mode_enabled;

// Calculate the demanded true airspeed
_update_speed_setpoint();

_calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude);
_updateFlightPhase(hgt_setpoint, hgt_rate_sp);

const float target_climb_rate = _tecs_mode == ECL_TECS_MODE_ECO ? _target_climb_rate_eco : _target_climb_rate;

_calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climb_rate, _target_sink_rate, baro_altitude);

// Calculate the specific energy values required by the control loop
_update_energy_estimates();
Expand All @@ -618,15 +662,24 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
} else if (_climbout_mode_active) {
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;

} else if (_eco_mode_enabled) {
_tecs_mode = ECL_TECS_MODE_ECO;

} else {
// This is the default operation mode
_tecs_mode = ECL_TECS_MODE_NORMAL;
}

tecs_status_publish(now);
}

void TECS::_update_speed_height_weights()
{
if (_eco_mode_enabled) {
_pitch_speed_weight = _pitch_speed_weight_eco;
_height_error_gain = _height_error_gain_eco;
}

// Calculate the weight applied to control of specific kinetic energy error
_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);

Expand All @@ -642,3 +695,67 @@ void TECS::_update_speed_height_weights()
_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
}

void
TECS::tecs_status_publish(const hrt_abstime &now)
{
tecs_status_s tecs_status{};

switch (_tecs_mode) {
case TECS::ECL_TECS_MODE_NORMAL:
tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
break;

case TECS::ECL_TECS_MODE_UNDERSPEED:
tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
break;

case TECS::ECL_TECS_MODE_BAD_DESCENT:
tecs_status.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
break;

case TECS::ECL_TECS_MODE_CLIMBOUT:
tecs_status.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
break;

case TECS::ECL_TECS_MODE_ECO:
tecs_status.mode = tecs_status_s::TECS_MODE_ECO;
break;

default:
tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
}

tecs_status.altitude_sp = _hgt_setpoint;
tecs_status.altitude_filtered = _vert_pos_state;
tecs_status.energy_distribution_error = _SEB_error;
tecs_status.energy_distribution_rate_error = _SEB_rate_error;
tecs_status.equivalent_airspeed_sp = _EAS_setpoint;
tecs_status.flight_phase = _flight_phase;
tecs_status.height_rate_setpoint = _hgt_rate_setpoint;
tecs_status.height_rate = _vert_vel_state;
tecs_status.pitch_integ = _pitch_integ_state;
tecs_status.pitch_sp_rad = get_pitch_setpoint();
tecs_status.throttle_integ = _throttle_integ_state;
tecs_status.throttle_sp = get_throttle_setpoint();
tecs_status.total_energy_balance = get_SEB();
tecs_status.total_energy_balance_sp = get_SEB_setpoint();
tecs_status.total_energy_balance_rate = get_SEB_rate();
tecs_status.total_energy_balance_rate_sp = get_SEB_rate_setpoint();
tecs_status.total_energy = get_STE();
tecs_status.total_energy_error = _STE_error;
tecs_status.total_energy_sp = get_STE_setpoint();
tecs_status.total_energy_rate = get_STE_rate();
tecs_status.total_energy_rate_error = _STE_rate_error;
tecs_status.total_energy_rate_sp = get_STE_rate_setpoint();
tecs_status.true_airspeed_sp = _TAS_setpoint_adj;
tecs_status.true_airspeed_filtered = _tas_state;
tecs_status.true_airspeed_derivative = _tas_rate_filtered;
tecs_status.true_airspeed_derivative_raw = _tas_rate_raw;
tecs_status.true_airspeed_derivative_sp = _TAS_rate_setpoint;
tecs_status.true_airspeed_innovation = _tas_innov;

tecs_status.timestamp = now;

_tecs_status_pub.publish(tecs_status);
}
93 changes: 45 additions & 48 deletions src/lib/tecs/TECS.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,14 @@
#include <motion_planning/VelocitySmoothing.hpp>
#include <motion_planning/ManualVelocitySmoothingZ.hpp>

#include <uORB/Publication.hpp>
#include <uORB/topics/tecs_status.h>
#include <uORB/uORB.h>

class TECS
{
public:
TECS() = default;
TECS();
~TECS() = default;

// no copy, assignment, move, move assignment
Expand Down Expand Up @@ -90,19 +94,17 @@ class TECS
void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);

float get_throttle_setpoint() { return _last_throttle_setpoint; }
float get_pitch_setpoint() { return _last_pitch_setpoint; }
float get_speed_weight() { return _pitch_speed_weight; }
float pitch_limit_min, float pitch_limit_max, float hgt_rate_sp = NAN,
bool eco_mode_enabled = false);

void reset_state() { _states_initialized = false; }

enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT,
ECL_TECS_MODE_CLIMBOUT
ECL_TECS_MODE_CLIMBOUT,
ECL_TECS_MODE_ECO
};

void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
Expand Down Expand Up @@ -140,51 +142,31 @@ class TECS

void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }

void set_target_climb_rate(float target_climb_rate) { _target_climb_rate = target_climb_rate; }
void set_target_sink_rate(float target_sink_rate) { _target_sink_rate = target_sink_rate; }

// TECS status
uint64_t timestamp() { return _pitch_update_timestamp; }
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }

float hgt_setpoint() { return _hgt_setpoint; }
float vert_pos_state() { return _vert_pos_state; }

float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
float tas_state() { return _tas_state; }
float getTASInnovation() { return _tas_innov; }

float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
float vert_vel_state() { return _vert_vel_state; }

float get_EAS_setpoint() { return _EAS_setpoint; };
float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
float speed_derivative() { return _tas_rate_filtered; }
float speed_derivative_raw() { return _tas_rate_raw; }

float STE_error() { return _STE_error; }
float STE_rate_error() { return _STE_rate_error; }

float SEB_error() { return _SEB_error; }
float SEB_rate_error() { return _SEB_rate_error; }

float throttle_integ_state() { return _throttle_integ_state; }
float pitch_integ_state() { return _pitch_integ_state; }
// eco mode settings
void set_speed_weight_eco(float weight_eco) { _pitch_speed_weight_eco = weight_eco; }
void set_height_error_time_constant_eco(float time_const_eco) { _height_error_gain_eco = 1.0f / math::max(time_const_eco, 0.1f); }
void set_target_climb_rate_eco(float target_climb_rate_eco) { _target_climb_rate_eco = target_climb_rate_eco; }

float STE() { return _SPE_estimate + _SKE_estimate; }

float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }

float STE_rate() { return _SPE_rate + _SKE_rate; }

float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }

float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }

float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }

float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
// getters
float get_throttle_setpoint() { return _last_throttle_setpoint; }
float get_pitch_setpoint() { return _last_pitch_setpoint; }

float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
float get_hgt_setpoint() { return _hgt_setpoint; }
float get_hgt_rate_setpoint() { return _hgt_rate_setpoint; }
float get_STE() { return _SPE_estimate + _SKE_estimate; }
float get_STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
float get_STE_rate() { return _SPE_rate + _SKE_rate; }
float get_STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
float get_SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
float get_SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
float get_SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
float get_SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
int get_flight_phase() { return _flight_phase; }

void tecs_status_publish(const hrt_abstime &now);

/**
* Handle the altitude reset
Expand Down Expand Up @@ -237,6 +219,10 @@ class TECS
float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s)
float _SEB_rate_ff{1.0f};

float _height_error_gain_eco{0.2f}; ///< in eco mode: height error inverse time constant [1/s]
float _pitch_speed_weight_eco{1.0f}; ///< in eco mode: speed control weighting used by pitch demand calculation
float _target_climb_rate_eco{2.f};

// complimentary filter states
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec)
float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m)
Expand Down Expand Up @@ -295,6 +281,9 @@ class TECS
float _SPE_weighting{1.0f};
float _SKE_weighting{1.0f};

float _target_climb_rate{3.f};
float _target_sink_rate{2.f};

// time steps (non-fixed)
float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec)
static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec)
Expand All @@ -307,6 +296,10 @@ class TECS
bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled
bool _states_initialized{false}; ///< true when TECS states have been iniitalized
bool _in_air{false}; ///< true when the vehicle is flying
bool _eco_mode_enabled{false};

// flight phase
int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL};

/**
* Update the airspeed internal state using a second order complementary filter
Expand Down Expand Up @@ -351,6 +344,8 @@ class TECS

void _updateTrajectoryGenerationConstraints();

void _updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint);

void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
float target_sinkrate, float altitude_amsl);

Expand All @@ -376,4 +371,6 @@ class TECS
ManualVelocitySmoothingZ
_velocity_control_traj_generator; // generates height rate and altitude setpoint trajectory when height rate is commanded

uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};

};
Loading