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

correct thrust limiting during landing with margin, don't reset position setpoint when switching from takeoff to posctl and allow high enough z velocity for position lock

This commit is contained in:
Andreas Antener 2015-12-19 13:09:52 +01:00 committed by Lorenz Meier
parent 09b5bdb1ee
commit ea7a1a92b5
2 changed files with 26 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/4030_solo
View File

@ -24,6 +24,7 @@ then
param set MIS_TAKEOFF_ALT 5.0
param set COM_DISARM_LAND 1
param set RTL_LAND_DELAY 1
param set MPC_HOLD_MAX_Z 1.5
# enable high bandwidth mavlink by default
param set SYS_COMPANION 921601

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

@ -242,6 +242,7 @@ private:
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
float _landing_thrust;
hrt_abstime _landing_started;
/**
* Update our local parameter cache.
@ -372,7 +373,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_run_pos_control(true),
_run_alt_control(true),
_yaw(0.0f),
_landing_thrust(1.0f)
_landing_thrust(1.0f),
_landing_started(0)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
@ -951,10 +953,6 @@ void MulticopterPositionControl::control_auto(float dt)
}
if (current_setpoint_valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
@ -1059,14 +1057,20 @@ void MulticopterPositionControl::control_auto(float dt)
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
/* if we're near the current pos SP don't reset it anymore, else it will make if jump back,
* especially noticable in altitude after takeoff.
/*
* if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
* this makes the takeoff finish smoothly.
*/
if (current_setpoint_valid
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _pos_sp_triplet.current.acceptance_radius > 0.0f
&& (curr_sp - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius) {
&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.1f) {
_reset_pos_sp = false;
_reset_alt_sp = false;
/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
} else {
_reset_pos_sp = true;
_reset_alt_sp = true;
}
} else {
@ -1432,6 +1436,7 @@ MulticopterPositionControl::task_main()
thr_min = 0.0f;
}
float thrust_abs = thrust_sp.length();
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
@ -1440,19 +1445,26 @@ MulticopterPositionControl::task_main()
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (_landing_started == 0) {
_landing_started = hrt_absolute_time();
}
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
/* don't let it throttle up again during landing */
if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) {
_landing_thrust = thrust_sp(2);
if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust
&& hrt_elapsed_time(&_landing_started) > 15e5) {
_landing_thrust = thrust_abs;
}
// XXX: we probably need to add a margin here becaue we're limiting the complete thrust vector further down
thr_max = -_landing_thrust;
/* add 5% to give it some margin to compensate external influences */
thr_max = _landing_thrust + 0.05f * _landing_thrust;
/*PX4_WARN("thrust: abs %.4f, sp(2) %.4f, _landing_thrust %.4f, vel_err(2) %.4f, vel_sp(2) %.4f, vel(2) %.4f",
(double) thrust_abs, (double) thrust_sp(2), (double) _landing_thrust, (double) vel_err(2), (double) _vel_sp(2), (double) _vel(2));*/
} else {
_landing_started = 0;
_landing_thrust = _params.thr_max;
}
@ -1503,8 +1515,6 @@ MulticopterPositionControl::task_main()
}
/* limit max thrust */
float thrust_abs = thrust_sp.length();
if (thrust_abs > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {