diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index fe8a935dc7c74..b8d6e21db733c 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -735,6 +735,9 @@ class Plane : public AP_Vehicle { // are we trying to follow terrain? bool terrain_following; + // are we waiting to load terrain data to init terrain following + bool terrain_following_pending; + // target altitude above terrain in cm, valid if terrain_following // is set int32_t terrain_alt_cm; @@ -855,7 +858,7 @@ class Plane : public AP_Vehicle { void reset_offset_altitude(void); void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc); bool above_location_current(const Location &loc); - void setup_terrain_target_alt(Location &loc) const; + void setup_terrain_target_alt(Location &loc); int32_t adjusted_altitude_cm(void); int32_t adjusted_relative_altitude_cm(void); float mission_alt_offset(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 04aa72b270746..4c3a35f4097ca 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -212,6 +212,12 @@ void Plane::set_target_altitude_location(const Location &loc) target_altitude.amsl_cm += home.alt; } #if AP_TERRAIN_AVAILABLE + if (target_altitude.terrain_following_pending) { + /* we didn't get terrain data to init when we started on this + target, retry + */ + setup_terrain_target_alt(next_WP_loc); + } /* if this location has the terrain_alt flag set and we know the terrain altitude of our current location then treat it as a @@ -469,12 +475,16 @@ bool Plane::above_location_current(const Location &loc) modify a destination to be setup for terrain following if TERRAIN_FOLLOW is enabled */ -void Plane::setup_terrain_target_alt(Location &loc) const +void Plane::setup_terrain_target_alt(Location &loc) { #if AP_TERRAIN_AVAILABLE if (terrain_enabled_in_current_mode()) { - loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN); + if (!loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) { + target_altitude.terrain_following_pending = true; + return; + } } + target_altitude.terrain_following_pending = false; #endif } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 9ad4f9531318b..79887fabdeb3c 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -8,7 +8,11 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // default to non-VTOL loiter auto_state.vtol_loiter = false; - // log when new commands start +#if AP_TERRAIN_AVAILABLE + plane.target_altitude.terrain_following_pending = false; +#endif + + // log when new commands start if (should_log(MASK_LOG_CMD)) { logger.Write_Mission_Cmd(mission, cmd); } diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a9af6555b7a6a..068e5c5ec8849 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -97,6 +97,10 @@ bool Mode::enter() quadplane.mode_enter(); #endif +#if AP_TERRAIN_AVAILABLE + plane.target_altitude.terrain_following_pending = false; +#endif + bool enter_result = _enter(); if (enter_result) { diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index cec10da0ae226..9906a2256fe15 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1836,6 +1836,82 @@ def FenceRetRally(self): self.do_fence_disable() # Disable fence so we can land self.fly_home_land_and_disarm() # Pack it up, we're going home. + def TerrainRally(self): + """ Tests terrain follow with a rally point """ + self.context_push() + self.install_terrain_handlers_context() + + def terrain_following_above_80m(mav, m): + if m.get_type() == 'TERRAIN_REPORT': + if m.current_height < 50: + raise NotAchievedException( + "TERRAIN_REPORT.current_height below 50m %fm" % m.current_height) + if m.get_type() == 'VFR_HUD': + if m.groundspeed < 2: + raise NotAchievedException("hit ground") + + def terrain_wait_path(loc1, loc2, steps): + '''wait till we have terrain for N steps from loc1 to loc2''' + tstart = self.get_sim_time_cached() + self.progress("Waiting for terrain data") + while True: + now = self.get_sim_time_cached() + if now - tstart > 60: + raise NotAchievedException("Did not get correct required terrain") + for i in range(steps): + lat = loc1.lat + i * (loc2.lat-loc1.lat)/steps + lon = loc1.lng + i * (loc2.lng-loc1.lng)/steps + self.mav.mav.terrain_check_send(int(lat*1.0e7), int(lon*1.0e7)) + + report = self.assert_receive_message('TERRAIN_REPORT', timeout=60) + self.progress("Terrain pending=%u" % report.pending) + if report.pending == 0: + break + self.progress("Got required terrain") + + self.wait_ready_to_arm() + self.homeloc = self.mav.location() + + guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0) + rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0) + + terrain_wait_path(self.homeloc, rally_loc, 10) + + # set a rally point to the west of home + self.upload_rally_points_from_locations([rally_loc]) + + self.set_parameter("TKOFF_ALT", 100) + self.change_mode("TAKEOFF") + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("TERRAIN_FOLLOW", 1) + self.wait_altitude(90, 120, timeout=30, relative=True) + self.progress("Done takeoff") + + self.install_message_hook_context(terrain_following_above_80m) + + self.change_mode("GUIDED") + self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + self.progress("Flying to guided location") + self.wait_location(guided_loc, + accuracy=200, + target_altitude=None, + timeout=600) + + self.progress("Reached guided location") + self.set_parameter("RALLY_LIMIT_KM", 50) + self.change_mode("RTL") + self.progress("Flying to rally point") + self.wait_location(rally_loc, + accuracy=200, + target_altitude=None, + timeout=600) + self.progress("Reached rally point") + + self.context_pop() + self.disarm_vehicle(force=True) + self.reboot_sitl() + def Parachute(self): '''Test Parachute''' self.set_rc(9, 1000) @@ -5125,6 +5201,7 @@ def tests(self): self.MAV_CMD_DO_LAND_START, self.InteractTest, self.MAV_CMD_MISSION_START, + self.TerrainRally, ]) return ret diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 094c76f52a30d..723a24199fdfc 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -7085,6 +7085,35 @@ def assert_rc_channel_value(self, channel, value): raise NotAchievedException("Expected %s to be %u got %u" % (channel, value, m_value)) + def do_reposition(self, + loc, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT): + '''send a DO_REPOSITION command for a location''' + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + 0, + 0, + 0, + 0, + int(loc.lat*1e7), # lat* 1e7 + int(loc.lng*1e7), # lon* 1e7 + loc.alt, + frame=frame + ) + + def add_rally_point(self, loc, seq, total): + '''add a rally point at the given location''' + self.mav.mav.rally_point_send(1, # target system + 0, # target component + seq, # sequence number + total, # total count + int(loc.lat * 1e7), + int(loc.lng * 1e7), + loc.alt, # relative alt + 0, # "break" alt?! + 0, # "land dir" + 0) # flags + def wait_location(self, loc, accuracy=5.0,