fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 10:01:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: keep _pitch_integ_state in radians

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-01-26 17:04:43 +01:00
parent e16f98c2b6
commit 5099a91f87
2 changed files with 22 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.hpp
+8 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -257,7 +257,7 @@ public:
*
*/
struct Input {
float altitude; ///< Current altitude of the UAS [m].
float altitude; ///< Current altitude amsl of the UAS [m].
float altitude_rate; ///< Current altitude rate of the UAS [m/s].
float tas; ///< Current true airspeed of the UAS [m/s].
float tas_rate; ///< Current true airspeed rate of the UAS [m/s²].
@@ -456,13 +456,14 @@ private:
/**
* @brief Calculate the pitch control update function.
* Update the states of the pitch control
* Update the states of the pitch control (pitch integrator).
*
* @param dt is the update time intervall in [s].
* @param input is the current input measurement of the UAS.
* @param seb_rate is the specific energy balance rate in [m²/s³].
* @param param is the control parameters.
*/
void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param);
void _calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate, const Param &param);
/**
* @brief Calculate the pitch control output function.
@@ -499,7 +500,7 @@ private:
/**
* @brief Calculate the throttle control update function.
* Update the throttle control states.
* Update the throttle control states (throttle integrator).
*
* @param dt is the update time intervall in [s].
* @param limit is the specific total energy rate limits in [m²/s³].
@@ -526,8 +527,7 @@ private:
// State
AlphaFilter<float> _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate.
float _pitch_integ_state{0.0f}; ///< Pitch integrator state [rad].
float _throttle_integ_state{0.0f}; ///< Throttle integrator state.
float _throttle_integ_state{0.0f}; ///< Throttle integrator state [-].
// Output
DebugOutput _debug_output; ///< Debug output.
@@ -602,7 +602,7 @@ public:
void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
// // setters for parameters
// setters for parameters
void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;};
void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;};
void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;};
@@ -654,7 +654,6 @@ public:
float get_pitch_setpoint() {return _control.getPitchSetpoint();}
float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
// // TECS status
uint64_t timestamp() { return _update_timestamp; }
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }