Browse Source

Copter: rewrite_get_alt_above_ground_cm for clarity

It's not entirely clear at a glance that we don't return an
uninitialised value off the stack here.
zr-v5.1
Peter Barker 5 years ago committed by Randy Mackay
parent
commit
136d0cce0b
  1. 19
      ArduCopter/mode.cpp

19
ArduCopter/mode.cpp

@ -508,14 +508,19 @@ void Mode::make_safe_spool_down() @@ -508,14 +508,19 @@ void Mode::make_safe_spool_down()
*/
int32_t Mode::get_alt_above_ground_cm(void)
{
int32_t alt_above_ground;
if (!copter.get_rangefinder_height_interpolated_cm(alt_above_ground)) {
bool navigating = pos_control->is_active_xy();
if (!navigating || !copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) {
alt_above_ground = copter.current_loc.alt;
}
int32_t alt_above_ground_cm;
if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) {
return alt_above_ground_cm;
}
if (!pos_control->is_active_xy()) {
return copter.current_loc.alt;
}
return alt_above_ground;
if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) {
return alt_above_ground_cm;
}
// Assume the Earth is flat:
return copter.current_loc.alt;
}
void Mode::land_run_vertical_control(bool pause_descent)

Loading…
Cancel
Save