dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: format fixes

This commit is contained in:
Paul Riseborough 2018-05-11 15:10:32 +10:00 committed by Lorenz Meier
parent 9fb674929c
commit 0dc2377c2f
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -853,14 +853,17 @@ MulticopterPositionControl::limit_altitude()
{ {
float altitude_above_home = -(_pos(2) - _home_pos.z); float altitude_above_home = -(_pos(2) - _home_pos.z);
bool applying_flow_height_limit = false; bool applying_flow_height_limit = false;
if (_terrain_follow && _local_pos.limit_hagl) { if (_terrain_follow && _local_pos.limit_hagl) {
// Don't allow the height setpoint to exceed the optical flow limits // Don't allow the height setpoint to exceed the optical flow limits
if (_pos_sp(2) < -_flow_max_hgt.get()) { if (_pos_sp(2) < -_flow_max_hgt.get()) {
_pos_sp(2) = -_flow_max_hgt.get(); _pos_sp(2) = -_flow_max_hgt.get();
} }
applying_flow_height_limit = true; applying_flow_height_limit = true;
} else if (_run_alt_control && (_vehicle_land_detected.alt_max > 0.0f) && (altitude_above_home > _vehicle_land_detected.alt_max)) { } else if (_run_alt_control && (_vehicle_land_detected.alt_max > 0.0f)
&& (altitude_above_home > _vehicle_land_detected.alt_max)) {
// we are above maximum altitude // we are above maximum altitude
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
@ -876,10 +879,12 @@ MulticopterPositionControl::limit_altitude()
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t; float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t;
if (!applying_flow_height_limit && (-(pos_z_next - _home_pos.z) > _vehicle_land_detected.alt_max) && (_vehicle_land_detected.alt_max > 0.0f)) { if (!applying_flow_height_limit && (-(pos_z_next - _home_pos.z) > _vehicle_land_detected.alt_max)
&& (_vehicle_land_detected.alt_max > 0.0f)) {
// prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint // prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
_run_alt_control = true; _run_alt_control = true;
} else if (applying_flow_height_limit && (pos_z_next < -_flow_max_hgt.get())) { } else if (applying_flow_height_limit && (pos_z_next < -_flow_max_hgt.get())) {
// prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint // prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint
_pos_sp(2) = -_flow_max_hgt.get(); _pos_sp(2) = -_flow_max_hgt.get();
@ -2276,6 +2281,7 @@ MulticopterPositionControl::update_velocity_derivative()
_reset_alt_sp = true; _reset_alt_sp = true;
reset_alt_sp(); reset_alt_sp();
} }
_pos(2) = -_local_pos.dist_bottom; _pos(2) = -_local_pos.dist_bottom;
} else { } else {
@ -2284,6 +2290,7 @@ MulticopterPositionControl::update_velocity_derivative()
_reset_alt_sp = true; _reset_alt_sp = true;
reset_alt_sp(); reset_alt_sp();
} }
_pos(2) = _local_pos.z; _pos(2) = _local_pos.z;
} }
} }