Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-26 17:02:20 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

sensors: sensor_preflight_imu -> sensors_status_imu and run continuously

Browse Source 
- inconsistency checks now run continuously instead of only preflight
 - keep inconsistencies for all sensors
 - add per sensor data validator state as overall health flag
This commit is contained in:
Daniel Agar
2020-09-06 22:06:13 -04:00
committed by GitHub
parent d4ecf24bf2
commit 60d613ea04
12 changed files with 165 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/ecl_ekf/analysis/post_processing.py
View File

@@ -146,7 +146,7 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict:
return gps_fail_flags
def magnetic_field_estimates_from_status(estimator_states: dict) -> Tuple[float, float, float]:
def magnetic_field_estimates_from_states(estimator_states: dict) -> Tuple[float, float, float]:
"""
:param estimator_states:

35
Tools/ecl_ekf/plotting/pdf_report.py
View File

@@ -11,7 +11,7 @@ import numpy as np
from matplotlib.backends.backend_pdf import PdfPages
from pyulog import ULog
from analysis.post_processing import magnetic_field_estimates_from_status, get_estimator_check_flags
from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags
from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
CheckFlagsPlot
from analysis.detectors import PreconditionError
@@ -34,6 +34,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
except:
raise PreconditionError('could not find estimator_status data')
try:
estimator_states = ulog.get_dataset('estimator_states').data
print('found estimator_states data')
except:
raise PreconditionError('could not find estimator_states data')
try:
estimator_innovations = ulog.get_dataset('estimator_innovations').data
estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
@@ -45,12 +51,6 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
except:
raise PreconditionError('could not find innovation data')
try:
sensor_preflight_imu = ulog.get_dataset('sensor_preflight_imu').data
print('found sensor_preflight data')
except:
raise PreconditionError('could not find sensor_preflight_imu data')
control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
status_time = 1e-6 * estimator_status['timestamp']
@@ -60,17 +60,6 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
with PdfPages(output_plot_filename) as pdf_pages:
# plot IMU consistency data
if ('accel_inconsistency_m_s_s' in sensor_preflight_imu.keys()) and (
'gyro_inconsistency_rad_s' in sensor_preflight_imu.keys()):
data_plot = TimeSeriesPlot(
sensor_preflight_imu, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
x_labels=['data index', 'data index'],
y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'],
plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# vertical velocity and position innovations
data_plot = InnovationPlot(
innovation_data, [('gps_vpos', 'var_gps_vpos'),
@@ -292,10 +281,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
data_plot.close()
# Plot the earth frame magnetic field estimates
declination, field_strength, inclination = magnetic_field_estimates_from_status(
estimator_status)
declination, field_strength, inclination = magnetic_field_estimates_from_states(
estimator_states)
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'],
1e-6 * estimator_states['timestamp'],
{'strength': field_strength, 'declination': declination, 'inclination': inclination},
[['declination'], ['inclination'], ['strength']],
x_label='time (sec)', y_labels=['declination (deg)', 'inclination (deg)',
@@ -307,7 +296,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# Plot the body frame magnetic field estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'], estimator_status,
1e-6 * estimator_states['timestamp'], estimator_states,
[['states[19]'], ['states[20]'], ['states[21]']],
x_label='time (sec)', y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'],
plot_title='Magnetometer Bias Estimates', annotate=False, pdf_handle=pdf_pages)
@@ -316,7 +305,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# Plot the EKF wind estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'], estimator_status,
1e-6 * estimator_states['timestamp'], estimator_states,
[['states[22]'], ['states[23]']], x_label='time (sec)',
y_labels=['North (m/s)', 'East (m/s)'], plot_title='Wind Velocity Estimates',
annotate=False, pdf_handle=pdf_pages)