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

Unify optical flow height limiting

Browse Source
This commit is contained in:
Mohammed Kabir
2018-06-09 13:38:04 +02:00
committed by Lorenz Meier
parent 0113212b34
commit b3c5e53333
4 changed files with 93 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -15
View File
@@ -230,7 +230,8 @@ private:
(ParamFloat<px4::params::RC_FLT_CUTOFF>) _rc_flt_cutoff,
(ParamFloat<px4::params::RC_FLT_SMP_RATE>) _rc_flt_smp_rate,
(ParamFloat<px4::params::MPC_ACC_HOR_FLOW>) _acc_max_flow_xy,
(ParamFloat<px4::params::MPC_MAX_FLOW_HGT>) _flow_max_hgt
(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _flow_min_hgt,
(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _flow_max_hgt
);
@@ -302,8 +303,6 @@ private:
float _takeoff_vel_limit; /**< velocity limit value which gets ramped up */
float _min_hagl_limit; /**< minimum continuous height above ground (m) */
// counters for reset events on position and velocity states
// they are used to identify a reset event
uint8_t _z_reset_counter;
@@ -481,7 +480,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_manual_jerk_limit_z(1.0f),
_z_derivative(0.0f),
_takeoff_vel_limit(0.0f),
_min_hagl_limit(0.0f),
_z_reset_counter(0),
_xy_reset_counter(0),
_heading_reset_counter(0)
@@ -635,13 +633,6 @@ MulticopterPositionControl::parameters_update(bool force)
/* we only use jerk for braking if jerk_hor_max > jerk_hor_min; otherwise just set jerk very large */
_manual_jerk_limit_z = (_jerk_hor_max.get() > _jerk_hor_min.get()) ? _jerk_hor_max.get() : 1000000.f;
/* Get parameter values used to fly within optical flow sensor limits */
param_t handle = param_find("SENS_FLOW_MINRNG");
if (handle != PARAM_INVALID) {
param_get(handle, &_min_hagl_limit);
}
}
return OK;
@@ -2275,7 +2266,7 @@ MulticopterPositionControl::update_velocity_derivative()
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
if (((_alt_mode.get() == 1) || _local_pos.limit_hagl) && _local_pos.dist_bottom_valid) {
if ((_alt_mode.get() == 1) && _local_pos.dist_bottom_valid) {
if (!_terrain_follow) {
_terrain_follow = true;
_reset_alt_sp = true;
@@ -2442,11 +2433,12 @@ MulticopterPositionControl::calculate_velocity_setpoint()
}
// encourage pilot to respect flow sensor minimum height limitations
if (_local_pos.limit_hagl && _local_pos.dist_bottom_valid && _control_mode.flag_control_manual_enabled
if (_terrain_follow && _local_pos.limit_hagl
&& _control_mode.flag_control_manual_enabled
&& _control_mode.flag_control_altitude_enabled) {
// If distance to ground is less than limit, increment set point upwards at up to the landing descent rate
if (_local_pos.dist_bottom < _min_hagl_limit) {
float climb_rate_bias = fminf(1.5f * _pos_p(2) * (_min_hagl_limit - _local_pos.dist_bottom), _land_speed.get());
if (_local_pos.dist_bottom < _flow_min_hgt.get()) {
float climb_rate_bias = fminf(1.5f * _pos_p(2) * (_flow_min_hgt.get() - _local_pos.dist_bottom), _land_speed.get());
_vel_sp(2) -= climb_rate_bias;
_pos_sp(2) -= climb_rate_bias * _dt;
src/modules/mc_pos_control/mc_pos_control_params.c
-15
View File
@@ -447,21 +447,6 @@ PARAM_DEFINE_FLOAT(MPC_DEC_HOR_SLOW, 5.0f);
*/
PARAM_DEFINE_FLOAT(MPC_ACC_HOR_FLOW, 0.5f);
/**
* Maximum height above ground when reliant on optical flow.
* The height setpoint will be limited to be no greater than this value when the navigation system
* is completely reliant on optical flow data and the height above ground estimate is valid as indicated
* by the local_position.dist_bottom_valid message being true.
*
* @unit m
* @min 1.0
* @max 25.0
* @increment 0.5
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_MAX_FLOW_HGT, 3.0f);
/**
* Maximum vertical acceleration in velocity controlled modes upward
*