fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: limit altitude

Browse Source 
mc_pos_control: delete spaces

mc_pos_control: cleanup and threshold for velocity control to altitude control when close to max altitude
This commit is contained in:
Dennis Mannhart 2017-02-27 09:38:53 +01:00 committed by Lorenz Meier
parent 497a210742
commit c8690fd072
2 changed files with 49 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -208,7 +208,7 @@ private:
param_t alt_mode;
param_t opt_recover;
param_t xy_vel_man_expo;
param_t altitude_max;
} _params_handles; /**< handles for interesting parameters */
struct {
@ -236,6 +236,7 @@ private:
float vel_max_up;
float vel_max_down;
float xy_vel_man_expo;
float altitude_max;
uint32_t alt_mode;
int opt_recover;
@ -290,6 +291,7 @@ private:
float _vel_z_lp;
float _acc_z_lp;
float _takeoff_thrust_sp;
float _valid_altitude_max; /**< maximum altitude that can be reached based on several conditions */
// counters for reset events on position and velocity states
// they are used to identify a reset event
@ -374,6 +376,11 @@ private:
void generate_attitude_setpoint(float dt);
/**
* limit altitude based on several conditions
*/
void limit_altitude();
/**
* Shim for calling task_main from task_create.
*/
@ -532,6 +539,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.alt_mode = param_find("MPC_ALT_MODE");
_params_handles.opt_recover = param_find("VT_OPT_RECOV_EN");
_params_handles.xy_vel_man_expo = param_find("MPC_XY_MAN_EXPO");
_params_handles.altitude_max = param_find("MPC_ALTITUDE_MAX");
_valid_altitude_max = _params_handles.altitude_max;
/* fetch initial parameter values */
parameters_update(true);
@ -649,6 +659,11 @@ MulticopterPositionControl::parameters_update(bool force)
_params.acc_down_max = v;
param_get(_params_handles.xy_vel_man_expo, &v);
_params.xy_vel_man_expo = v;
param_get(_params_handles.altitude_max, &v);
_params.altitude_max = v;
/* ToDo: the max altitude will be sent from navigator */
_valid_altitude_max = _params.altitude_max;
/*
* increase the maximum horizontal acceleration such that stopping
@ -912,6 +927,25 @@ MulticopterPositionControl::reset_alt_sp()
}
}
void
MulticopterPositionControl::limit_altitude()
{
float alt = _pos(2);
/* in altitude control, limit setpoint */
if (-alt >= _valid_altitude_max && _run_alt_control && _pos_sp(2) <= alt) {
_pos_sp(2) = -_valid_altitude_max;
return;
}
/* in velocity control in z, prevent vehicle from flying upwards if 0.5m close to altitude max*/
if (-alt + 0.5f >= _valid_altitude_max && !_run_alt_control && _vel_sp(2) <= 0.0f) {
_pos_sp(2) = -_valid_altitude_max;
_run_alt_control = true;
return;
}
}
void
@ -1667,11 +1701,12 @@ MulticopterPositionControl::control_position(float dt)
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
limit_altitude();
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));

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

@ -424,7 +424,6 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f);
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f);
/**
* Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)
*
@ -519,3 +518,15 @@ PARAM_DEFINE_INT32(MPC_ALT_MODE, 0);
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.0f);
/**
* Maximum altitude that can be reached
*
* @unit m
* @min 5
* @max 150
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_ALTITUDE_MAX, 10.0f);