plot_app: update vehicle_gps_position message

PX4 change: PX4/PX4-Autopilot#21678

app/plot_app/configured_plots.py

@@ -40,6 +40,7 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page,
         magnetometer_ga_topic = 'sensor_combined'
     manual_control_sp_controls = ['roll', 'pitch', 'yaw', 'throttle']
     manual_control_sp_throttle_range = '[-1, 1]'
+    vehicle_gps_position_altitude = None
     for topic in data:
         if topic.name == 'system_power':
             # COMPATIBILITY: rename fields to new format
@@ -49,13 +50,18 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page,
                 topic.data['sensors3v3[0]'] = topic.data.pop('voltage3V3_v')
             if 'voltage3v3_v' in topic.data:
                 topic.data['sensors3v3[0]'] = topic.data.pop('voltage3v3_v')
-        if topic.name == 'tecs_status':
+        elif topic.name == 'tecs_status':
             if 'airspeed_sp' in topic.data: # old (prior to PX4-Autopilot/pull/16585)
                 topic.data['true_airspeed_sp'] = topic.data.pop('airspeed_sp')
-        if topic.name == 'manual_control_setpoint':
+        elif topic.name == 'manual_control_setpoint':
             if 'throttle' not in topic.data: # old (prior to PX4-Autopilot/pull/15949)
                 manual_control_sp_controls = ['y', 'x', 'r', 'z']
                 manual_control_sp_throttle_range = '[0, 1]'
+        elif topic.name == 'vehicle_gps_position':
+            if ulog.msg_info_dict.get('ver_data_format', 0) >= 2:
+                vehicle_gps_position_altitude = topic.data['altitude_msl_m']
+            else: # COMPATIBILITY
+                vehicle_gps_position_altitude = topic.data['alt'] * 0.001
 
     if any(elem.name == 'vehicle_angular_velocity' for elem in data):
         rate_estimated_topic_name = 'vehicle_angular_velocity'
@@ -180,8 +186,8 @@ def generate_plots(ulog, px4_ulog, db_data, vehicle_data, link_to_3d_page,
     data_plot = DataPlot(data, plot_config, 'vehicle_gps_position',
                          y_axis_label='[m]', title='Altitude Estimate',
                          changed_params=changed_params, x_range=x_range)
-    data_plot.add_graph([lambda data: ('alt', data['alt']*0.001)],
-                        colors8[0:1], ['GPS Altitude'])
+    data_plot.add_graph([lambda data: ('alt', vehicle_gps_position_altitude)],
+                        colors8[0:1], ['GPS Altitude (MSL)'])
     data_plot.change_dataset(baro_alt_meter_topic)
     data_plot.add_graph(['baro_alt_meter'], colors8[1:2], ['Barometer Altitude'])
     data_plot.change_dataset('vehicle_global_position')

app/plot_app/helper.py

@@ -422,6 +422,21 @@ def thrust_z_neg(self):
         return self._thrust_z_neg
 
 
+def get_lat_lon_alt_deg(ulog: ULog, vehicle_gps_position_dataset: ULog.Data):
+    """
+    Get (lat, lon, alt) tuple in degrees and altitude in meters
+    """
+    if ulog.msg_info_dict.get('ver_data_format', 0) >= 2:
+        lat = vehicle_gps_position_dataset.data['latitude_deg']
+        lon = vehicle_gps_position_dataset.data['longitude_deg']
+        alt = vehicle_gps_position_dataset.data['altitude_msl_m']
+    else: # COMPATIBILITY
+        lat = vehicle_gps_position_dataset.data['lat'] / 1e7
+        lon = vehicle_gps_position_dataset.data['lon'] / 1e7
+        alt = vehicle_gps_position_dataset.data['alt'] / 1e3
+    return lat, lon, alt
+
+
 def get_airframe_name(ulog, multi_line=False):
     """
     get the airframe name and autostart ID.

app/plot_app/leaflet.py

@@ -3,6 +3,7 @@
 
 from colors import HTML_color_to_RGB
 from config_tables import flight_modes_table
+from helper import get_lat_lon_alt_deg
 
 #pylint: disable=consider-using-enumerate
 
@@ -23,25 +24,15 @@ def rgb_colors(flight_mode):
 
         return "#" + "".join(map(lambda x: format(x, '02x'), rgb))
     cur_data = ulog.get_dataset('vehicle_gps_position')
-    pos_lon = cur_data.data['lon']
-    pos_lat = cur_data.data['lat']
-    pos_alt = cur_data.data['alt']
+    pos_lat, pos_lon, _ = get_lat_lon_alt_deg(ulog, cur_data)
     pos_t = cur_data.data['timestamp']
 
     if 'fix_type' in cur_data.data:
         indices = cur_data.data['fix_type'] > 2  # use only data with a fix
         pos_lon = pos_lon[indices]
         pos_lat = pos_lat[indices]
-        pos_alt = pos_alt[indices]
         pos_t = pos_t[indices]
 
-    # scale if it's an integer type
-    lon_type = [f.type_str for f in cur_data.field_data if f.field_name == 'lon']
-    if len(lon_type) > 0 and lon_type[0] == 'int32_t':
-        pos_lon = pos_lon / 1e7  # to degrees
-        pos_lat = pos_lat / 1e7
-        pos_alt = pos_alt / 1e3  # to meters
-
     pos_datas = []
     flight_modes = []
     last_t = 0

app/plot_app/overview_generator.py

@@ -10,7 +10,7 @@
 import matplotlib.pyplot as plt
 
 from config import get_log_filepath, get_overview_img_filepath
-from helper import load_ulog_file
+from helper import load_ulog_file, get_lat_lon_alt_deg
 
 MAXTILES = 16
 def get_zoom(input_box, z=18):
@@ -41,10 +41,10 @@ def generate_overview_img(ulog, log_id):
 
     try:
         cur_dataset = ulog.get_dataset('vehicle_gps_position')
-        t = cur_dataset.data['timestamp']
         indices = cur_dataset.data['fix_type'] > 2 # use only data with a fix
-        lon = cur_dataset.data['lon'][indices] / 1e7 # degrees
-        lat = cur_dataset.data['lat'][indices] / 1e7
+        lat, lon, _ = get_lat_lon_alt_deg(ulog, cur_dataset)
+        lat = lat[indices]
+        lon = lon[indices]
 
         min_lat = min(lat)
         max_lat = max(lat)

app/plot_app/plotting.py

@@ -22,7 +22,7 @@
 
 from downsampling import DynamicDownsample
 from helper import (
-    map_projection, WGS84_to_mercator, flight_modes_table, vtol_modes_table
+    map_projection, WGS84_to_mercator, flight_modes_table, vtol_modes_table, get_lat_lon_alt_deg
     )
 
 
@@ -258,9 +258,7 @@ def plot_map(ulog, config, map_type='plain', api_key=None, setpoints=False,
         t = cur_dataset.data['timestamp']
         indices = cur_dataset.data['fix_type'] > 2 # use only data with a fix
         t = t[indices]
-        lon = cur_dataset.data['lon'][indices] / 1e7 # degrees
-        lat = cur_dataset.data['lat'][indices] / 1e7
-        altitude = cur_dataset.data['alt'][indices] / 1e3 # meters
+        lat, lon, _ = get_lat_lon_alt_deg(ulog, cur_dataset)
 
         plots_width = config['plot_width']
         plots_height = config['plot_height']['large']

app/tornado_handlers/three_d.py

@@ -12,7 +12,7 @@
 sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../plot_app'))
 from config import get_bing_maps_api_key, get_cesium_api_key
 from helper import validate_log_id, get_log_filename, load_ulog_file, \
-    get_flight_mode_changes, flight_modes_table
+    get_flight_mode_changes, flight_modes_table, get_lat_lon_alt_deg
 
 #pylint: disable=relative-beyond-top-level
 from .common import get_jinja_env, CustomHTTPError, TornadoRequestHandlerBase
@@ -38,7 +38,7 @@ def get(self, *args, **kwargs):
 
         try:
             # required topics: none of these are optional
-            gps_pos = ulog.get_dataset('vehicle_gps_position').data
+            gps_pos = ulog.get_dataset('vehicle_gps_position')
             attitude = ulog.get_dataset('vehicle_attitude').data
         except (KeyError, IndexError, ValueError) as error:
             raise CustomHTTPError(
@@ -54,22 +54,22 @@ def get(self, *args, **kwargs):
         except (KeyError, IndexError, ValueError) as error:
             pass
 
+        lat, lon, alt = get_lat_lon_alt_deg(ulog, gps_pos)
 
         # Get the takeoff location. We use the first position with a valid fix,
         # and assume that the vehicle is not in the air already at that point
         takeoff_index = 0
-        gps_indices = np.nonzero(gps_pos['fix_type'] > 2)
+        gps_indices = np.nonzero(gps_pos.data['fix_type'] > 2)
         if len(gps_indices[0]) > 0:
             takeoff_index = gps_indices[0][0]
-        takeoff_altitude = '{:.3f}' \
-            .format(gps_pos['alt'][takeoff_index] * 1.e-3)
-        takeoff_latitude = '{:.10f}'.format(gps_pos['lat'][takeoff_index] * 1.e-7)
-        takeoff_longitude = '{:.10f}'.format(gps_pos['lon'][takeoff_index] * 1.e-7)
+        takeoff_altitude = '{:.3f}' .format(alt[takeoff_index])
+        takeoff_latitude = '{:.10f}'.format(lat[takeoff_index])
+        takeoff_longitude = '{:.10f}'.format(lon[takeoff_index])
 
 
         # calculate UTC time offset (assume there's no drift over the entire log)
-        utc_offset = int(gps_pos['time_utc_usec'][takeoff_index]) - \
-                int(gps_pos['timestamp'][takeoff_index])
+        utc_offset = int(gps_pos.data['time_utc_usec'][takeoff_index]) - \
+                int(gps_pos.data['timestamp'][takeoff_index])
 
         # flight modes
         flight_mode_changes = get_flight_mode_changes(ulog)
@@ -110,7 +110,7 @@ def get(self, *args, **kwargs):
 
 
         # position
-        # Note: alt_ellipsoid from gps_pos would be the better match for
+        # Note: altitude_ellipsoid_m from gps_pos would be the better match for
         # altitude, but it's not always available. And since we add an offset
         # (to match the takeoff location with the ground altitude) it does not
         # matter as much.
@@ -119,18 +119,15 @@ def get(self, *args, **kwargs):
         # - altitude requires an offset (to match the GPS data)
         # - it's worse for some logs where the estimation is bad -> acro flights
         #   (-> add both: user-selectable between GPS & estimated trajectory?)
-        for i in range(len(gps_pos['timestamp'])):
-            lon = gps_pos['lon'][i] * 1.e-7
-            lat = gps_pos['lat'][i] * 1.e-7
-            alt = gps_pos['alt'][i] * 1.e-3
-            t = gps_pos['timestamp'][i] + utc_offset
+        for i in range(len(gps_pos.data['timestamp'])):
+            t = gps_pos.data['timestamp'][i] + utc_offset
             utctimestamp = datetime.datetime.utcfromtimestamp(t/1.e6).replace(
                 tzinfo=datetime.timezone.utc)
             if i == 0:
                 start_timestamp = utctimestamp
             end_timestamp = utctimestamp
             position_data += '["{:}", {:.10f}, {:.10f}, {:.3f}], ' \
-                .format(utctimestamp.isoformat(), lon, lat, alt)
+                .format(utctimestamp.isoformat(), lon[i], lat[i], alt[i])
         position_data += ' ]'
 
         start_timestamp_str = '"{:}"'.format(start_timestamp.isoformat())