#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()
