def GetTimeSeries(self, params, sim, control):
time, tether_elevation, tether_elevation_valid = self._SelectTelemetry(
sim, control, ['time', 'tether_elevation', 'tether_elevation_valid'])
if not (scoring_util.IsSelectionValid(tether_elevation) or
scoring_util.IsSelectionValid(time)):
return {'tether_elevation_std': np.array([float('nan')])}
tether_elevation_deg = np.rad2deg(
tether_elevation[tether_elevation_valid == 1])
# Low pass, symmetrically (2nd order) filter the tether elevation.
tether_elevation_deg_f = scoring_util.LpFiltFiltTimeSeries(
time, tether_elevation_deg, self._cut_off_freq)
# Calculate a rolling StDev of the difference between the raw signal and
# the filtered signal.
t_samp = scoring_util.GetTimeSamp(time)
if np.isnan(t_samp):
return {'tether_elevation_std': np.array([float('nan')])}
n_window = int(self._t_window / t_samp)
elevation_deviation_df = pd.DataFrame(
tether_elevation_deg - tether_elevation_deg_f)
tether_elevation_std = elevation_deviation_df.rolling(
n_window, min_periods=n_window).std().values.flatten()
return {'tether_elevation_std': tether_elevation_std}
def GetTimeSeries(self, params, sim, control):
time, tether_pitch = self._SelectTelemetry(sim, control,
['time', 'tether_pitch'])
if (not scoring_util.IsSelectionValid(time)
or not scoring_util.IsSelectionValid(tether_pitch)):
return {
'min_sustained_tether_pitch': float('nan'),
'max_sustained_tether_pitch': float('nan')
}
tether_pitch_deg = np.rad2deg(tether_pitch)
# Low pass, symmetrically (2nd order) filter the tether pitch.
cut_off_freq = 2.0
tether_pitch_deg_f = scoring_util.LpFiltFiltTimeSeries(
time, tether_pitch_deg, cut_off_freq)
min_sustained_tether_pitch, max_sustained_tether_pitch = (
scoring_util.GetSustainedValue(tether_pitch_deg_f,
self._good_lower_limit,
self._good_upper_limit,
self._sustained_duration,
scoring_util.GetTimeSamp(time)))
return {
'min_sustained_tether_pitch': min_sustained_tether_pitch,
'max_sustained_tether_pitch': max_sustained_tether_pitch
}
def GetTimeSeries(self, params, sim, control):
time = self._SelectTelemetry(sim, control, 'time')
if scoring_util.IsSelectionValid(time):
t_samp = scoring_util.GetTimeSamp(time)
return {'sample_time_error': np.abs(np.diff(time) - t_samp)}
else:
return {'sample_time_error': np.array([float('nan')])}
def GetTimeSeries(self, params, sim, control):
# The detwist command is in [0, TETHER_DETWIST_REVS * 2 * pi).
time, gs_detwist_cmd = self._SelectTelemetry(
sim, control, ['time', 'gs_detwist_cmd'])
dt = scoring_util.GetTimeSamp(time)
if np.isnan(dt):
return {'gs_detwist_cmd_rate': np.array([float('nan')])}
gs_detwist_cmd_diff = diff_wrap_angle(
pack_avionics_messages.TETHER_DETWIST_REVS, gs_detwist_cmd)
gs_detwist_cmd_deriv = gs_detwist_cmd_diff / dt
return {'gs_detwist_cmd_rate': np.rad2deg(gs_detwist_cmd_deriv)}
def GetTimeSeries(self, params, sim, control):
hover_params = params['control_params']['hover']
thrust_moment, thrust_moment_avail, control_time = self._SelectTelemetry(
sim, control, ['thrust_moment', 'thrust_moment_avail', 'time'])
if (not scoring_util.IsSelectionValid(thrust_moment)
or not scoring_util.IsSelectionValid(control_time)):
return {'max_saturation_duration': float('nan')}
if self._axis == 'thrust':
cmd = thrust_moment['thrust']
cmd_avail = thrust_moment_avail['thrust']
min_software_limit = float('-infinity')
max_software_limit = hover_params['altitude']['max_thrust']
elif self._axis == 'moment_y':
cmd = thrust_moment['moment']['y']
cmd_avail = thrust_moment_avail['moment']['y']
min_software_limit = hover_params['angles']['min_moment']['y']
max_software_limit = hover_params['angles']['max_moment']['y']
elif self._axis == 'moment_z':
cmd = thrust_moment['moment']['z']
cmd_avail = thrust_moment_avail['moment']['z']
min_software_limit = hover_params['angles']['min_moment']['z']
max_software_limit = hover_params['angles']['max_moment']['z']
else:
assert False, 'The axis %s is not supported.' % self._axis
t_samp = scoring_util.GetTimeSamp(control_time)
if np.isnan(t_samp):
return {'max_saturation_duration': np.array([float('nan')])}
saturation_mask = np.argwhere(
np.logical_or(
np.abs(cmd - cmd_avail) > 1e-2,
np.logical_or(
np.abs(cmd - min_software_limit) < 1e-2,
np.abs(cmd - max_software_limit) < 1e-2)))
if saturation_mask.size > 0:
saturated_intervals = scoring_util.GetIntervals(saturation_mask)
max_saturation_size = np.max([
interval[1] - interval[0] for interval in saturated_intervals
])
else:
max_saturation_size = 0.0
return {'max_saturation_duration': max_saturation_size * t_samp}
def GetTimeSeries(self, params, sim, control):
# TODO: Use the Gs02TransformStageFilter.
if self._transform_stages:
gs02_mode, gs02_transform_stage, tether_elevation = (
self._SelectTelemetry(
sim, control, ['gs02_mode', 'gs02_transform_stage',
'tether_elevation']))
if (not scoring_util.IsSelectionValid(gs02_mode) or
not scoring_util.IsSelectionValid(gs02_transform_stage) or
not scoring_util.IsSelectionValid(tether_elevation)):
return {
'gs02_mode': None,
'gs02_transform_stage': None,
'tether_elevation': None
}
else:
tether_elevation_deg = np.rad2deg(tether_elevation)
return {
'gs02_mode': gs02_mode,
'gs02_transform_stage': gs02_transform_stage,
'tether_elevation': tether_elevation_deg
}
else:
time, tether_elevation = self._SelectTelemetry(
sim, control, ['time', 'tether_elevation'])
if not (scoring_util.IsSelectionValid(tether_elevation) or
scoring_util.IsSelectionValid(time)):
return {
'tether_elevation': None
}
tether_elevation_deg = np.rad2deg(tether_elevation)
# Low pass, symmetrically (2nd order) filter the tether elevation.
cut_off_freq = 0.4
tether_elevation_deg_f = scoring_util.LpFiltFiltTimeSeries(
time, tether_elevation_deg, cut_off_freq)
self._t_samp = scoring_util.GetTimeSamp(time)
# Return filtered data.
return {
'tether_elevation': tether_elevation_deg_f
}
def GetTimeSeries(self, params, sim, control):
time, gs_detwist_pos, loop_angle = self._SelectTelemetry(
sim, control, ['time', 'gs_detwist_pos', 'loop_angle'])
accum_loops = np.cumsum(np.diff(loop_angle) > np.deg2rad(350.))
dt = scoring_util.GetTimeSamp(time)
if np.isnan(dt):
return {'gs_detwist_ratios_per_loop': []}
# Obtain the derivative of the detwist angle to remove the steadily
# decreasing ramp and the jump at every revolution. The frequency content
# is preserved.
# Wrapping first the detwist position to [0, 2*pi).
gs_detwist_pos_diff = diff_wrap_angle(
pack_avionics_messages.TETHER_DETWIST_REVS, gs_detwist_pos)
gs_detwist_pos_deriv = gs_detwist_pos_diff / dt
# Obtain the number of points in the FFT based on the desired frequency
# resolution and the sampling time: frequency_resolution ~= fs/nfft Hz.
frequency_resolution = 0.1 # [Hz]
nfft = 2.0**np.ceil(np.log2(1.0 / dt / frequency_resolution))
# Iterate through all loops.
num_loops = np.floor(accum_loops[-1])
if np.isnan(num_loops):
num_loops = 0
gs_detwist_ratios_per_loop = []
for loop in range(1, int(num_loops) + 1):
detwist_deriv_this_loop = gs_detwist_pos_deriv[np.where(
np.floor(accum_loops).astype(int) == loop)]
f_this_loop, pxx_this_loop = periodogram(detwist_deriv_this_loop,
fs=1.0 / dt,
nfft=int(nfft),
scaling='spectrum',
detrend=False)
# Find the peaks in the spectrum.
# Peaks are where the derivative changes sign and the second derivative
# is negative.
pxx_diff_this_loop = np.diff(10.0 * np.log10(pxx_this_loop))
pxx_diff_sign_change_this_loop = (pxx_diff_this_loop[1:] *
pxx_diff_this_loop[0:-1])
pxx_diff_diff_this_loop = np.diff(pxx_diff_this_loop)
min_freq = 0.25 # [Hz]
pxx_peaks_idx_this_loop = np.where(
(pxx_diff_sign_change_this_loop < 0.)
& (pxx_diff_diff_this_loop < 0.)
& (f_this_loop[0:-2] > min_freq))[0] + 1
if pxx_peaks_idx_this_loop.size == 0:
# No peaks were found.
continue
# Get the highest secondary lobe.
max_peak_this_loop_db = 10.0 * np.log10(
np.max(pxx_this_loop[pxx_peaks_idx_this_loop]))
max_ratio_this_loop_db = (max_peak_this_loop_db -
10.0 * np.log10(pxx_this_loop[0]))
f_secondary_this_loop = f_this_loop[pxx_peaks_idx_this_loop[
np.argmax(pxx_this_loop[pxx_peaks_idx_this_loop])]]
# Store tuple (loop number, lobe frequency [Hz], lobe power ratio [dB]).
gs_detwist_ratios_per_loop.append(
(loop + 1, f_secondary_this_loop, max_ratio_this_loop_db))
return {'gs_detwist_ratios_per_loop': gs_detwist_ratios_per_loop}