mirror of https://github.com/ArduPilot/ardupilot
autotest: add offset_location_heading_distance
This commit is contained in:
parent
eec8823eb9
commit
ef18e9bc84
|
@ -9428,6 +9428,20 @@ Also, ignores heartbeats not from our target system'''
|
||||||
location.alt,
|
location.alt,
|
||||||
location.heading)
|
location.heading)
|
||||||
|
|
||||||
|
def offset_location_heading_distance(self, location, bearing, distance):
|
||||||
|
(target_lat, target_lng) = mavextra.gps_newpos(
|
||||||
|
location.lat,
|
||||||
|
location.lng,
|
||||||
|
bearing,
|
||||||
|
distance
|
||||||
|
)
|
||||||
|
return mavutil.location(
|
||||||
|
target_lat,
|
||||||
|
target_lng,
|
||||||
|
location.alt,
|
||||||
|
location.heading
|
||||||
|
)
|
||||||
|
|
||||||
def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
|
def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
|
||||||
tstart = self.get_sim_time()
|
tstart = self.get_sim_time()
|
||||||
while True:
|
while True:
|
||||||
|
|
Loading…
Reference in New Issue