def test_cubic(self):
        current = State.by_value_and_speed(10, 30)
        target = State.by_value_and_speed(0, 0)
        plan = current.plan_change_to(target).in_seconds(1.5)

        self.assertEqual(plan.valueAt(0), 10)
        self.assertEqual(plan.valueAt(1.5), 0)
        self.assertEqual(plan.calculate_target_speed_for_time_period(0, 1.5), -10 / 1.5)
    def test_simpletime(self):
        roll = 5 # radian
        rollSpeed =  1 # radian per second
        rollState = State.by_value_and_speed(roll, rollSpeed)

        rollTarget = State.by_value_and_speed(6, 1)

        plan = rollState.plan_change_to(rollTarget).in_seconds(1)
        self.assertEqual(plan.valueAt(0), 5)
        self.assertEqual(plan.valueAt(1), 6)
        self.assertEqual(plan.calculate_target_speed_for_time_period(0, 1), 1.0)
    def test_quadratic(self):
        current = State.by_value_and_speed(0, 2)
        target = State.by_value_and_speed(0, -2)
        plan = current.plan_change_to(target).in_seconds(0.25)

        self.assertEqual(plan.valueAt(0), 0)
        self.assertEqual(plan.valueAt(0.25), 0)
        self.assertEqual(plan.valueAt(0.125), 0.5)
        self.assertEqual(plan.calculate_target_speed_for_time_period(0, 0.25), 0)
        self.assertEqual(plan.calculate_target_speed_for_time_period(0, 0.125), 2)
        self.assertEqual(plan.calculate_target_speed_for_time_period(0.125, 0.25), -2)
    def measure(self, queue, stop_flag, time_between_measurements):

        self.log.debug("starting ultrasonic measurement")
        try:
            time_last_update = time.time()
            while not stop_flag.value:

                distance_now = self._measure_distance()
                time_now = time.time()
                duration_measurement = time_now-time_last_update
                time_last_update = time_now

                # use weighted average as filter
                if self.distance is None:
                    self.distance = distance_now
                    self.speed = 0.0
                    self.log.debug("measured distance is zero!")
                else:
                    newfiltereddistance = self.distance * VALUE_FILTER_STRENGTH + distance_now * (1 - VALUE_FILTER_STRENGTH)
                    self.speed = (newfiltereddistance - self.distance) / duration_measurement
                    self.distance = newfiltereddistance

                self.log.debug("Measured Distance = %.12f m" % distance_now)
                self.log.debug("Filtered Distance = %.12f m" % self.distance)

                queue.put(State.by_value_and_speed(self.distance, self.speed))

                time.sleep(time_between_measurements)
        finally:
            # after we stop measuring, we should clean up
            # TODO: is this really right? Maybe someone else is still active on GPIO?
            # should not be the case, we are in our own process
            GPIO.cleanup()
            self.log.debug("GPIO cleanup finished!")
    def read_distance(self, orientation_quaternion):

        # get the newest reading from the queue without blocking
        while not self.distance_queue.empty():
            self.distance = self.distance_queue.get(False)

        if self.distance is None:
            return State.by_value_and_speed(0.0, 0.0), SENSOR_ERROR_MAX
        else:
            # Second value is expected error
            return self.distance, _estimated_error_above_ground(orientation_quaternion)
    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 __init__(self):
     self.x = State.by_value_and_speed(0, 0)
     self.y = State.by_value_and_speed(0, 0)
     self.z = State.by_value_and_speed(0, 0)