yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Unify optical flow height limiting

Browse Source
This commit is contained in:
Mohammed Kabir
2018-05-12 16:47:07 -04: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

22
src/modules/mc_pos_control/mc_pos_control_main.cpp
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;

15
src/modules/mc_pos_control/mc_pos_control_params.c
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
*

31
src/modules/sensors/sensor_params.c
View File

@@ -162,37 +162,6 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
*/
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
/**
* PX4Flow board rotation
*
* This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame.
* Zero rotation is defined as X on flow board pointing towards front of vehicle.
* The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw).
*
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
*
* @reboot_required true
*
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6);
/**
* Optical Flow minimum focus distance
*
* This parameter defines the minimum distance from ground required for the optical flow sensor to operate reliably. The sensor may be usable below this height, but accuracy will progressively reduce to loss of focus.
* *
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_FLOW_MINRNG, 0.7f);
/**
* Board rotation Y (Pitch) offset
*

86
src/modules/sensors/sensor_params_flow.c Normal file
View File

@@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* PX4Flow board rotation
*
* This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame.
* Zero rotation is defined as X on flow board pointing towards front of vehicle.
* The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw).
*
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
*
* @reboot_required true
*
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6);
/**
* Minimum height above ground when reliant on optical flow.
*
* This parameter defines the minimum distance from ground at which the optical flow sensor operates reliably.
* The sensor may be usable below this height, but accuracy will progressively reduce to loss of focus.
*
* @unit m
* @min 0.0
* @max 1.0
* @increment 0.1
* @decimal 1
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f);
/**
* Maximum height above ground when reliant on optical flow.
*
* This parameter defines the maximum distance from ground at which the optical flow sensor operates reliably.
* 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.
* The sensor may be usable above this height, but accuracy will progressively degrade.
*
* @unit m
* @min 1.0
* @max 25.0
* @increment 0.1
* @decimal 1
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f);