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()
