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