fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 13:58:53 +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
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
-10
View File
@@ -629,16 +629,6 @@ FixedwingPositionControl::in_takeoff_situation()
&& (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get());
}
void
FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min)
{
/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
if (in_takeoff_situation()) {
*hold_altitude = _takeoff_ground_alt + _param_fw_clmbout_diff.get();
*pitch_limit_min = radians(10.0f);
}
}
void
FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
{
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
-8
View File
@@ -112,7 +112,6 @@ static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort l
static constexpr float THROTTLE_THRESH =
0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
@@ -312,13 +311,6 @@ private:
*/
bool in_takeoff_situation();
/**
* Do takeoff help when in altitude controlled modes
* @param hold_altitude altitude setpoint for controller
* @param pitch_limit_min minimum pitch allowed
*/
void do_takeoff_help(float *hold_altitude, float *pitch_limit_min);
/**
* Update desired altitude base on user pitch stick input
*