def main():
"""
test function
"""
print "creating drone master...\n"
drone_master = Drone("/test")
print "creating drone slave...\n"
drone_slave = DroneSlave("/drone1")
print "set master...\n"
drone_slave.set_master(drone_master)
print "refresh flight infos...\n"
Drone.refresh_flight_info([drone_slave])
def slave_control():
"""
main program loop
"""
Drone.refresh_flight_info([master_d, slave_d])
check_safety_issues()
# Altitude fix
if((master_d.z - settings.MARGIN_ALT) > (slave_d.z + settings.ALT_DIFF)) or ((slave_d.z + settings.ALT_DIFF) > (master_d.z + settings.MARGIN_ALT)):
slave_d.fix_z(settings.ALT_DIFF)
# Distance Fix
elif((settings.DIST - settings.MARGIN_DIST) > Drone.calc_distance(master_d, slave_d)) or (Drone.calc_distance(master_d, slave_d) > (settings.DIST + settings.MARGIN_DIST)):
slave_d.fix_distance(settings.DIST)
# Orinetation Fix
elif((master_d.o - settings.MARGIN_ORIEN) > (slave_d.o + settings.ORIEN_DIFF)) or ((slave_d.o + settings.ORIEN_DIFF) > (master_d.o + settings.MARGIN_ORIEN)):
slave_d.fix_o(settings.ORIEN_DIFF)
# Coliniearity Fix ( the angle must be 0 if we wants to be colinear)
elif((settings.COL_DIFF - settings.MARGIN_COL) > Drone.calc_colinearity(master_d, slave_d)) or (Drone.calc_colinearity(master_d, slave_d) > (settings.COL_DIFF + settings.MARGIN_COL)):
slave_d.fix_colinearity(settings.COL_DIFF)
