fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 05:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: use math::constrain

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent 947d63fb11
commit 2ceb703613
1 changed files with 3 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -10
View File
@@ -953,15 +953,9 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
vel_close = a * powf(b, angle) + c;
}
/* sanity check: vel_close needs to be in between max and min */
if ((vel_close - min_cruise_speed) < SIGMA_SINGLE_OP) {
vel_close = min_cruise_speed;
/* vel_close needs to be in between max and min */
return math::constrain(vel_close, min_cruise_speed, get_cruising_speed_xy());
} else if (vel_close > get_cruising_speed_xy()) {
vel_close = get_cruising_speed_xy();
}
return vel_close;
}
float
@@ -1746,8 +1740,7 @@ void MulticopterPositionControl::control_auto(float dt)
}
/* enforce minimum cruise speed */
vel_sp_along_track = (vel_sp_along_track < SIGMA_NORM) ? SIGMA_NORM : vel_sp_along_track;
vel_sp_along_track = (vel_sp_along_track > final_cruise_speed) ? final_cruise_speed : vel_sp_along_track;
vel_sp_along_track = math::constrain(vel_sp_along_track, SIGMA_NORM, final_cruise_speed);
} else if (close_to_current) {
/* slow down when close to current setpoint */