fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 23:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged release_v1.0.0 branch into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-25 21:45:17 +02:00
parent 74bdec2cc6 5b93d92339
commit 454becdae5
73 changed files with 2726 additions and 887 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+19 -3
View File
@@ -1037,6 +1037,21 @@ MulticopterPositionControl::task_main()
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
/* 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));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
@@ -1384,14 +1399,15 @@ MulticopterPositionControl::task_main()
}
//Control roll and pitch directly if we no aiding velocity controller is active
if(!_control_mode.flag_control_velocity_enabled) {
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//Control climb rate directly if no aiding altitude controller is active
if(!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
if (!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, _params.thr_max);
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
}
//Construct attitude setpoint rotation matrix
src/modules/mc_pos_control/mc_pos_control_params.c
+3 -2
View File
@@ -50,7 +50,7 @@
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
/**
* Maximum thrust
@@ -107,9 +107,10 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
*
* @unit m/s
* @min 0.0
* @max 8 m/s
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
/**
* Vertical velocity feed forward