From 3b4e5a17a20b140f2010af199733aa5e32a62c17 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Sat, 30 Sep 2023 00:56:48 +0900 Subject: [PATCH] cuav_util: consider elevation differences in distance calculation --- cuav/lib/cuav_util.py | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/cuav/lib/cuav_util.py b/cuav/lib/cuav_util.py index 84ce6005..6f019ee8 100755 --- a/cuav/lib/cuav_util.py +++ b/cuav/lib/cuav_util.py @@ -188,7 +188,24 @@ def pixel_coordinates(xpos, ypos, latitude, longitude, height, pitch, roll, yaw, bearing = math.degrees(math.atan2(xofs, yofs)) distance = math.sqrt(xofs**2 + yofs**2) - return gps_newpos(latitude, longitude, bearing, distance) + initial_pos = gps_newpos(latitude, longitude, bearing, distance) + + from MAVProxy.modules.mavproxy_map import mp_elevation + EleModel = mp_elevation.ElevationModel() + ground_elevation = EleModel.GetElevation(latitude, longitude) + + # Get the elevation of the estimated point + initial_elevation = EleModel.GetElevation(initial_pos[0], initial_pos[1]) + + # Recalculate the height difference and adjust the distance accordingly + height_diff = initial_elevation - ground_elevation + if height_diff < 0: + adjusted_distance = math.sqrt(distance**2 + height_diff**2) + else: + adjusted_distance = distance + + # Recalculate the new position with the adjusted distance + return gps_newpos(latitude, longitude, bearing, adjusted_distance) def gps_position_from_xy(x, y, pos, C=None, altitude=None, shape=None): '''