diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index bfca95df146f9d..dd5697330f29cb 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -886,15 +886,15 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const bool Plane::set_target_location(const Location &target_loc) { Location loc{target_loc}; + fix_terrain_WP(loc, __LINE__); if (plane.control_mode != &plane.mode_guided) { // only accept position updates when in GUIDED mode return false; } - // add home alt if needed - if (loc.relative_alt) { - loc.alt += plane.home.alt; - loc.relative_alt = 0; + // convert to absolute + if (!loc.terrain_alt) { + loc.change_alt_frame(Location::AltFrame::ABSOLUTE); } plane.set_guided_WP(loc); return true; @@ -942,6 +942,8 @@ bool Plane::update_target_location(const Location &old_loc, const Location &new_ } next_WP_loc = new_loc; + fix_terrain_WP(next_WP_loc, __LINE__); + #if HAL_QUADPLANE_ENABLED if (control_mode == &mode_qland || control_mode == &mode_qloiter) { mode_qloiter.last_target_loc_set_ms = AP_HAL::millis(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b2bc113b697f9d..1b3a80cc3957ee 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -748,12 +748,17 @@ bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd) */ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &cmd) { - plane.next_WP_loc.alt = cmd.content.location.alt; - if (cmd.content.location.relative_alt && !cmd.content.location.terrain_alt) { - plane.next_WP_loc.alt += plane.home.alt; + plane.fix_terrain_WP(cmd.content.location, __LINE__); + + if (cmd.content.location.terrain_alt) { + plane.next_WP_loc.set_alt_cm(cmd.content.location.alt, Location::AltFrame::ABOVE_TERRAIN); + } else { + // convert to absolute alt + float abs_alt_m; + if (cmd.content.location.get_alt_m(Location::AltFrame::ABSOLUTE, abs_alt_m)) { + plane.next_WP_loc.set_alt_m(abs_alt_m, Location::AltFrame::ABSOLUTE); + } } - plane.next_WP_loc.relative_alt = false; - plane.next_WP_loc.terrain_alt = cmd.content.location.terrain_alt; plane.reset_offset_altitude(); } @@ -839,6 +844,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com if (!location_from_command_t(packet, requested_position)) { return MAV_RESULT_DENIED; } + plane.fix_terrain_WP(requested_position, __LINE__); if (isnan(packet.param4) || is_zero(packet.param4)) { requested_position.loiter_ccw = 0; @@ -864,10 +870,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com (plane.control_mode == &plane.mode_guided)) { plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); - // add home alt if needed - if (requested_position.relative_alt && !requested_position.terrain_alt) { - requested_position.alt += plane.home.alt; - requested_position.relative_alt = 0; + // convert to absolute alt + if (!requested_position.terrain_alt) { + requested_position.change_alt_frame(Location::AltFrame::ABSOLUTE); } plane.set_guided_WP(requested_position); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1a78a38c1d8e96..7c453eacf3b8dd 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -897,6 +897,9 @@ class Plane : public AP_Vehicle { int32_t relative_target_altitude_cm(void); void change_target_altitude(int32_t change_cm); void set_target_altitude_proportion(const Location &loc, float proportion); +#if AP_TERRAIN_AVAILABLE + bool set_target_altitude_proportion_terrain(void); +#endif void constrain_target_altitude_location(const Location &loc1, const Location &loc2); int32_t calc_altitude_error_cm(void); void check_fbwb_altitude(void); @@ -909,6 +912,7 @@ class Plane : public AP_Vehicle { float mission_alt_offset(void); float height_above_target(void); float lookahead_adjustment(void); + void fix_terrain_WP(Location &loc, uint32_t linenum); #if AP_RANGEFINDER_ENABLED float rangefinder_correction(void); void rangefinder_height_update(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 1b2ea00d362569..2b792dbe0e17f9 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -34,15 +34,8 @@ void Plane::check_home_alt_change(void) if (home_alt_cm != auto_state.last_home_alt_cm && hal.util->get_soft_armed()) { // cope with home altitude changing const int32_t alt_change_cm = home_alt_cm - auto_state.last_home_alt_cm; - if (next_WP_loc.terrain_alt) { - /* - next_WP_loc for terrain alt WP are quite strange. They - have terrain_alt=1, but also have relative_alt=0 and - have been calculated to be relative to home. We need to - adjust for the change in home alt - */ - next_WP_loc.alt += alt_change_cm; - } + fix_terrain_WP(next_WP_loc, __LINE__); + // reset TECS to force the field elevation estimate to reset TECS_controller.offset_altitude(alt_change_cm * 0.01f); } @@ -244,10 +237,6 @@ void Plane::set_target_altitude_location(const Location &loc) if (loc.terrain_alt && terrain.height_above_terrain(height, true)) { target_altitude.terrain_following = true; target_altitude.terrain_alt_cm = loc.alt; - if (!loc.relative_alt) { - // it has home added, remove it - target_altitude.terrain_alt_cm -= home.alt; - } } else { target_altitude.terrain_following = false; } @@ -300,6 +289,7 @@ void Plane::change_target_altitude(int32_t change_cm) } #endif } + /* change target altitude by a proportion of the target altitude offset (difference in height to next WP from previous WP). proportion @@ -329,6 +319,46 @@ void Plane::set_target_altitude_proportion(const Location &loc, float proportion } } +#if AP_TERRAIN_AVAILABLE +/* + change target altitude along a path between two locations + (prev_WP_loc and next_WP_loc) where the second location is a terrain + altitude + */ +bool Plane::set_target_altitude_proportion_terrain(void) +{ + if (!next_WP_loc.terrain_alt || + !next_WP_loc.relative_alt) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return false; + } + /* + we first need to get the height of the terrain at prev_WP_loc + */ + float prev_WP_height_terrain; + if (!plane.prev_WP_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, + prev_WP_height_terrain)) { + return false; + } + // and next_WP_loc alt as terrain + float next_WP_height_terrain; + if (!plane.next_WP_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, + next_WP_height_terrain)) { + return false; + } + const float p = constrain_float(plane.auto_state.wp_proportion, 0, 1); + Location loc = next_WP_loc; + const auto alt = linear_interpolate(prev_WP_height_terrain, next_WP_height_terrain, + p, 0, 1); + + loc.set_alt_m(alt, Location::AltFrame::ABOVE_TERRAIN); + + set_target_altitude_location(loc); + + return true; +} +#endif // AP_TERRAIN_AVAILABLE + /* constrain target altitude to be between two locations. Used to ensure we stay within two waypoints in altitude @@ -914,3 +944,24 @@ float Plane::get_landing_height(bool &rangefinder_active) return height; } + +/* + if a terrain location doesn't have the relative_alt flag set + then fix the alt and trigger a flow of control error + */ +void Plane::fix_terrain_WP(Location &loc, uint32_t linenum) +{ + if (loc.terrain_alt && !loc.relative_alt) { + AP::internalerror().error(AP_InternalError::error_t::flow_of_control, linenum); + /* + we definately have a bug, now we need to guess what was + really meant. The lack of the relative_alt flag notionally + means that home.alt has been added to loc.alt, so remove it, + but only if it doesn't lead to a negative terrain altitude + */ + if (loc.alt - home.alt > -500) { + loc.alt -= home.alt; + } + loc.relative_alt = true; + } +} diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 7879defdd794df..bc13d32153fd6b 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -33,16 +33,15 @@ void Plane::set_next_WP(const Location &loc) next_WP_loc.lng = current_loc.lng; // additionally treat zero altitude as current altitude if (next_WP_loc.alt == 0) { - next_WP_loc.alt = current_loc.alt; - next_WP_loc.relative_alt = false; - next_WP_loc.terrain_alt = false; + next_WP_loc.set_alt_cm(current_loc.alt, Location::AltFrame::ABSOLUTE); } } + fix_terrain_WP(next_WP_loc, __LINE__); + // convert relative alt to absolute alt - if (next_WP_loc.relative_alt) { - next_WP_loc.relative_alt = false; - next_WP_loc.alt += home.alt; + if (!next_WP_loc.terrain_alt) { + next_WP_loc.change_alt_frame(Location::AltFrame::ABSOLUTE); } // are we already past the waypoint? This happens when we jump @@ -85,6 +84,8 @@ void Plane::set_guided_WP(const Location &loc) // --------------------- next_WP_loc = loc; + fix_terrain_WP(next_WP_loc, __LINE__); + // used to control FBW and limit the rate of climb // ----------------------------------------------- set_target_altitude_current(); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index e019b92b0523bc..6e0badfca3f1a9 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -337,6 +337,9 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) auto_state.crosstrack = false; prev_WP_loc = current_loc; next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); + + fix_terrain_WP(next_WP_loc, __LINE__); + setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 6264f50dd2fdd8..261d85bcf5daa5 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -199,7 +199,13 @@ void Mode::update_target_altitude() // once we reach a loiter target then lock to the final // altitude target plane.set_target_altitude_location(plane.next_WP_loc); - } else if (plane.target_altitude.offset_cm != 0 && +#if AP_TERRAIN_AVAILABLE + } else if (plane.next_WP_loc.terrain_alt && + plane.set_target_altitude_proportion_terrain()) { + // special case for target as terrain relative handled inside + // set_target_altitude_proportion_terrain +#endif + } else if (plane.target_altitude.offset_cm != 0 && !plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) { // control climb/descent rate plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion); diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index cb4a1ccd4baa46..82077a61bdb247 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -108,10 +108,10 @@ void ModeGuided::navigate() bool ModeGuided::handle_guided_request(Location target_loc) { + plane.fix_terrain_WP(target_loc, __LINE__); // add home alt if needed - if (target_loc.relative_alt && !target_loc.terrain_alt) { - target_loc.alt += plane.home.alt; - target_loc.relative_alt = 0; + if (!target_loc.terrain_alt) { + target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE); } plane.set_guided_WP(target_loc); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 322d2fc16db0b2..fb87e02b0c70cb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2814,13 +2814,6 @@ void QuadPlane::vtol_position_controller(void) 0, 1); } } -#if AP_TERRAIN_AVAILABLE - float terrain_altitude_offset; - if (plane.next_WP_loc.terrain_alt && plane.terrain.height_terrain_difference_home(terrain_altitude_offset, true)) { - // Climb if current terrain is above home, target_altitude_cm is reltive to home - target_altitude_cm += MAX(terrain_altitude_offset*100,0); - } -#endif float zero = 0; float target_z = target_altitude_cm; pos_control->input_pos_vel_accel_z(target_z, zero, 0); diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index f5ab47b6f6a1c8..977b6c11a5f810 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -2456,82 +2456,161 @@ def terrain_height_range(mav, m): self.location_offset_ne(loc2, 250, 0) loc2.alt = 150 - self.progress(f"Flying to loc1 at {loc1.alt:.1f} from {start_alt:.1f}") - self.send_do_reposition(loc1, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + loc3 = copy.copy(loc2) + loc3.alt = 100 + + positions = [ + ("Loc1", loc1), + ("Loc2", loc2), + ("Loc1", loc1), + ("Loc3", loc3), + ("Loc1", loc1), + ("Loc3", loc3), + ("Loc1", loc1), + ("Loc3", loc3), + ("Loc2", loc2), + ("Loc3", loc3), + ("Loc2", loc2), + ] + for (name, loc) in positions: + start_alt = self.assert_receive_message('TERRAIN_REPORT').current_height + terrain_height_min = start_alt + terrain_height_max = start_alt + + self.progress(f"Flying to {name} at {loc.alt:.1f} from {start_alt:.1f}") + self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + + self.wait_location( + loc, + accuracy=10, + timeout=600, + height_accuracy=10, + ) + self.delay_sim_time(10) + self.wait_altitude( + loc.alt-5, + loc.alt+5, + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + self.wait_groundspeed(0, 2) + self.wait_altitude( + loc.alt-5, + loc.alt+5, + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + min_alt_ok = min(start_alt, loc.alt) - 10 + max_alt_ok = max(start_alt, loc.alt) + 10 + self.progress(f"theight {terrain_height_min:.0f} to {terrain_height_max:.0f} accept {min_alt_ok:.0f}:{max_alt_ok:.0f}") # noqa:E501 + if terrain_height_min < min_alt_ok or terrain_height_max > max_alt_ok: + raise NotAchievedException(f"terrain range breach {start_alt:.1f} {terrain_height_min:.1f} {terrain_height_max:.1f}") # noqa:E501 - self.wait_location( - loc1, - accuracy=10, - timeout=600, - height_accuracy=10, - ) - self.delay_sim_time(10) - self.wait_altitude( - loc1.alt-5, - loc1.alt+5, - minimum_duration=10, - timeout=30, - relative=False, - altitude_source="TERRAIN_REPORT.current_height" + self.change_mode("QLAND") + self.mav.motors_disarmed_wait() + + def DoRepositionTerrain2(self): + '''test handling of DO_REPOSITION terrain alt2''' + self.install_terrain_handlers_context() + self.start_subtest("test reposition terrain alt2") + + takeoff_loc = mavutil.location(-35.28243788, 149.00502473, 583.7) + self.customise_SITL_commandline( + ["--home", f"{takeoff_loc.lat},{takeoff_loc.lng},{takeoff_loc.alt},0"] ) - self.progress(f"terrain_height range {terrain_height_min:.1f} {terrain_height_max:.1f}") - if terrain_height_min < min(start_alt, loc1.alt) - 10 or terrain_height_max > max(start_alt, loc1.alt)+10: - raise NotAchievedException(f"terrain range breach {start_alt:.1f} {terrain_height_min:.1f} {terrain_height_max:.1f}") # noqa:E501 + self.reboot_sitl(check_position=False) + self.wait_ready_to_arm() - start_alt = self.assert_receive_message('TERRAIN_REPORT').current_height - terrain_height_min = start_alt - terrain_height_max = start_alt + dest = copy.copy(takeoff_loc) + dest.alt = 45 - self.progress(f"Flying to loc2 at {loc2.alt:.1f} from {start_alt:.1f}") - self.send_do_reposition(loc2, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + self.set_parameters({ + 'Q_GUIDED_MODE': 1, + }) - self.wait_location( - loc2, - accuracy=10, - timeout=600, - height_accuracy=10, - ) - self.delay_sim_time(10) - self.wait_altitude( - loc2.alt-5, - loc2.alt+5, - minimum_duration=10, - timeout=30, - relative=False, - altitude_source="TERRAIN_REPORT.current_height" - ) - self.progress(f"terrain_height range {terrain_height_min:.1f} {terrain_height_max:.1f}") - if terrain_height_min < min(start_alt, loc2.alt) - 10 or terrain_height_max > max(start_alt, loc2.alt)+10: - raise NotAchievedException(f"terrain range breach {start_alt:.1f} {terrain_height_min:.1f} {terrain_height_max:.1f}") # NOQA:E501 + self.takeoff(75, mode='GUIDED', timeout=60) + # remember the range of heights we go through in the tests start_alt = self.assert_receive_message('TERRAIN_REPORT').current_height terrain_height_min = start_alt terrain_height_max = start_alt - self.progress(f"Flying back to loc1 at {loc1.alt:.1f} from {start_alt:.1f}") - self.send_do_reposition(loc1, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + def terrain_height_range(mav, m): + if m.get_type() == 'TERRAIN_REPORT': + nonlocal terrain_height_min, terrain_height_max + terrain_height_min = min(terrain_height_min, m.current_height) + terrain_height_max = max(terrain_height_max, m.current_height) - self.wait_location( - loc1, - accuracy=10, - timeout=600, - height_accuracy=10, - ) - self.delay_sim_time(10) - self.wait_altitude( - loc1.alt-5, - loc1.alt+5, - minimum_duration=10, - timeout=30, - relative=False, - altitude_source="TERRAIN_REPORT.current_height" - ) - self.progress(f"terrain_height range {terrain_height_min:.1f} {terrain_height_max:.1f}") - if terrain_height_min < min(start_alt, loc1.alt) - 10 or terrain_height_max > max(start_alt, loc1.alt)+10: - raise NotAchievedException(f"terrain range breach {start_alt:.1f} {terrain_height_min:.1f} {terrain_height_max:.1f}") # NOQA:E501 + self.install_message_hook_context(terrain_height_range) + + loc1 = mavutil.location(-35.27502040, 148.98635977, 75) + loc2 = mavutil.location(-35.28505202, 148.98604378, 75) + + loc3 = mavutil.location(-35.27502040, 148.98635977, 120) + loc4 = mavutil.location(-35.28505202, 148.98604378, 120) + + loc5 = mavutil.location(-35.28505202, 148.98604378, 100) + + positions = [ + ("Loc1", loc1), + ("Loc2", loc2), + ("Loc1", loc1), + ("Loc2", loc2), + ("Loc1", loc1), + ("Loc4", loc4), + ("Loc3", loc3), + ("Loc2", loc2), + ("Loc5", loc5), + ("Loc1", loc1), + ("Loc5", loc5), + ("Loc1", loc1), + ] + for (name, loc) in positions: + start_alt = self.assert_receive_message('TERRAIN_REPORT').current_height + terrain_height_min = start_alt + terrain_height_max = start_alt + + self.progress(f"Flying to {name} at {loc.alt:.1f} from {start_alt:.1f}") + self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + + self.wait_location( + loc, + accuracy=10, + timeout=600, + height_accuracy=10, + ) + self.delay_sim_time(10) + self.wait_altitude( + loc.alt-5, + loc.alt+5, + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + + self.wait_groundspeed(0, 2) + self.wait_altitude( + loc.alt-5, + loc.alt+5, + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + min_alt_ok = min(start_alt, loc.alt) - 10 + max_alt_ok = max(start_alt, loc.alt) + 25 + self.progress(f"theight {terrain_height_min:.0f} to {terrain_height_max:.0f} accept {min_alt_ok:.0f}:{max_alt_ok:.0f}") # noqa:E501 + if terrain_height_min < min_alt_ok or terrain_height_max > max_alt_ok: + raise NotAchievedException(f"terrain range breach {start_alt:.1f} {terrain_height_min:.1f} {terrain_height_max:.1f}") # noqa:E501 self.change_mode("QLAND") self.mav.motors_disarmed_wait() + self.reset_SITL_commandline() def tests(self): '''return list of all tests''' @@ -2585,9 +2664,10 @@ def tests(self): self.RTL_AUTOLAND_1, # as in fly-home then go to landing sequence self.RTL_AUTOLAND_1_FROM_GUIDED, # as in fly-home then go to landing sequence self.AHRSFlyForwardFlag, + self.DoRepositionTerrain, + self.DoRepositionTerrain2, self.QLoiterRecovery, self.FastInvertedRecovery, self.CruiseRecovery, - self.DoRepositionTerrain, ]) return ret diff --git a/Tools/autotest/tilecache/srtm/S36E148.hgt.zip b/Tools/autotest/tilecache/srtm/S36E148.hgt.zip new file mode 100644 index 00000000000000..f556db65b38976 Binary files /dev/null and b/Tools/autotest/tilecache/srtm/S36E148.hgt.zip differ diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d7aa5c7cd3da50..6a710866adf650 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1064,14 +1064,14 @@ bool AP_AHRS_DCM::get_location(Location &loc) const loc.lng = _last_lng; const auto &baro = AP::baro(); const auto &gps = AP::gps(); + int32_t alt_cm; if (_gps_use == GPSUse::EnableWithHeight && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - loc.alt = gps.location().alt; + alt_cm = gps.location().alt; } else { - loc.alt = baro.get_altitude() * 100 + AP::ahrs().get_home().alt; + alt_cm = baro.get_altitude() * 100 + AP::ahrs().get_home().alt; } - loc.relative_alt = 0; - loc.terrain_alt = 0; + loc.set_alt_cm(alt_cm, Location::AltFrame::ABSOLUTE); loc.offset(_position_offset_north, _position_offset_east); if (_have_position) { const uint32_t now = AP_HAL::millis(); diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 27803566d0bfd2..42ee309ab84530 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -96,6 +96,11 @@ bool Location::change_alt_frame(AltFrame desired_frame) Location::AltFrame Location::get_alt_frame() const { if (terrain_alt) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (!relative_alt) { + AP_HAL::panic("terrain loc must be relative_alt1"); + } +#endif return AltFrame::ABOVE_TERRAIN; } if (origin_alt) { @@ -114,6 +119,9 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const if (!initialised()) { AP_HAL::panic("Should not be called on invalid location: Location cannot be (0, 0, 0)"); } + if (terrain_alt && !relative_alt) { + AP_HAL::panic("terrain loc must be relative_alt2"); + } #endif Location::AltFrame frame = get_alt_frame(); diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index f61756d7d9ece8..14cb3b36a25996 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -35,6 +35,10 @@ class Location // set altitude void set_alt_cm(int32_t alt_cm, AltFrame frame); + // set_alt_m - set altitude in metres + void set_alt_m(float alt_m, AltFrame frame) { + set_alt_cm(alt_m*100, frame); + } // get altitude (in cm) in the desired frame // returns false on failure to get altitude in the desired frame which can only happen if the original frame or desired frame is: