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)