def __init__(self):
        self.log = logging.getLogger("StateProvider")
        self.__listeners = []

        self.log.info("Using settings file " + RT_IMU_LIB_SETTINGS_FILE + ".ini")
        if not os.path.exists(RT_IMU_LIB_SETTINGS_FILE + ".ini"):
            self.log.info("Settings file does not exist, will be created")

        self.settings = RTIMU.Settings(RT_IMU_LIB_SETTINGS_FILE)
        self.imu = RTIMU.RTIMU(self.settings)
        self.pressure = RTIMU.RTPressure(self.settings)

        self.log.info("IMU Name: " + self.imu.IMUName())
        self.log.info("Pressure Name: " + self.pressure.pressureName())

        if not self.imu.IMUInit():
            self.log.critical("IMU Init Failed. Exiting...")
            sys.exit(1)
        else:
            self.log.info("IMU Init Succeeded");

        # this is a good time to set any fusion parameters

        self.imu.setSlerpPower(0.02)
        self.imu.setGyroEnable(True)
        self.imu.setAccelEnable(True)
        self.imu.setCompassEnable(True)

        if not self.pressure.pressureInit():
            self.log.critical("Pressure sensor Init Failed. Exiting...")
            sys.exit(1)
        else:
            self.log.info("Pressure sensor Init Succeeded")

        # we should try to ask for new data every x seconds
        # IMUGetPollInterval returns milliseconds, we translate to seconds
        # print("Recommended Poll Interval: %dms\n" % self.imu.IMUGetPollInterval())
        self.pollInterval = self.imu.IMUGetPollInterval() / 1000.0
        self.log.info("Used Poll Interval: %fs\n" % self.pollInterval)

        self.log.info("Initiating ultrasonic range finder")
        self.downDistanceSensor = UltrasonicRangeFinderProcess(RANGE_FINDER_DOWN_Trigger, RANGE_FINDER_DOWN_Echo)
        self.downDistanceSensor.start()

        self.log.info("Done! We can start...")

        self.firstReading = True
        self.startHeightAboveSea = None
        self.lastUpdate = datetime.now()
class IMUStateProvider:
    def __init__(self):
        self.log = logging.getLogger("StateProvider")
        self.__listeners = []

        self.log.info("Using settings file " + RT_IMU_LIB_SETTINGS_FILE + ".ini")
        if not os.path.exists(RT_IMU_LIB_SETTINGS_FILE + ".ini"):
            self.log.info("Settings file does not exist, will be created")

        self.settings = RTIMU.Settings(RT_IMU_LIB_SETTINGS_FILE)
        self.imu = RTIMU.RTIMU(self.settings)
        self.pressure = RTIMU.RTPressure(self.settings)

        self.log.info("IMU Name: " + self.imu.IMUName())
        self.log.info("Pressure Name: " + self.pressure.pressureName())

        if not self.imu.IMUInit():
            self.log.critical("IMU Init Failed. Exiting...")
            sys.exit(1)
        else:
            self.log.info("IMU Init Succeeded");

        # this is a good time to set any fusion parameters

        self.imu.setSlerpPower(0.02)
        self.imu.setGyroEnable(True)
        self.imu.setAccelEnable(True)
        self.imu.setCompassEnable(True)

        if not self.pressure.pressureInit():
            self.log.critical("Pressure sensor Init Failed. Exiting...")
            sys.exit(1)
        else:
            self.log.info("Pressure sensor Init Succeeded")

        # we should try to ask for new data every x seconds
        # IMUGetPollInterval returns milliseconds, we translate to seconds
        # print("Recommended Poll Interval: %dms\n" % self.imu.IMUGetPollInterval())
        self.pollInterval = self.imu.IMUGetPollInterval() / 1000.0
        self.log.info("Used Poll Interval: %fs\n" % self.pollInterval)

        self.log.info("Initiating ultrasonic range finder")
        self.downDistanceSensor = UltrasonicRangeFinderProcess(RANGE_FINDER_DOWN_Trigger, RANGE_FINDER_DOWN_Echo)
        self.downDistanceSensor.start()

        self.log.info("Done! We can start...")

        self.firstReading = True
        self.startHeightAboveSea = None
        self.lastUpdate = datetime.now()

    def registerListener(self, listener):
        self.log.debug("Appending listener: " + listener.name)
        self.__listeners.append(listener)

    # Combines IMU and Pressure readings into a full quadrocopter state
    def _read_state(self, imu_data, pressure_data, elevation_state):
        # here we compile the data
        newstate = QuadrocopterState()

        # collect fused data
        newstate.raw = imu_data

        # add the pressure data
        (air_pressure_valid, air_pressure, temperature_valid, temperature) = pressure_data

        self.log.debug("---------------  pressureRead  ---------------------")
        self.log.debug("air pressure valid: " + str(air_pressure_valid))
        self.log.debug("air pressure: " + str(air_pressure))
        self.log.debug("temperature valid: " + str(temperature_valid))
        self.log.debug("temperature: " + str(temperature))
        self.log.debug("------------------------------------")

        # copy so that everything is in one place
        newstate.raw["pressureValid"] = air_pressure_valid
        newstate.raw["pressure"] = air_pressure
        newstate.raw["temperatureValid"] = temperature_valid
        newstate.raw["temperature"] = temperature

        self.log.debug("---------------  getIMUData  ---------------------")
        self.log.debug(newstate.raw)

        if temperature_valid:
            newstate.temperature = temperature
        else:
            self.log.warn("Temperature reading is not valid!")

        if newstate.raw["accelValid"]:
            newstate.body_frame_acceleration = newstate.raw["accel"]
        else:
            self.log.error("Accelerometer reading is not valid!")

        # pressure should always be valid, but the code should not crash if we get a False here.
        if air_pressure_valid:
            newstate.airPressure = air_pressure
            newstate.heightAboveSeaBarometer = compute_height(newstate.airPressure)

            # special operation for first reading
            if self.firstReading:
                # TODO: we should use the average pressure over all measurements before liftoff
                self.startHeightAboveSea = newstate.heightAboveSea
                self.firstReading = False

            # calculate height above starting point
            # TODO: At the moment we have no simple way of getting the vertical speed
            #       Maybe this gets better with ultrasonic sensors
            newstate.elevation = State.by_value_and_speed(newstate.heightAboveSea - self.startHeightAboveSea, 0)
        else:
            self.log.warn("pressure reading is not valid!")

        (newstate.rotation.x.value, newstate.rotation.y.value, newstate.rotation.z.value) = newstate.raw["fusionPose"]
        (newstate.rotation.x.speed, newstate.rotation.y.speed, newstate.rotation.z.speed) = newstate.raw["gyro"]

        # TODO: For now, we don't take the orientation into account here
        # Of course this is not correct if the quadrocopter is not even in the air.
        newstate.elevation = elevation_state

        return newstate

    def update(self):
        finished = False
        while not finished:
            if self.imu.IMURead():

                finished = True

                newstate = self._read_state(self.imu.getIMUData(), self.pressure.pressureRead(), self.downDistanceSensor.read_distance())
                self.fuse_height_data(newstate)

                # new state is ready, we can print it
                self.log.debug(newstate)

                # give new state to listeners
                current_time = datetime.now()
                time_since_last_update = (current_time - self.lastUpdate).total_seconds()
                for listener in self.__listeners:
                    listener.new_sensor_reading(time_since_last_update, newstate)

                # save time for next iteration
                self.lastUpdate = current_time

        self.downDistanceSensor.stop()