class SensorFusionMaster(LoggingStateProviderWithListeners): """ This class is responsible for taking the single sensor fusion results and integrating them into a VehicleState. - AttitudeProvider (gyro, accelerometer, magnetometer) -> AttitudeState (orientation in air) - HeightProvider (gps, ultrasonic sensor, barometer, attitude) -> HeightState (height above ground, vertical speed) """ def __init__(self): self.attitudeProvider = AttitudeProvider() self.heightProvider = HeightProvider() def update(self): # this will trigger the heightProvider via the listener attitude = self.attitudeProvider.update() vehicle_state = self.heightProvider.update(attitude) return vehicle_state
def test_zero(self): mock_listener = MockListener(self) invalid_counter = InvalidCounterListener() height_provider = HeightProvider() attitude_state = AttitudeState(imu_reading) # Some warmups for x in range(10): time.sleep(0.001) height_provider.update(attitude_state) print("Warmup is done. Now the pressure sensor should be ready...") height_provider.registerListener(mock_listener) height_provider.registerListener(invalid_counter) # give the sensor time to collect values time.sleep(0.05) height_provider.update(attitude_state) self.assertEqual(mock_listener.numUpdates, 1) self.assertGreater(mock_listener.sumDeltas, 0.05) # give the sensor time to collect values time.sleep(0.05) height_provider.update(attitude_state) self.assertEqual(mock_listener.numUpdates, 2) self.assertGreater(mock_listener.sumDeltas, 0.1) # check that all data is present self.assertEqual(invalid_counter.invalidGpsGroundHeightCounter, 0) self.assertEqual(invalid_counter.invalidBarometerGroundHeightCounter, 0) self.assertEqual(invalid_counter.pressureInvalidCounter, 0) self.assertEqual(invalid_counter.temperatureInvalidCounter, 0) self.assertEqual(invalid_counter.invalidElevationSpeedCounter, 0) self.assertEqual(invalid_counter.invalidElevationCounter, 0) height_provider.stop()
def __init__(self): self.attitudeProvider = AttitudeProvider() self.heightProvider = HeightProvider()