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: warning for invalid thrust sp

Browse Source
This commit is contained in:
Dennis Mannhart
2017-05-05 15:11:58 +02:00
committed by Lorenz Meier
parent e58766c394
commit 13f8936cf1
1 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1476,6 +1476,11 @@ void MulticopterPositionControl::control_auto(float dt)
if (previous_in_front) {
/* just use the default velocity along track */
vel_sp_along_track = vec_prev_to_pos.length() * _params.pos_p(0);
if (vel_sp_along_track > get_cruising_speed_xy()) {
vel_sp_along_track = get_cruising_speed_xy();
}
} else if (current_behind) {
@@ -2079,6 +2084,13 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
thrust_body_z = thr_max;
}
/* if any of the thrust setpoint is bogus, send out a warning */
for (int i = 0; i < 3; ++i) {
if (!PX4_ISFINITE(thrust_sp(i))) {
warn_rate_limited("Thrust setpoint not finite");
}
}
_att_sp.thrust = math::max(thrust_body_z, thr_min);
/* update integrals */