class Initiator:
# Here we wire all the components together
def __init__(self):
logging.basicConfig(filename='freecopter.log', level=logging.INFO)
self.motors = AdafruitPwmControlledMotors(debug_flag=True)
self.controlLoop = ControlLoop(self.motors, PIDController)
self.sensor_fusion = SensorFusionMaster()
self.highLevelLogic = HighLevelLogic(self.controlLogic)
# Will be set on the first update.
self.time_of_last_update = None
def update(self):
now = datetime.now()
if self.time_of_last_update is None:
# some very small time Delta for the start
time_delta = 0.000001
else:
time_delta = (now - self.time_of_last_update).total_seconds()
# gather sensor information
vehicle_state = self.sensor_fusion.readState()
# high level logic updates the other systems
self.highLevelLogic.update(time_delta)
# save time for next iteration
self.time_of_last_update = now
def __init__(self):
logging.basicConfig(filename='freecopter.log', level=logging.INFO)
self.motors = AdafruitPwmControlledMotors(debug_flag=True)
self.controlLoop = ControlLoop(self.motors, PIDController)
self.sensor_fusion = SensorFusionMaster()
self.highLevelLogic = HighLevelLogic(self.controlLogic)
# Will be set on the first update.
self.time_of_last_update = None
def test_zero(self):
master = SensorFusionMaster()
master.update()
# Some warmups
for x in range(100):
master.update()
time.sleep(0.01)
print("Warmup is done. Now the pressure sensor should be ready...")
# give the sensor time to collect values
time.sleep(0.05)
for x in range(10000):
state = master.update()
self.assertIsNotNone(state.temperature)
self.assertIsNotNone(state.attitude)
self.assertIsNotNone(state.height)
self.assertIsNotNone(state.gps)
self.assertGreater(state.air_pressure, 100)
self.assertGreater(state.height.height_above_ground, 0)
self.assertGreater(state.height.ground_height_barometer, 0)
logging.info("height above ground: {}".format(state.height.height_above_ground))
logging.info("vertical speed: {}".format(state.height.vertical_speed))
#logging.info("barometer ground height: {}".format(state.height.ground_height_barometer))
# TODO: correct axis
#logging.info("yaw, pitch, roll: {}, {}, {}".format(state.attitude.rotation.x, state.attitude.rotation.y, state.attitude.rotation.z))
time.sleep(0.001)
