forked from Archive/PX4-Autopilot
TECS: rename TECSReferenceModel to TECSAltitudeReferenceModel
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
0fa0e2f583
commit
beeae1f673
|
@ -133,8 +133,8 @@ TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const
|
||||||
return _airspeed_state;
|
return _airspeed_state;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
|
void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
|
||||||
const Param ¶m)
|
const Param ¶m)
|
||||||
{
|
{
|
||||||
// Input checks
|
// Input checks
|
||||||
if (!TIMESTAMP_VALID(dt)) {
|
if (!TIMESTAMP_VALID(dt)) {
|
||||||
|
@ -147,7 +147,6 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
|
||||||
altitude = 0.0f;
|
altitude = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
|
|
||||||
if (PX4_ISFINITE(setpoint.alt_rate)) {
|
if (PX4_ISFINITE(setpoint.alt_rate)) {
|
||||||
_alt_rate_ref = setpoint.alt_rate;
|
_alt_rate_ref = setpoint.alt_rate;
|
||||||
|
|
||||||
|
@ -155,7 +154,6 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
|
||||||
_alt_rate_ref = 0.0f;
|
_alt_rate_ref = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Altitude setpoint reference
|
// Altitude setpoint reference
|
||||||
const bool altitude_control_enable{PX4_ISFINITE(setpoint.alt)};
|
const bool altitude_control_enable{PX4_ISFINITE(setpoint.alt)};
|
||||||
_alt_control_traj_generator.setMaxJerk(param.jerk_max);
|
_alt_control_traj_generator.setMaxJerk(param.jerk_max);
|
||||||
|
@ -182,9 +180,9 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeReference() const
|
TECSAltitudeReferenceModel::AltitudeReferenceState TECSAltitudeReferenceModel::getAltitudeReference() const
|
||||||
{
|
{
|
||||||
TECSReferenceModel::AltitudeReferenceState ref{
|
TECSAltitudeReferenceModel::AltitudeReferenceState ref{
|
||||||
.alt = _alt_control_traj_generator.getCurrentPosition(),
|
.alt = _alt_control_traj_generator.getCurrentPosition(),
|
||||||
.alt_rate = _alt_control_traj_generator.getCurrentVelocity(),
|
.alt_rate = _alt_control_traj_generator.getCurrentVelocity(),
|
||||||
};
|
};
|
||||||
|
@ -192,12 +190,12 @@ TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeRefere
|
||||||
return ref;
|
return ref;
|
||||||
}
|
}
|
||||||
|
|
||||||
float TECSReferenceModel::getAltitudeRateReference() const
|
float TECSAltitudeReferenceModel::getAltitudeRateReference() const
|
||||||
{
|
{
|
||||||
return _alt_rate_ref;
|
return _alt_rate_ref;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TECSReferenceModel::initialize(const AltitudeReferenceState &state)
|
void TECSAltitudeReferenceModel::initialize(const AltitudeReferenceState &state)
|
||||||
{
|
{
|
||||||
float init_state_alt{state.alt};
|
float init_state_alt{state.alt};
|
||||||
_alt_rate_ref = state.alt_rate;
|
_alt_rate_ref = state.alt_rate;
|
||||||
|
@ -639,7 +637,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
|
||||||
const float eas_to_tas)
|
const float eas_to_tas)
|
||||||
{
|
{
|
||||||
// Init subclasses
|
// Init subclasses
|
||||||
TECSReferenceModel::AltitudeReferenceState current_state{ .alt = altitude,
|
TECSAltitudeReferenceModel::AltitudeReferenceState current_state{ .alt = altitude,
|
||||||
.alt_rate = altitude_rate};
|
.alt_rate = altitude_rate};
|
||||||
_reference_model.initialize(current_state);
|
_reference_model.initialize(current_state);
|
||||||
_airspeed_filter.initialize(equivalent_airspeed);
|
_airspeed_filter.initialize(equivalent_airspeed);
|
||||||
|
@ -661,7 +659,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
|
||||||
const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
||||||
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
||||||
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
||||||
const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
|
const TECSAltitudeReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
|
||||||
_debug_status.altitude_sp_ref = ref_alt.alt;
|
_debug_status.altitude_sp_ref = ref_alt.alt;
|
||||||
_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
|
_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
|
||||||
_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
|
_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
|
||||||
|
@ -710,7 +708,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
||||||
const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
||||||
|
|
||||||
// Update Reference model submodule
|
// Update Reference model submodule
|
||||||
const TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
|
const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
|
||||||
.alt_rate = hgt_rate_sp};
|
.alt_rate = hgt_rate_sp};
|
||||||
|
|
||||||
_reference_model.update(dt, setpoint, altitude, _reference_param);
|
_reference_model.update(dt, setpoint, altitude, _reference_param);
|
||||||
|
|
|
@ -113,7 +113,7 @@ private:
|
||||||
AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state
|
AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state
|
||||||
};
|
};
|
||||||
|
|
||||||
class TECSReferenceModel
|
class TECSAltitudeReferenceModel
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
|
@ -139,8 +139,8 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
public:
|
public:
|
||||||
TECSReferenceModel() = default;
|
TECSAltitudeReferenceModel() = default;
|
||||||
~TECSReferenceModel() = default;
|
~TECSAltitudeReferenceModel() = default;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Initialize reference models.
|
* @brief Initialize reference models.
|
||||||
|
@ -249,7 +249,7 @@ public:
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
struct Setpoint {
|
struct Setpoint {
|
||||||
TECSReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model.
|
TECSAltitudeReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model.
|
||||||
float altitude_rate_setpoint; ///< Altitude rate setpoint.
|
float altitude_rate_setpoint; ///< Altitude rate setpoint.
|
||||||
float tas_setpoint; ///< True airspeed setpoint.
|
float tas_setpoint; ///< True airspeed setpoint.
|
||||||
};
|
};
|
||||||
|
@ -638,7 +638,7 @@ public:
|
||||||
*/
|
*/
|
||||||
void handle_alt_step(float altitude, float altitude_rate)
|
void handle_alt_step(float altitude, float altitude_rate)
|
||||||
{
|
{
|
||||||
TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
|
TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
|
||||||
.alt_rate = altitude_rate};
|
.alt_rate = altitude_rate};
|
||||||
|
|
||||||
// reset altitude reference model.
|
// reset altitude reference model.
|
||||||
|
@ -654,7 +654,7 @@ public:
|
||||||
private:
|
private:
|
||||||
TECSControl _control; ///< Control submodule.
|
TECSControl _control; ///< Control submodule.
|
||||||
TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule.
|
TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule.
|
||||||
TECSReferenceModel _reference_model; ///< Setpoint reference model submodule.
|
TECSAltitudeReferenceModel _reference_model; ///< Setpoint reference model submodule.
|
||||||
|
|
||||||
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode.
|
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode.
|
||||||
|
|
||||||
|
@ -677,7 +677,7 @@ private:
|
||||||
.airspeed_rate_noise_std_dev = 0.02f
|
.airspeed_rate_noise_std_dev = 0.02f
|
||||||
};
|
};
|
||||||
/// Reference model parameters.
|
/// Reference model parameters.
|
||||||
TECSReferenceModel::Param _reference_param{
|
TECSAltitudeReferenceModel::Param _reference_param{
|
||||||
.target_climbrate = 2.0f,
|
.target_climbrate = 2.0f,
|
||||||
.target_sinkrate = 2.0f,
|
.target_sinkrate = 2.0f,
|
||||||
.jerk_max = 1000.0f,
|
.jerk_max = 1000.0f,
|
||||||
|
|
Loading…
Reference in New Issue