Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

map: factor out methods for displaying vehicle on map #1407

Merged
merged 1 commit into from
Jun 26, 2024
Merged

map: factor out methods for displaying vehicle on map #1407

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 44 additions & 18 deletions MAVProxy/modules/mavproxy_map/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -894,6 +894,15 @@ def create_vehicle_icon(self, name, colour, follow=False, vehicle_type=None):
self.map.add_object(mp_slipmap.SlipIcon(name, (0,0), icon, layer=3, rotation=0, follow=follow,
trail=mp_slipmap.SlipTrail()))

def remove_vehicle_icon(self, name, vehicle_type=None):
from MAVProxy.modules.mavproxy_map import mp_slipmap
if vehicle_type is None:
vehicle_type = self.vehicle_type_name
if name not in self.have_vehicle or self.have_vehicle[name] != vehicle_type:
return
del self.have_vehicle[name]
self.map.remove_object(name)

def drawing_update(self):
'''update line drawing'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
Expand Down Expand Up @@ -1062,6 +1071,31 @@ def set_secondary_vehicle_position(self, m):
self.create_vehicle_icon('VehiclePos2', 'blue', follow=False, vehicle_type='plane')
self.map.set_position('VehiclePos2', (lat, lon), rotation=heading, label=agl_s, colour=(0,255,255))

def update_vehicle_icon(self, name, vehicle, colour, m, display):
'''update display of a vehicle on the map. m is expected to store
location in lat/lng *1e7
'''
latlon = (m.lat*1.0e-7, m.lng*1.0e-7)
yaw = math.degrees(m.yaw)
self.update_vehicle_icon_to_loc(name, vehicle, colour, display, latlon, yaw)

def update_vehicle_icon_to_loc(self, name, vehicle, colour, display, latlon, yaw):
'''update display of a vehicle on the map at latlon
'''
key = name + vehicle

# don't display this icon if the settings don't say so:
if not display:
# remove from display if it was being displayed:
self.remove_vehicle_icon(key)
return

# create the icon if we weren't displaying:
self.create_vehicle_icon(key, colour)

# update placement on map:
self.map.set_position(key, latlon, rotation=yaw)

def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
Expand Down Expand Up @@ -1108,34 +1142,24 @@ def mavlink_packet(self, m):
# in the air at the same time
vehicle = 'Vehicle%u' % m.get_srcSystem()

if mtype == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon('Sim' + vehicle, 'green')
self.map.set_position('Sim' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

if mtype == "SIMSTATE":
self.update_vehicle_icon('Sim', vehicle, 'green', m, self.map_settings.showsimpos)
elif mtype == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon('AHRS2' + vehicle, 'purple')
self.map.set_position('AHRS2' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

self.update_vehicle_icon('AHRS2', vehicle, 'purple', m, self.map_settings.showahrs2pos)
elif mtype == "AHRS3" and self.map_settings.showahrs3pos:
self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
self.map.set_position('AHRS3' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos:
self.update_vehicle_icon('AHRS3', vehicle, 'orange', m, self.map_settings.showahrs3pos)
elif mtype == "GPS_RAW_INT":
(lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
if lat != 0 or lon != 0:
if m.vel > 300 or m.get_srcSystem() not in self.lat_lon_heading:
heading = m.cog*0.01
else:
(_,_,heading) = self.lat_lon_heading[m.get_srcSystem()]
self.create_vehicle_icon('GPS' + vehicle, 'blue')
self.map.set_position('GPS' + vehicle, (lat, lon), rotation=heading)

elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos:
self.update_vehicle_icon_to_loc('GPS', vehicle, 'blue', self.map_settings.showgpspos, (lat, lon), heading)
elif mtype == "GPS2_RAW":
(lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS2' + vehicle, 'green')
self.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog*0.01)

self.update_vehicle_icon_to_loc('GPS2', vehicle, 'green', self.map_settings.showgps2pos, (lat, lon), m.cog*0.01)
elif mtype == 'GLOBAL_POSITION_INT':
(lat, lon, heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
self.lat_lon_heading[m.get_srcSystem()] = (lat,lon,heading)
Expand All @@ -1149,6 +1173,8 @@ def mavlink_packet(self, m):
label = None
self.map.set_position('Pos' + vehicle, (lat, lon), rotation=heading, label=label, colour=(255,255,255))
self.map.set_follow_object('Pos' + vehicle, self.message_is_from_primary_vehicle(m))
else:
self.remove_vehicle_icon('Pos' + vehicle)

elif mtype == "HIGH_LATENCY2" and self.map_settings.showahrspos:
(lat, lon) = (m.latitude*1.0e-7, m.longitude*1.0e-7)
Expand Down