diff --git a/app/plot_app/configured_plots.py b/app/plot_app/configured_plots.py index a7f2f02f1..9ecdb4030 100644 --- a/app/plot_app/configured_plots.py +++ b/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') diff --git a/app/plot_app/helper.py b/app/plot_app/helper.py index 42df02e63..fddd5b065 100644 --- a/app/plot_app/helper.py +++ b/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. diff --git a/app/plot_app/leaflet.py b/app/plot_app/leaflet.py index 58ab82397..3a94ff4b6 100644 --- a/app/plot_app/leaflet.py +++ b/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 diff --git a/app/plot_app/overview_generator.py b/app/plot_app/overview_generator.py index 4c585c04d..41034e39e 100644 --- a/app/plot_app/overview_generator.py +++ b/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) diff --git a/app/plot_app/plotting.py b/app/plot_app/plotting.py index 73394f5cb..186b77a48 100644 --- a/app/plot_app/plotting.py +++ b/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'] diff --git a/app/tornado_handlers/three_d.py b/app/tornado_handlers/three_d.py index 7cd3f4905..5fe414464 100644 --- a/app/tornado_handlers/three_d.py +++ b/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())