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

mc_pos_control: target_threshold value change

Browse Source 
mc_pos_control: reorder if statement

mc_pos_control:  add get function for cruising speed
This commit is contained in:
Dennis Mannhart
2017-03-17 13:16:27 +01:00
parent 0d6f994145
commit 4c4f214ec7
2 changed files with 53 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -191,7 +191,6 @@ private:
param_t xy_vel_d;
param_t xy_vel_max;
param_t xy_vel_cruise;
param_t xy_vel_cruise_min;
param_t xy_target_threshold;
param_t xy_ff;
param_t tilt_max_air;
@@ -232,7 +231,6 @@ private:
float vel_cruise_xy;
float vel_max_up;
float vel_max_down;
float vel_cruise_xy_min;
float target_threshold_xy;
float xy_vel_man_expo;
uint32_t alt_mode;
@@ -374,6 +372,8 @@ private:
void generate_attitude_setpoint(float dt);
float get_cruising_speed_xy();
/**
* limit altitude based on several conditions
*/
@@ -518,7 +518,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.xy_vel_d = param_find("MPC_XY_VEL_D");
_params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX");
_params_handles.xy_vel_cruise = param_find("MPC_XY_CRUISE");
_params_handles.xy_vel_cruise_min = param_find("MPC_CRUISE_MIN");
_params_handles.xy_target_threshold = param_find("MPC_TARGET_THRE");
_params_handles.xy_ff = param_find("MPC_XY_FF");
_params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR");
@@ -630,6 +629,7 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.xy_vel_cruise_min, &v);
_params.vel_cruise_xy_min = v;
param_get(_params_handles.xy_target_threshold, &v);
_params.target_threshold_xy = v;
param_get(_params_handles.xy_ff, &v);
@@ -928,14 +928,24 @@ MulticopterPositionControl::limit_vel_xy_gradually()
* the max velocity is defined by the linear line
* with x= (curr_sp - pos) and y = _vel_sp
*/
float slope = (_params.vel_cruise(0) - _params.vel_cruise_xy_min) / _params.target_threshold_xy;
float vel_limit = slope * (_curr_sp - _pos).length() + _params.vel_cruise_xy_min;
float slope = get_cruising_speed_xy() / _params.target_threshold_xy;
float vel_limit = slope * (_curr_sp - _pos).length();
float vel_mag_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
float vel_mag_valid = math::min(vel_mag_xy, vel_limit);
_vel_sp(0) = _vel_sp(0) / vel_mag_xy * vel_mag_valid;
_vel_sp(1) = _vel_sp(1) / vel_mag_xy * vel_mag_valid;
}
float
MulticopterPositionControl::get_cruising_speed_xy()
{
/*
* in mission the user can choose cruising speed different to default
*/
return ((PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && (_pos_sp_triplet.current.cruising_speed > 0.1f)) ?
_pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy);
}
void
@@ -952,7 +962,6 @@ MulticopterPositionControl::control_manual(float dt)
}
}
/*
* Map from stick input to velocity setpoint
*/
@@ -1470,32 +1479,17 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> cruising_speed(_params.vel_cruise(0),
_params.vel_cruise(1),
_params.vel_max_up);
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
_pos_sp_triplet.current.cruising_speed > 0.1f) {
cruising_speed(0) = _pos_sp_triplet.current.cruising_speed;
cruising_speed(1) = _pos_sp_triplet.current.cruising_speed;
}
/* set velocity limit if close to current setpoint and
* no next setpoint available: we only consider updated if xy is updated
*/
_limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
&& ((_curr_sp - _pos).length() <= _params.target_threshold_xy);
if (current_setpoint_valid &&
(_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {
float cruising_speed_xy = (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed)
&& (_pos_sp_triplet.current.cruising_speed > 0.1f)) ?
_pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy ;
float cruising_speed_z = (curr_sp(2) > _pos(2)) ? _params.vel_max_down : _params.vel_max_up;
float cruising_speed_xy = get_cruising_speed_xy();
float cruising_speed_z = (_curr_sp(2) > _pos(2)) ? _params.vel_max_down : _params.vel_max_up;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> cruising_speed(cruising_speed_xy,
@@ -1516,47 +1510,51 @@ void MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
/* find X - cross point of unit sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
if ((_curr_sp - prev_sp).length() > MIN_DIST) {
/* we are close to current setpoint */
if (curr_pos_s_len < 1.0f) {
/* find X - cross point of unit sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
/* if next is valid, we want to have smooth transition */
if (next_setpoint_valid && (next_sp - _curr_sp).length() > MIN_DIST) {
/* we are close to current setpoint */
if (curr_pos_s_len < 1.0f) {
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* if next is valid, we want to have smooth transition */
if (next_setpoint_valid && (next_sp - _curr_sp).length() > MIN_DIST) {
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/* cos(b), b = angle pos - _curr_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* cos(b), b = angle pos - _curr_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
/* if curr - next distance is larger than unit radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* if curr - next distance is larger than unit radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
}
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
} else {
/* if not close to current setpoint, check if we are within cross_sphere_line */
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (!near) {
@@ -1580,9 +1578,8 @@ void MulticopterPositionControl::control_auto(float dt)
/* scale back */
_pos_sp = pos_sp_s.edivide(scale);
/* default */
} else {
/* we just have a current setpoint that we want to go to */
_pos_sp = _curr_sp;
/* set max velocity to cruise */
@@ -2349,7 +2346,7 @@ MulticopterPositionControl::task_main()
setDt(dt);
/* set default max velocity in xy to vel_max */
_vel_max_xy = _params.vel_max(0);
_vel_max_xy = _params.vel_max_xy;
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */

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

@@ -256,22 +256,6 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
*/
PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
/**
* Nominal horizontal velocity minimum cruise speed when close to target
*
* Normal horizontal velocity in AUTO modes (includes
* also RTL / hold / etc.) and endpoint for
* position stabilized mode (POSCTRL).
*
* @unit m/s
* @min 0.0
* @max 10.0
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_CRUISE_MIN, 0.5f);
/**
* Distance Threshold Horizontal Auto
*
@@ -286,7 +270,7 @@ PARAM_DEFINE_FLOAT(MPC_CRUISE_MIN, 0.5f);
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TARGET_THRE, 10.0f);
PARAM_DEFINE_FLOAT(MPC_TARGET_THRE, 15.0f);
/**
* Maximum horizontal velocity