def _Filter(self, telemetry, *args):
stoplight = stoplights.MostSevereStoplight(
stoplights.SetByLimits(telemetry.estimator.gsg_bias.azi,
self.gsg_bias_azi_limits),
stoplights.SetByLimits(telemetry.estimator.gsg_bias.ele,
self.gsg_bias_ele_limits))
message = 'azi: {azi: 3.0f} deg ele: {ele: 3.0f} deg'.format(
azi=(180.0 / numpy.pi) * telemetry.estimator.gsg_bias.azi,
ele=(180.0 / numpy.pi) * telemetry.estimator.gsg_bias.ele)
return message, stoplight
def _Filter(self, *args):
gyro_biases = args[0].estimator.gyro_biases
gyro_bias_max = []
for i in range(labels.kNumWingImus):
gyro_bias_max.append(max(abs(gyro_biases[i].x), abs(gyro_biases[i].y),
abs(gyro_biases[i].z)))
stoplight = stoplights.SetByLimits(max(gyro_bias_max), self._limits)
return self._GetTimestamps(*args), gyro_bias_max, stoplight
def _Filter(self, telemetry, *args):
delta_norm = _Vec3Distance(
telemetry.state_est.Xg, telemetry.estimator.glas.Xg)
valid = telemetry.estimator.glas.wing_pos_valid
message = 'pos: {:6.2f} m'.format((delta_norm))
stoplight = stoplights.SetByLimits(delta_norm, self._limits)
return ((message, stoplight) if valid
else ('--', stoplights.STOPLIGHT_UNAVAILABLE))
def _Filter(self, telemetry):
gyro_bias_max = []
gyro_biases = telemetry.estimator.gyro_biases
for i in range(labels.kNumWingImus):
gyro_bias_max.append(max(abs(gyro_biases[i].x), abs(gyro_biases[i].y),
abs(gyro_biases[i].z)))
stoplight = stoplights.SetByLimits(max(gyro_bias_max), self._limits)
text = 'A: %6.4f B: %6.4f C: %6.4f' % (
gyro_bias_max[0], gyro_bias_max[1], gyro_bias_max[2])
return text, stoplight
def _Filter(self, telemetry):
gyro_bias_crosswind_drift_max = [0.0, 0.0, 0.0]
gyro_biases = telemetry.estimator.gyro_biases
is_crosswind = control_common.AnyCrosswindFlightMode(telemetry.flight_mode)
if is_crosswind:
for i in range(labels.kNumWingImus):
gyro_bias_crosswind_drift_max[0] = numpy.max((
gyro_bias_crosswind_drift_max[0],
abs(gyro_biases[i].x - self._hover_gyro_biases[i][0])))
gyro_bias_crosswind_drift_max[1] = numpy.max((
gyro_bias_crosswind_drift_max[1],
abs(gyro_biases[i].y - self._hover_gyro_biases[i][1])))
gyro_bias_crosswind_drift_max[2] = numpy.max((
gyro_bias_crosswind_drift_max[2],
abs(gyro_biases[i].z - self._hover_gyro_biases[i][2])))
# TODO(b/77811299): The limit is currently only applied to the
# X-axis gyros, as reasonable limits for the Y and Z axes are
# not yet known. If we keep this indicator around, limits for
# the Y and Z axes should be added.
if numpy.any(numpy.isnan(gyro_bias_crosswind_drift_max)):
stoplight = stoplights.STOPLIGHT_UNAVAILABLE
else:
stoplight = stoplights.SetByLimits(
gyro_bias_crosswind_drift_max[0], self._crosswind_drift_limits)
text = 'X: %6.4f Y: %6.4f Z: %6.4f' % (
gyro_bias_crosswind_drift_max[0], gyro_bias_crosswind_drift_max[1],
gyro_bias_crosswind_drift_max[2])
else:
for i in range(labels.kNumWingImus):
self._hover_gyro_biases[i, 0] = gyro_biases[i].x
self._hover_gyro_biases[i, 1] = gyro_biases[i].y
self._hover_gyro_biases[i, 2] = gyro_biases[i].z
stoplight = stoplights.STOPLIGHT_ANY
text = 'X: -- Y: -- Z: -- '
return text, stoplight