def __init__(
self, ulog: ULog, innov_flags: Dict[str, float], control_mode_flags: Dict[str, float],
check_type: CheckType = CheckType.UNDEFINED, check_id: str = '',
test_ratio_name: Optional[str] = '', innov_fail_names: Optional[List[str]] = None):
"""
:param ulog:
:param innov_flags:
:param check_type:
:param check_id:
:param test_ratio_name:
:param innov_fail_names:
"""
super(EstimatorCheck, self).__init__(
ulog, check_type=check_type)
self._innov_flags = innov_flags
self._control_mode_flags = control_mode_flags
self._check_id = check_id
self._test_ratio_name = test_ratio_name
self._innov_fail_names = innov_fail_names
if self._innov_fail_names is None:
self._innov_fail_names = list()
self._in_air_detector_no_ground_effects = InAirDetector(
ulog, min_flight_time_seconds=params.iad_min_flight_duration_seconds(),
in_air_margin_seconds=params.iad_in_air_margin_seconds())
if check_id in ['magnetometer', 'height', 'yaw', 'optical_flow']:
self._in_air_detector = self._in_air_detector_no_ground_effects
else:
self._in_air_detector = InAirDetector(
ulog, min_flight_time_seconds=params.iad_min_flight_duration_seconds())
def __init__(self, ulog: ULog):
"""
:param ulog:
"""
super().__init__(ulog, check_type=CheckType.IMU_VIBRATION_STATUS)
self._in_air_detector_no_ground_effects = InAirDetector(
ulog,
min_flight_time_seconds=params.iad_min_flight_duration_seconds(),
in_air_margin_seconds=params.iad_in_air_margin_seconds(),
)
def multiple_take_offs(ulog):
"""
tests whether multiple take offs can be handled by the in air detector.
:param ulog:
:param ts_length:
:return:
"""
ts_length = ulog.get_dataset(
'vehicle_land_detected').data['landed'].shape[0]
ulog.get_dataset('vehicle_land_detected').data['landed'] = np.zeros(
ts_length, dtype=int)
ulog.get_dataset('vehicle_land_detected').data['landed'][int(ts_length /
2)] = 1
ulog.get_dataset('vehicle_land_detected').data['landed'][int(ts_length /
2 + 2)] = 1
ulog.get_dataset('vehicle_land_detected').data['landed'][int(
3 * ts_length / 4)] = 1
ulog.get_dataset('vehicle_land_detected').data['landed'][int(ts_length /
4)] = 1
in_air_detector = InAirDetector(ulog,
min_flight_time_seconds=0.0,
in_air_margin_seconds=0.0)
airtimes_no_margin = in_air_detector.airtimes
assert len(airtimes_no_margin) == 5
in_air_detector = InAirDetector(ulog,
min_flight_time_seconds=2.0,
in_air_margin_seconds=2.0)
airtimes_margin = in_air_detector.airtimes
margin_difference_take_off = airtimes_margin[
0].take_off - airtimes_no_margin[0].take_off # 2
margin_difference_landing = airtimes_no_margin[
0].landing - airtimes_margin[0].landing # 2
assert len(airtimes_margin) == 4
assert 1.99 < margin_difference_take_off < 2.01, \
'margin is not applied correctly'
assert 1.99 < margin_difference_landing < 2.01, \
'margin is not applied correctly'
def calculate_stat_from_signal(
data: Dict[str, np.ndarray], dataset: str, variable: str,
in_air_det: InAirDetector, stat_function: Callable) -> float:
"""
:param data:
:param variable:
:param in_air_detector:
:return:
"""
return float(stat_function(data[variable][in_air_det.get_airtime(dataset)]))
def __init__(self, ulog: ULog):
"""
:param ulog:
"""
super(IMU_Output_Predictor_Check,
self).__init__(ulog,
check_type=CheckType.IMU_OUTPUT_PREDICTOR_STATUS)
self._in_air_detector_no_ground_effects = InAirDetector(
ulog,
min_flight_time_seconds=params.iad_min_flight_duration_seconds(),
in_air_margin_seconds=params.iad_in_air_margin_seconds())
def __init__(self, ulog: ULog):
"""
:param ulog:
"""
super().__init__(ulog, check_type=CheckType.IMU_BIAS_STATUS)
self._in_air_detector_no_ground_effects = InAirDetector(
ulog,
min_flight_time_seconds=params.iad_min_flight_duration_seconds(),
in_air_margin_seconds=params.iad_in_air_margin_seconds(),
)
messages = {elem.name for elem in self.ulog.data_list}
self._estimator_states_msg = ("estimator_states" if "estimator_states"
in messages else "estimator_status")
def original_take_offs(ulog):
"""
:param ulog:
:return:
"""
in_air_detector = InAirDetector(ulog,
min_flight_time_seconds=0.0,
in_air_margin_seconds=0.0)
airtimes = in_air_detector.airtimes
assert len(airtimes) == 1
assert 3.7 < airtimes[0].take_off < 3.8 # 3.797098
assert 403.9 < airtimes[-1].landing < 404.0 # 403.945201
def take_off_at_second_time_stamp(ulog):
"""
tests whether
:param ulog:
:param ts_length:
:return:
"""
ts_length = ulog.get_dataset(
'vehicle_land_detected').data['landed'].shape[0]
ulog.get_dataset('vehicle_land_detected').data['landed'] = np.zeros(
ts_length, dtype=int)
ulog.get_dataset('vehicle_land_detected').data['landed'][0] = 1
in_air_detector = InAirDetector(ulog,
min_flight_time_seconds=0.0,
in_air_margin_seconds=0.0)
airtimes = in_air_detector.airtimes
assert len(airtimes) == 1
def always_on_ground(ulog):
"""
:param ulog:
:return:
"""
ts_length = ulog.get_dataset(
'vehicle_land_detected').data['landed'].shape[0]
ulog.get_dataset('vehicle_land_detected').data['landed'] = np.ones(
ts_length, dtype=int)
in_air_detector = InAirDetector(ulog,
min_flight_time_seconds=0.0,
in_air_margin_seconds=0.0)
first_take_off = in_air_detector.take_off
airtimes = in_air_detector.airtimes
assert first_take_off is None, 'The first take_off is not None.'
assert len(airtimes) == 0
def always_in_air(ulog):
"""
:param ulog:
:return:
"""
ts_length = ulog.get_dataset(
'vehicle_land_detected').data['landed'].shape[0]
ulog.get_dataset('vehicle_land_detected').data['landed'] = np.zeros(
ts_length, dtype=int)
in_air_detector = InAirDetector(ulog,
min_flight_time_seconds=0.0,
in_air_margin_seconds=0.0)
airtimes = in_air_detector.airtimes
assert len(airtimes) == 1
assert airtimes[
0].take_off <= 0.0 # for this log the landed timestamp happends to be
# earlier than the start timestamp
assert 405.9 < airtimes[0].landing <= 406.0 # 405.969268
def calculate_windowed_mean_per_airphase(
data: Dict[str, np.ndarray], dataset: str, variable: str,
in_air_det: InAirDetector, threshold: Optional[float] = None,
window_len_s: float = 30.0) -> List[Tuple[Airtime, np.ndarray]]:
"""
calculates a windowed mean for a thresholded signal. This is needs to be done per airphase,
as otherwise ground effects might influcence the signal.
:param data:
:param variable:
:param in_air_detector:
:param stat_function:
:param window_len_s:
:return:
"""
windowed_stats = []
for airtime, at_indices in zip(in_air_det.airtimes, in_air_det.get_airtime_per_phase(dataset)):
duration_s = airtime.landing - airtime.take_off
window_len = int((window_len_s / duration_s) * len(at_indices))
if (window_len % 2) == 0:
window_len += 1
if len(at_indices) > 0:
window_len_after_s = duration_s * (window_len / float(len(at_indices)))
input_signal = data[variable][at_indices]
if threshold is not None:
input_signal = 100.0 * (input_signal > threshold)
smoothed_air_phase = smooth_1d_boundaries(
input_signal, window_len=window_len, mode='valid', mean_for_short_signals=True)
smoothed_airtime = Airtime(
take_off=airtime.take_off + min(window_len_after_s, duration_s) / 2.0,
landing=airtime.landing - min(window_len_after_s, duration_s) / 2.0)
windowed_stats.append((smoothed_airtime, smoothed_air_phase))
return windowed_stats