zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

mc_pos_control: format fixes

sbg
Paul Riseborough 7 years ago committed by Lorenz Meier
parent
9fb674929c
commit
0dc2377c2f
1 changed files with 9 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      src/modules/mc_pos_control/mc_pos_control_main.cpp

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

@ -853,14 +853,17 @@ MulticopterPositionControl::limit_altitude() @@ -853,14 +853,17 @@ MulticopterPositionControl::limit_altitude()
{
float altitude_above_home = -(_pos(2) - _home_pos.z);
bool applying_flow_height_limit = false;
if (_terrain_follow && _local_pos.limit_hagl) {
// Don't allow the height setpoint to exceed the optical flow limits
if (_pos_sp(2) < -_flow_max_hgt.get()) {
_pos_sp(2) = -_flow_max_hgt.get();
}
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
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
@ -876,10 +879,12 @@ MulticopterPositionControl::limit_altitude() @@ -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;
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
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
_run_alt_control = true;
} 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
_pos_sp(2) = -_flow_max_hgt.get();
@ -2276,6 +2281,7 @@ MulticopterPositionControl::update_velocity_derivative() @@ -2276,6 +2281,7 @@ MulticopterPositionControl::update_velocity_derivative()
_reset_alt_sp = true;
reset_alt_sp();
}
_pos(2) = -_local_pos.dist_bottom;
} else {
@ -2284,6 +2290,7 @@ MulticopterPositionControl::update_velocity_derivative() @@ -2284,6 +2290,7 @@ MulticopterPositionControl::update_velocity_derivative()
_reset_alt_sp = true;
reset_alt_sp();
}
_pos(2) = _local_pos.z;
}
}

Write Preview
Loading…
Cancel
Save