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

addressed review comments

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-11-05 18:04:33 +01:00
committed by Silvan Fuhrer
parent 0ac3077bdc
commit 924298e9c3
4 changed files with 14 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+9 -8
View File
@@ -181,10 +181,11 @@ void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target
target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate);
target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate);
const float altitude_error_m = alt_sp_amsl_m - _alt_control_traj_generator.getCurrentPosition();
const float delta_trajectory_to_target_m = alt_sp_amsl_m - _alt_control_traj_generator.getCurrentPosition();
float height_rate_target = math::signNoZero<float>(altitude_error_m) * math::trajectory::computeMaxSpeedFromDistance(
1000, _vert_accel_limit, fabsf(altitude_error_m), 0.0f);
float height_rate_target = math::signNoZero<float>(delta_trajectory_to_target_m) *
math::trajectory::computeMaxSpeedFromDistance(
_jerk_max, _vert_accel_limit, fabsf(delta_trajectory_to_target_m), 0.0f);
height_rate_target = math::constrain(height_rate_target, -target_sinkrate_m_s, target_climbrate_m_s);
@@ -440,11 +441,11 @@ void TECS::_update_pitch_setpoint()
void TECS::_updateTrajectoryGenerationConstraints()
{
_alt_control_traj_generator.setMaxJerk(1000.0f); // we want infinite jerk, so just set a high value
_alt_control_traj_generator.setMaxJerk(_jerk_max);
_alt_control_traj_generator.setMaxAccel(_vert_accel_limit);
_alt_control_traj_generator.setMaxVel(_max_climb_rate);
_alt_control_traj_generator.setMaxVel(max(_max_climb_rate, _max_sink_rate));
_velocity_control_traj_generator.setMaxJerk(1000.0f); // we want infinite jerk, so just set a high value
_velocity_control_traj_generator.setMaxJerk(_jerk_max);
_velocity_control_traj_generator.setMaxAccelUp(_vert_accel_limit);
_velocity_control_traj_generator.setMaxAccelDown(_vert_accel_limit);
_velocity_control_traj_generator.setMaxVelUp(_max_climb_rate);
@@ -457,6 +458,8 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
bool control_altitude = true;
const bool input_is_height_rate = PX4_ISFINITE(height_rate_sp);
_velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint);
if (input_is_height_rate) {
_velocity_control_traj_generator.setCurrentPositionEstimate(altitude_amsl);
_velocity_control_traj_generator.update(_dt, height_rate_sp);
@@ -466,13 +469,11 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
} else {
_velocity_control_traj_generator.reset(0, _hgt_rate_setpoint, _hgt_setpoint);
_velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint);
}
if (control_altitude) {
runAltitudeControllerSmoothVelocity(altitude_sp_amsl, target_climbrate, target_sinkrate, altitude_amsl);
_velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint);
} else {
_alt_control_traj_generator.setCurrentVelocity(_hgt_rate_setpoint);
src/lib/tecs/TECS.hpp
+5
View File
@@ -203,6 +203,11 @@ public:
private:
static constexpr float _jerk_max =
1000.0f; // maximum jerk for creating height rate trajectories, we want infinite jerk so set a high value
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
// timestamps