dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MPC: limit tilt to maximum safe value of 89 degrees 

The algorithm fails at 90 degrees due to tanf being INF
This commit is contained in:
bresch 2020-03-23 11:24:06 +01:00 committed by Mathieu Bresciani
parent 315135c07e
commit 5babf644f0
2 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -199,6 +199,8 @@ private:
/** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */
static constexpr float ALTITUDE_THRESHOLD = 0.3f;
static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf
systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
WeatherVane *_wv_controller{nullptr};
@ -343,6 +345,18 @@ MulticopterPositionControl::parameters_update(bool force)
ModuleParams::updateParams();
SuperBlock::updateParams();
if (_param_mpc_tiltmax_air.get() > MAX_SAFE_TILT_DEG) {
_param_mpc_tiltmax_air.set(MAX_SAFE_TILT_DEG);
_param_mpc_tiltmax_air.commit();
mavlink_log_critical(&_mavlink_log_pub, "Tilt constrained to safe value");
}
if (_param_mpc_tiltmax_lnd.get() > _param_mpc_tiltmax_air.get()) {
_param_mpc_tiltmax_lnd.set(_param_mpc_tiltmax_air.get());
_param_mpc_tiltmax_lnd.commit();
mavlink_log_critical(&_mavlink_log_pub, "Land tilt has been constrained by max tilt");
}
_control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get()));
_control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()),
Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()),

4
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -306,7 +306,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f);
*
* @unit deg
* @min 20.0
* @max 180.0
* @max 89.0
* @decimal 1
* @group Multicopter Position Control
*/
@ -319,7 +319,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
*
* @unit deg
* @min 10.0
* @max 90.0
* @max 89.0
* @decimal 1
* @group Multicopter Position Control
*/