#this is an experiment with the auto launch on feature from pyparrot.Minidrone import Mambo mamboAddr = "e0:14:bc:c5:3d:cb" mambo = Mambo(mamboAddr, use_wifi=False) print("trying to connect") success = mambo.connect(num_retries=3) print("connected: %s" % success) if (success): print("getting updates") mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) print("ready for takeoff!") mambo.turn_on_auto_takeoff() mambo.hover() mambo.smart_sleep(4) while True: print(mambo.sensors.flying_state) if mambo.sensors.flying_state is "hovering": print("landing!") mambo.safe_land(5) mambo.smart_sleep(5) break mambo.disconnect()
def init(): global tko global land global cannon global auto_tko global linX global linY global Alt global Hdg global p_mode global need_to_change_mode global need_to_toggle_mode rospy.init_node('mambo_node', anonymous=True) mamboAdd = rospy.get_param('~bt', str("e0:14:60:5c:3d:c7")) wifi = rospy.get_param('~mambo_wifi', False) retries = rospy.get_param('~mambo_retries', 3) rospy.loginfo("\n" + rospy.get_name() + "\nParameters:\n" + mamboAdd + "\n" + str(wifi) + "\n" + str(retries) + "\n") s_cmd_vel = rospy.Subscriber('cmd_vel', Twist, cb_cmd_vel) s_take_off = rospy.Subscriber('take_off', Empty, cb_take_off) s_land = rospy.Subscriber('land', Empty, cb_land) s_cannon = rospy.Subscriber('cannon', Empty, cb_shoot_cannon) s_auto_tko = rospy.Subscriber('auto_tko', Empty, cb_auto_take_off) s_piloting_mode = rospy.Subscriber('piloting_mode', UInt8, cb_pilotmode) s_toggle_mode = rospy.Subscriber('toggle_mode', Empty, cb_toggle_mode) p_ready = rospy.Publisher('ready', String, queue_size=30) mambo = Mambo(mamboAdd, use_wifi=wifi) success = mambo.connect(retries) if (success): mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) mambo.flat_trim() rate = rospy.Rate(100) while not rospy.is_shutdown(): if tko == True: mambo.safe_takeoff(2) p_ready.publish("ok") tko = False if land == True: mambo.safe_land(2) land = False if cannon == True: mambo.fire_gun() cannon = False if auto_tko == True: mambo.turn_on_auto_takeoff() auto_tko = False if need_to_change_mode == True: if pilotmode(mambo, p_mode): print("Changed mode successfully") else: print("Failed changing mode") need_to_change_mode = False if need_to_toggle_mode == True: if togglemode(mambo): print("Activating preffered mode...") else: print("Failed activating preferred mode") need_to_toggle_mode = False r_y = round(linY, 2) p_x = round(linX, 2) v_z = round(Alt, 2) y_z = round(Hdg, 2) r_y = sat(r_y, 0.98) p_x = sat(p_x, 0.98) v_z = sat(v_z, 0.98) y_z = sat(y_z, 0.98) mambo.fly_direct(roll=(-r_y * 100), pitch=(p_x * 100), yaw=(-y_z * 100), vertical_movement=(v_z * 100), duration=0.01) # linY = 0 # linX = 0 # Hdg = 0 # Alt = 0 rate.sleep()