fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 12:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added checks for setting mpc saturation flags

Browse Source
This commit is contained in:
devbharat
2016-12-27 17:38:00 +01:00
committed by Lorenz Meier
parent b596874b91
commit 5a8c542d46
1 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -6
View File
@@ -1880,7 +1880,8 @@ MulticopterPositionControl::control_position(float dt)
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
/* Don't freeze altitude integral if it wants to throttle up */
saturation_z = vel_err(2) > 0.0f ? true : saturation_z;
}
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
@@ -1898,7 +1899,8 @@ MulticopterPositionControl::control_position(float dt)
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
/* Don't freeze x,y integrals if they both want to throttle down */
saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true;
}
}
}
@@ -1934,7 +1936,8 @@ MulticopterPositionControl::control_position(float dt)
thrust_sp(1) = 0.0f;
thrust_sp(2) = -thr_max;
saturation_xy = true;
saturation_z = true;
/* Don't freeze altitude integral if it wants to throttle down */
saturation_z = vel_err(2) < 0.0f ? true : saturation_z;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
@@ -1943,7 +1946,8 @@ MulticopterPositionControl::control_position(float dt)
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
/* Don't freeze x,y integrals if they both want to throttle down */
saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true;
}
} else {
@@ -2154,7 +2158,6 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt)
_att_sp.landing_gear = -1.0f;
}
_att_sp.timestamp = hrt_absolute_time();
}
@@ -2178,7 +2181,6 @@ MulticopterPositionControl::task_main()
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
parameters_update(true);
/* initialize values of critical structs until first regular update */