From 642a4a5fc7afd913661c24b2394497917f5b53da Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 18 Oct 2017 08:22:35 +1100
Subject: [PATCH] mc_pos_control: respect estimator max speed and min height
limits
---
.../mc_pos_control/mc_pos_control_main.cpp | 48 ++++++++++++++++++-
.../mc_pos_control/mc_pos_control_params.c | 14 ++++++
2 files changed, 60 insertions(+), 2 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 94fb42bbcd..009d2e43f3 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -231,6 +231,7 @@ private:
param_t opt_recover;
param_t rc_flt_smp_rate;
param_t rc_flt_cutoff;
+ param_t acc_max_flow_xy;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -259,6 +260,7 @@ private:
float rc_flt_smp_rate;
float rc_flt_cutoff;
+ float acc_max_flow_xy;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
@@ -298,6 +300,8 @@ 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;
@@ -474,6 +478,7 @@ 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)
@@ -537,6 +542,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.alt_mode = param_find("MPC_ALT_MODE");
_params_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF");
_params_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE");
+ _params_handles.acc_max_flow_xy = param_find("MPC_ACC_HOR_FLOW");
/* fetch initial parameter values */
parameters_update(true);
@@ -703,6 +709,16 @@ 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);
+ }
+
+ if (_params_handles.acc_max_flow_xy != PARAM_INVALID) {
+ param_get(handle, &_params.acc_max_flow_xy);
+ }
}
@@ -1357,6 +1373,7 @@ MulticopterPositionControl::control_manual()
/* reset position setpoint to current position if needed */
reset_pos_sp();
+
}
/* prepare yaw to rotate into NED frame */
@@ -2482,6 +2499,18 @@ MulticopterPositionControl::calculate_velocity_setpoint()
_vel_sp(2) = math::max(_vel_sp(2), -vel_limit);
}
+ // encourage pilot to respect respect flow sensor minimum height limitations
+ if (_local_pos.limit_hagl && _local_pos.dist_bottom_valid && _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 * _params.pos_p(2) * (_min_hagl_limit - _local_pos.dist_bottom), _params.land_speed);
+ _vel_sp(2) -= climb_rate_bias;
+ _pos_sp(2) -= climb_rate_bias * dt;
+
+ }
+ }
+
/* limit vertical downwards speed (positive z) close to ground
* for now we use the altitude above home and assume that we want to land at same height as we took off */
float vel_limit = math::gradual(altitude_above_home,
@@ -3048,8 +3077,23 @@ MulticopterPositionControl::task_main()
/* set dt for control blocks */
setDt(dt);
- /* set default max velocity in xy to vel_max */
- _vel_max_xy = _params.vel_max_xy;
+ /* set default max velocity in xy to vel_max
+ * Apply estimator limits if applicable */
+ if (_local_pos.vxy_max > 0.0f) {
+ // use the minimum of the estimator and user specified limit
+ _vel_max_xy = fminf(_params.vel_max_xy, _local_pos.vxy_max);
+ // Allow for a minimum of 0.3 m/s for repositioning
+ _vel_max_xy = fmaxf(_vel_max_xy, 0.3f);
+
+ } else {
+ // raise the limit at a constant rate up to the user specified value
+ if (_vel_max_xy >= _params.vel_max_xy) {
+ _vel_max_xy = _params.vel_max_xy;
+
+ } else {
+ _vel_max_xy += dt * _params.acc_max_flow_xy;
+ }
+ }
/* reset flags when landed */
if (_vehicle_land_detected.landed) {
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 511bfec7c9..245cbaadbf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -437,6 +437,20 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 5.0f);
*/
PARAM_DEFINE_FLOAT(MPC_DEC_HOR_SLOW, 5.0f);
+/**
+ * Horizontal acceleration in manual modes when optical flow ground speed limit is removed.
+ * If full stick is being applied and the EKF starts using GPS whilst using optical flow,
+ * the vehicle will accelerate at this rate until the normal position control speed is achieved.
+ *
+ * @unit m/s/s
+ * @min 0.2
+ * @max 2.0
+ * @increment 0.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_ACC_HOR_FLOW, 0.5f);
+
/**
* Maximum vertical acceleration in velocity controlled modes upward
*
--
GitLab