def talkerpozicija1():
pub = rospy.Publisher('pozicija', Imu, queue_size=10)
rospy.init_node('letjelica', anonymous=True)
rate = rospy.Rate(1) # 10hz
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/virtualcom0")
while not rospy.is_shutdown():
board.getData(MultiWii.RAW_IMU)
message = Imu()
message.header.frame_id = "/map"
message.header.stamp = rospy.Time.now()
message.orientation_covariance[0] = -1
message.angular_velocity_covariance[0] = -1
message.linear_acceleration_covariance[0] = -1
message.linear_acceleration.x = board.rawIMU['ax']
message.linear_acceleration.y = board.rawIMU['ay']
message.linear_acceleration.z = board.rawIMU['az']
message.angular_velocity.x = board.rawIMU['gx']
message.angular_velocity.y = board.rawIMU['gy']
message.angular_velocity.z = board.rawIMU['gz']
message.orientation.w = 1
message.orientation.x = board.rawIMU['ax'] * 3.14159265 / 180
message.orientation.y = board.rawIMU['ay'] * 3.14159265 / 180
message.orientation.z = board.rawIMU['az'] * 3.14159265 / 180
#elapsed = board.attitude['elapsed']
#message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t" .format(float(message.linear_acceleration.x),float(message.linear_acceleration.y),float(message.linear_acceleration.z),float(message.angular_velocity.x),float(message.angular_velocity.y),float(message.angular_velocity.z),float(message.orientation.x),float(message.orientation.y),float(message.orientation.z))
rospy.loginfo(message)
pub.publish(message)
rate.sleep()
def __init__(self, serPort, port):
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server.bind(("0.0.0.0", port))
self.server.listen(1)
print("Waiting for client...")
self.connection, self.clientAddress = self.server.accept()
print(self.connection)
print(self.clientAddress)
MultiWii.__init__(self, serPort)
def run(self):
self._board = MultiWii('/dev/ttyUSB0')
while True:
try:
message =str(self._board.getData(MultiWii.ATTITUDE))
print message
self._s.sendto(message,self._address)
except KeyboardInterrupt:
print 'Liberando socket...'
self._s.close()
break
def runAttitude():
board = MultiWii("/dev/bus/usb/001/002")
print "MARK2"
try:
while True:
board.getData(MultiWii.ATTITUDE)
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(
float(board.attitude['angx']), float(board.attitude['angy']),
float(board.attitude['heading']),
float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
stdout.flush()
except Exception, error:
print "Error on Main: " + str(error)
def __init__(self):
self.board = MultiWii("/dev/ttyACM0")
rospy.init_node("pi_drone", anonymous=True)
self.imu_pub = rospy.Publisher("/pi_drone/imu", Imu, queue_size=1)
self.attitude_pub = rospy.Publisher("/pi_drone/rpy", Vector3, queue_size=1)
rospy.Subscriber("/pi_drone/RC_in", sendRC, self.send_rc_callback)
rospy.Subscriber("/pi_drone/arm", Bool, self.arm_callback)
self.imu_msg = Imu()
self.rpy_msg = Vector3()
self.rc_channels = [1500, 1500, 1500, 1000, 1000, 1000, 1000, 1000]
self.arm = False
self.prev_imu_time = 0
self.prev_attitude_time = 0
while not rospy.is_shutdown():
self.attitude_publisher(30)
self.imu_publisher(50)
def talkerpozicija():
pub = rospy.Publisher('pozicija', koordinate, queue_size=10)
rospy.init_node('letjelica', anonymous=True)
rate = rospy.Rate(1) # 10hz
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/virtualcom0")
while not rospy.is_shutdown():
board.getData(MultiWii.RAW_IMU)
ax = board.rawIMU['ax']
ay = board.rawIMU['ay']
az = board.rawIMU['az']
gx = board.rawIMU['gx']
gy = board.rawIMU['gy']
gz = board.rawIMU['gz']
mx = board.rawIMU['mx']
my = board.rawIMU['my']
mz = board.rawIMU['mz']
elapsed = board.attitude['elapsed']
#message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t" .format(float(board.rawIMU['ax']),float(board.rawIMU['ay']),float(board.rawIMU['az']),float(board.rawIMU['gx']),float(board.rawIMU['gy']),float(board.rawIMU['gz']),float(board.rawIMU['mx']),float(board.rawIMU['my']),float(board.rawIMU['mz']),float(board.attitude['elapsed']))
#rospy.loginfo(message)
pub.publish(koordinate(ax, ay, az, gx, gy, gz, mx, my, mz, elapsed))
rate.sleep()
def talkerpid():
pub = rospy.Publisher('pid', pid, queue_size=10)
rospy.init_node('letjelica', anonymous=True)
rate = rospy.Rate(1) # 10hz
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/virtualcom0")
while not rospy.is_shutdown():
board.getData(MultiWii.PID)
message = pid()
message.rp = board.PIDcoef['rp']
message.ri = board.PIDcoef['ri']
message.rd = board.PIDcoef['rd']
message.pp = board.PIDcoef['pp']
message.pi = board.PIDcoef['pi']
message.pd = board.PIDcoef['pd']
message.yp = board.PIDcoef['yp']
message.yi = board.PIDcoef['yi']
message.yd = board.PIDcoef['yd']
#message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t" .format(float(message.linear_acceleration.x),float(message.linear_acceleration.y),float(message.linear_acceleration.z),float(message.angular_velocity.x),float(message.angular_velocity.y),float(message.angular_velocity.z),float(message.orientation.x),float(message.orientation.y),float(message.orientation.z))
rospy.loginfo(message)
pub.publish(message)
rate.sleep()
class Monitor():
def __init__(self, direccion, port):
self._s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._address = (direccion,port)
def run(self):
self._board = MultiWii('/dev/ttyUSB0')
while True:
try:
message =str(self._board.getData(MultiWii.ATTITUDE))
print message
self._s.sendto(message,self._address)
except KeyboardInterrupt:
print 'Liberando socket...'
self._s.close()
break
class FlightController:
def __init__(self):
self.im_width = 640
self.im_height = 480
self.board = MultiWii("COM4")
def compute_action(self, x1, y1, x2, y2, scale_factor=0.25):
# define the possible turning and moving action
self.board.getData(MultiWii.RAW_IMU)
turning, moving, vertical = self.board.rawIMU
area = (x2 - x1) * (y2 - y1)
center = [(x2 + x1) / 2, (y2 + y1) / 2]
# obtain a x center between 0.0 and 1.0
normalized_center = center[0] / self.im_width
# obtain a y center between 0.0 and 1.0
normalized_center.append(center[1] / self.im_height)
if normalized_center[0] > 0.6:
turning += scale_factor
elif normalized_center[0] < 0.4:
turning -= scale_factor
if normalized_center[1] > 0.6:
vertical += scale_factor
elif normalized_center[1] < 0.4:
vertical -= scale_factor
# if the area is too big move backwards
if area > 100:
moving += scale_factor
elif area < 80:
moving -= scale_factor
return [turning, moving, vertical]
def get_observation(self, velocities): #imu [ax, ay, az]
self.board.getData(MultiWii.RAW_IMU)
imu = self.board.rawIMU
return imu[:3] - velocities
def send_action(self, motors):
self.board.sendCMD(16, MultiWii.SET_MOTORS, motors)
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1.1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("COM4")
try:
while True:
board.getData(MultiWii.ATTITUDE)
# print (board.attitude) #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(
float(board.attitude['angx']), float(board.attitude['angy']),
float(board.attitude['heading']),
float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
stdout.flush()
# End of fancy printing
except Exception, error:
print("Error on Main: " + str(error))
__author__ = "Guru Prashanth Sridhar"
__copyright__ = "Copyright 2020 trakster.de"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Guru Prashanth Sridhar"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
board = MultiWii("/dev/ttyACM0")
pitch=1500
roll=1500
rotate=1500
throttle=1000
try:
board.arm()
while True:
board.getData(MultiWii.RAW_IMU)
#print (board.attitude) #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t" .format(float(board.rawIMU['ax']),float(board.rawIMU['ay']),float(board.rawIMU['az']),float(board.rawIMU['gx']),float(board.rawIMU['gy']),float(board.rawIMU['gz']),float(board.rawIMU['mx']),float(board.rawIMU['my']),float(board.rawIMU['mz']),float(board.attitude['elapsed']))
stdout.write("\r%s" % message )
stdout.flush()
pygame.event.get()
return button_analog
# Set the width and height of the screen [width,height]
size = [1000, 600]
# Define some colors
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
if __name__ == "__main__":
program_run = True
butterfly = MultiWii("COM4")
print("Butterfly Connected")
time.sleep(1)
controller = xbox1()
print("Controller Connected")
time.sleep(1)
sensitivity = 200 #max 500
print('Default Sensitivity : %d' % sensitivity)
time.sleep(1)
while program_run:
controller.update()
screen = pygame.display.set_mode(size)
pygame.display.set_caption("Control Panel")
def main():
board = MultiWii("/dev/ttyACM0")
"""Creation de la video"""
# Video Resolution
VIDEO_HEIGHT = 960
VIDEO_WIDTH = 1280
# Cross Hair Image
crossHair = Image.new("RGBA", (VIDEO_WIDTH, VIDEO_HEIGHT), (0, 0, 0, 0))
crossHairPixels = crossHair.load()
with picamera.PiCamera() as camera:
camera.resolution = (VIDEO_WIDTH, VIDEO_HEIGHT)
camera.framerate = 30
camera.led = False
overlay_renderer = None
camera.start_preview(fullscreen=True, window=(0, 0, 720, 1280))
#Add cockpit
img = Image.open('cockpit.png')
img = img.resize((1280, 960))
# Create an image padded to the required size with
# mode 'RGB'
pad = Image.new('RGBA', (
((img.size[0] + 31) // 32) * 32,
((img.size[1] + 15) // 16) * 16,
))
pad.paste(img, (0, 0))
# Add the overlay with the padded image as the source,
# but the original image's dimensions
topOverlay = camera.add_overlay(pad.tobytes(), size=img.size)
topOverlay.alpha = 128
topOverlay.layer = 3
time.sleep(0.5)
"""Connection de la board Inav Mavlink"""
board.getData(MultiWii.ATTITUDE)
#print board.attitude #uncomment for regular printing
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(
float(board.attitude['angx']), float(board.attitude['angy']),
float(board.attitude['heading']), float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
print "\r"
"""Connection du IR RX"""
irRxPin = 16
irRx = IRModule.IRRemote(callback='DECODE')
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # uses numbering outside circles
GPIO.setup(irRxPin, GPIO.IN) # set irPin to input
print(
'Starting IR remote sensing using DECODE function and verbose setting equal True '
)
GPIO.add_event_detect(irRxPin, GPIO.BOTH, callback=irRx.pWidth)
irRx.set_verbose(False)
irRx.set_callback(remote_callback)
print('Use ctrl-c to exit program')
"""Connection du IR TX"""
irTxPin = 21 # --> PIN11/GPIO17
GPIO.setmode(GPIO.BCM)
GPIO.setup(irTxPin, GPIO.OUT)
GPIO.output(irTxPin, GPIO.HIGH)
"""----"""
try:
while True:
if (hit >= 4):
print "PERDU drone a terre"
GPIO.output(irTxPin, GPIO.LOW)
break
time.sleep(0.5)
except KeyboardInterrupt:
camera.remove_overlay(topOverlay)
print "Cancelled"
__author__ = "Aldo Vargas" __copyright__ = "Copyright 2016 Altax.net" __license__ = "GPL" __version__ = "1" __maintainer__ = "Aldo Vargas" __email__ = "*****@*****.**" __status__ = "Development" __modified_by__ = "Curtis Wang, [email protected], for Python 3.5+" from pymultiwii import MultiWii if __name__ == "__main__": #board = MultiWii("/dev/ttyUSB0") board = MultiWii("/dev/tty.usbserial-A801WZA1") try: while True: #example of 8 RC channels to be send data = [1500, 1550, 1600, 1560, 1000, 1040, 1000, 1000] # Old function #board.sendCMD(16,MultiWii.SET_RAW_RC,data) #New function that will receive attitude after setting the rc commands board.sendCMDreceiveATT(16, MultiWii.SET_RAW_RC, data) print(board.attitude) except Exception as error: print("Error on Main: " + str(error))
'''predicting images'''
from pymultiwii import MultiWii
j = 1
if __name__ == "__main__":
pitch = 1500
roll = 1500
rotate = 1500
throttle = 1000
changerate = 10
i = 0
cam = 1
control_signal_log = {}
control_signal_log_timer_start = time.time()
board = MultiWii("COM4")
#cap = cv2.VideoCapture(0)
try:
print('Press button g to start control')
print('Press button q to quit ')
while True:
#camera show
#ret, frame = cap.read()
#cv2.imshow('frame',frame)
#cv2.imwrite('img{:>05}.png'.format(i), frame)
CNN = -1
prestate = 'None'
if (msvcrt.kbhit()):
key = msvcrt.getch()
if (key.decode() == 'd'):
board.disarm()
from pymultiwii import MultiWii
from sys import stdout
import time
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/serial0")
try:
print("start")
board.arm()
print("armed")
while True:
data = [1500, 1550, 1600, 1560, 1000, 1040, 1000, 1000]
board.sendCMDreceiveATT(16, MultiWii.SET_RAW_RC, data)
#board.sendCMD(16,MultiWii.SET_RAW_RC,data)
print board.getData(MultiWii.RC)
time.sleep(0.5)
except Exception, error:
print "Error on Main: " + str(error)
print("end")
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.usbserial-A801WZA1")
try:
while True:
#example of 8 RC channels to be send
data = [1500,1550,1600,1560,1000,1040,1000,1000]
# Old function
#board.sendCMD(16,MultiWii.SET_RAW_RC,data)
#New function that will receive attitude after setting the rc commands
board.sendCMDreceiveATT(16,MultiWii.SET_RAW_RC,data)
print (board.attitude)
except Exception,error:
print ("Error on Main: "+str(error))
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1.1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
import time
if __name__ == "__main__":
board = MultiWii()
board.start('/dev/ttyACM0')
board.requestFrame(board.ATTITUDE)
time.sleep(0.5)
d = board.receivePacket('<3h', 6)
if d:
print '\nPitch:\t%.0f'%(float(d[1])/10.0)
print 'Roll:\t%.0f'%(float(d[0])/10.0)
print 'Yaw:\t%.0f'%(float(d[2]))
print 'Test:\t',board.rx_attitude['angx']
#printf(drone.sendFrame(drone.SET_RAW_RC, set_raw_rc, '<8H'), 1)
from pymultiwii import MultiWii
from sys import stdout
import time
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/serial0")
try:
while True:
board.getData(MultiWii.ATTITUDE)
print board.attitude
time.sleep(1)
except Exception, error:
print "Error on Main: " + str(error)
import socket
from pymultiwii import MultiWii
import time
ip = ''
port = 5003
bufflen = 19
address = ("192.168.1.33", 5000)
cmd_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
mon_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
dir = ('', 5003)
cmd_socket.bind((ip, port))
uav = MultiWii("/dev/ttyUSB0")
uav.arm()
time.sleep(5)
orden = []
##orden={"roll":-1,"pitch":-1,"yaw":-1,"throttle":-1}
try:
while True:
data, _ = cmd_socket.recvfrom(bufflen)
data = data.split(':')
if data == ['']:
continue
#print str(data)
orden_s = [int(data[0]), int(data[1]), int(data[2]), int(data[3])]
def main():
global laserOverlay, camera, hit, vivant
laserShot = None
board = MultiWii("/dev/ttyACM0")
"""Creation de la video"""
# Video Resolution
VIDEO_HEIGHT = 960
VIDEO_WIDTH = 1280
# Cross Hair Image
crossHair = Image.new("RGBA", (VIDEO_WIDTH, VIDEO_HEIGHT), (0, 0, 0, 0))
crossHairPixels = crossHair.load()
with picamera.PiCamera() as camera:
camera.resolution = (VIDEO_WIDTH, VIDEO_HEIGHT)
camera.framerate = 30
camera.led = False
overlay_renderer = None
camera.start_preview(fullscreen=True, window=(0, 0, 720, 1280))
#Add cockpit
img = Image.open('cockpit.png')
img = img.resize((1280, 960))
# Add cockpit Image to FPV feed
pad = Image.new('RGBA', (
((img.size[0] + 31) // 32) * 32,
((img.size[1] + 15) // 16) * 16,
))
pad.paste(img, (0, 0))
topOverlay = camera.add_overlay(pad.tobytes(), size=img.size)
topOverlay.alpha = 128
topOverlay.layer = 3
#Add Laser PNG for fire
imgLaser = Image.open('laser-fire.png')
imgLaser = imgLaser.resize((1280, 960))
laser = Image.new('RGBA', (
((img.size[0] + 31) // 32) * 32,
((img.size[1] + 15) // 16) * 16,
))
laser.paste(imgLaser, (0, 0))
laserOverlay = camera.add_overlay(laser.tobytes(),
size=imgLaser.size,
layer=1)
laserOverlay.alpha = 128
#laserOverlay.layer = 2
#Add Explosion layer
imgExpl = Image.open('explosion.png')
imgExpl = imgExpl.resize((1280, 960))
explosion = Image.new('RGBA', (
((img.size[0] + 31) // 32) * 32,
((img.size[1] + 15) // 16) * 16,
))
explosion.paste(imgExpl, (0, 0))
explOverlay = camera.add_overlay(explosion.tobytes(),
size=imgExpl.size,
layer=1)
explOverlay.alpha = 128
time.sleep(0.1)
"""Connection de la board Inav Mavlink"""
board.getData(MultiWii.ATTITUDE)
#print board.attitude #uncomment for regular printing
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(
float(board.attitude['angx']), float(board.attitude['angy']),
float(board.attitude['heading']), float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
print "\r"
"""Connection du IR RX"""
irRxPin = 16
irRx = IRModule.IRRemote(callback='DECODE')
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # uses numbering outside circles
GPIO.setup(irRxPin, GPIO.IN) # set irPin to input
print(
'Starting IR remote sensing using DECODE function and verbose setting equal True '
)
GPIO.add_event_detect(irRxPin, GPIO.BOTH, callback=irRx.pWidth)
irRx.set_verbose(False)
irRx.set_callback(remote_callback)
print('Use ctrl-c to exit program')
"""Connection du IR TX"""
irTxPin = 21 # --> PIN11/GPIO17
GPIO.setmode(GPIO.BCM)
GPIO.setup(irTxPin, GPIO.OUT)
GPIO.output(irTxPin, GPIO.HIGH)
"""----"""
try:
while True:
while vivant == 1:
if (hit >= 4):
GPIO.output(irTxPin, GPIO.LOW)
explOverlay.layer = 5
vivant = 0
print "Fin - Toucher #", hit, "fois"
except KeyboardInterrupt:
camera.remove_overlay(topOverlay)
print "Cancelled"
#!/usr/bin/env python
"""test-send.py: Test script to send RC commands to a MultiWii Board."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__license__ = "GPL"
__version__ = "2"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
import time
if __name__ == "__main__":
#board = MultiWii("/dev/tty.usbserial-AM016WP4")
board = MultiWii("COM4")
try:
board.arm()
print("Board is armed now!")
print("In 3 seconds it will disarm...")
time.sleep(3)
board.disarm()
print("Disarmed.")
time.sleep(3)
except Exception as error:
print("Error on Main: " + str(error))
class pi_drone_driver():
def __init__(self):
self.board = MultiWii("/dev/ttyACM0")
rospy.init_node("pi_drone", anonymous=True)
self.imu_pub = rospy.Publisher("/pi_drone/imu", Imu, queue_size=1)
self.attitude_pub = rospy.Publisher("/pi_drone/rpy", Vector3, queue_size=1)
rospy.Subscriber("/pi_drone/RC_in", sendRC, self.send_rc_callback)
rospy.Subscriber("/pi_drone/arm", Bool, self.arm_callback)
self.imu_msg = Imu()
self.rpy_msg = Vector3()
self.rc_channels = [1500, 1500, 1500, 1000, 1000, 1000, 1000, 1000]
self.arm = False
self.prev_imu_time = 0
self.prev_attitude_time = 0
while not rospy.is_shutdown():
self.attitude_publisher(30)
self.imu_publisher(50)
def deg2rad(self, deg):
rad = (3.14159/180.0) * deg
return rad
def attitude_publisher(self, rate):
if ((time.time() - self.prev_attitude_time) >= (1.0 / rate)):
self.prev_attitude_time = time.time()
self.board.getData(MultiWii.ATTITUDE)
self.rpy_msg.x = self.board.attitude['angx']
self.rpy_msg.y = self.board.attitude['angy']
self.rpy_msg.z = self.board.attitude['heading']
self.attitude_pub.publish(self.rpy_msg)
def imu_publisher(self, rate):
if ((time.time() - self.prev_imu_time) >= (1.0 / rate)):
self.prev_imu_time = time.time()
self.board.getData(MultiWii.RAW_IMU)
ax = self.board.rawIMU['ax']
ay = self.board.rawIMU['ay']
az = self.board.rawIMU['az']
gx = self.board.rawIMU['gx']
gy = self.board.rawIMU['gy']
gz = self.board.rawIMU['gz']
self.imu_msg.header.stamp = rospy.Time.now()
self.imu_msg.header.frame_id = "imu_link"
self.imu_msg.orientation_covariance = [-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.imu_msg.angular_velocity_covariance = [-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.imu_msg.linear_acceleration_covariance = [-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.imu_msg.linear_acceleration.x = 9.81 * ax/512
self.imu_msg.linear_acceleration.y = 9.81 * ay/512
self.imu_msg.linear_acceleration.z = -9.81 * az/512
self.imu_msg.angular_velocity.x = self.deg2rad(gx/4.096)
self.imu_msg.angular_velocity.y = self.deg2rad(gy/4.096)
self.imu_msg.angular_velocity.z = self.deg2rad(gz/4.096)
#rospy.loginfo(imu_msg)
self.imu_pub.publish(self.imu_msg)
def send_rc_callback(self, rc_data):
self.rc_channels[0] = rc_data.channels[0]
self.rc_channels[1] = rc_data.channels[1]
self.rc_channels[2] = rc_data.channels[2]
self.rc_channels[3] = rc_data.channels[3]
if self.arm:
self.rc_channels[4] = 2000
else:
self.rc_channels[4] = 1000
#print(self.rc_channels)
self.board.sendCMD(16,MultiWii.SET_RAW_RC, self.rc_channels)
def arm_callback(self, msg):
if msg.data:
self.arm = True
else:
self.arm = False
def main():
laserShot=None
board = MultiWii("/dev/ttyACM0")
"""Creation de la video"""
# Cross Hair Image
crossHair = Image.new("RGBA", (VIDEO_WIDTH, VIDEO_HEIGHT),(0, 0, 0, 0))
crossHairPixels = crossHair.load()
#Add cockpit
img = Image.open('cockpit.png')
img = img.resize((1280,960))
# Create an image padded to the required size with
# mode 'RGB'
pad = Image.new('RGBA', (
((img.size[0] + 31) // 32) * 32,
((img.size[1] + 15) // 16) * 16,
))
pad.paste(img, (0, 0))
# Add the overlay with the padded image as the source,
# but the original image's dimensions
topOverlay = camera.add_overlay(pad.tobytes(), size=img.size)
topOverlay.alpha = 128
topOverlay.layer = 3
imgLaser = Image.open('laser-fire.png')
imgLaser = imgLaser.resize((1280,960))
# Create an image padded to the required size with
# mode 'RGB'
laser = Image.new('RGBA', (
((img.size[0] + 31) // 32) * 32,
((img.size[1] + 15) // 16) * 16,
))
laser.paste(imgLaser, (0, 0))
laserOverlay = camera.add_overlay(laser.tobytes(), size=imgLaser.size)
laserOverlay.alpha = 128
laserOverlay.layer = 2
time.sleep(0.5)
#camera.remove_overlay(laserOverlay)
"""Connection de la board Inav Mavlink"""
board.getData(MultiWii.ATTITUDE)
#print board.attitude #uncomment for regular printing
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(float(board.attitude['angx']),float(board.attitude['angy']),float(board.attitude['heading']),float(board.attitude['elapsed']))
stdout.write("\r%s" % message )
print "\r"
"""Connection du IR RX"""
irRxPin = 16
irRx = IRModule.IRRemote(callback='DECODE')
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # uses numbering outside circles
GPIO.setup(irRxPin,GPIO.IN) # set irPin to input
print('Starting IR remote sensing using DECODE function and verbose setting equal True ')
GPIO.add_event_detect(irRxPin,GPIO.BOTH,callback=irRx.pWidth)
irRx.set_verbose(False)
irRx.set_callback(remote_callback)
print('Use ctrl-c to exit program')
"""Connection du IR TX"""
irTxPin = 21 # --> PIN11/GPIO17
GPIO.setmode(GPIO.BCM)
GPIO.setup(irTxPin, GPIO.OUT)
GPIO.output(irTxPin, GPIO.HIGH)
"""----"""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1.1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
while True:
board.getData(MultiWii.ATTITUDE)
#print board.attitude #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(
float(board.attitude['angx']), float(board.attitude['angy']),
float(board.attitude['heading']),
float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
stdout.flush()
# End of fancy printing
except Exception as error:
print("Error on Main: " + str(error))
if output_buffer > self.K['SAT']:
output = self.K['SAT'] * self.K['KU']
self.s_flag = 0
elif output_buffer < -(self.K['SAT']):
output = -(self.K['SAT'] * self.K['KU'])
self.s_flag = 0
else:
output = output_buffer * self.K['KU']
self.s_flag = 1
return output
drone = MultiWii()
input("press any key to continue")
try:
f = open("decision.txt", mode='w')
f.write(str(init + 0.1))
print("detected")
except Exception as e:
print(e)
finally:
f.close()
try:
drone.open("/dev/rfcomm0")
time.sleep(2)
drone.arm()
time.sleep(0.5)
"""test-send.py: Test script to send RC commands to a MultiWii Board."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
import time
if __name__ == "__main__":
#board = MultiWii("/dev/tty.usbserial-AM016WP4")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
board.arm()
print ("Board is armed now!")
print ("In 3 seconds it will disarm...")
time.sleep(3)
board.disarm()
print ("Disarmed.")
time.sleep(3)
except Exception,error:
print ("Error on Main: "+str(error))
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
board = MultiWii("/dev/ttyUSB0")
#board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
while True:
board.getData(MultiWii.RAW_IMU)
#print board.attitude #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t".format(
float(board.rawIMU['ax']), float(board.rawIMU['ay']),
float(board.rawIMU['az']), float(board.rawIMU['gx']),
float(board.rawIMU['gy']), float(board.rawIMU['gz']),
float(board.rawIMU['mx']), float(board.rawIMU['my']),
float(board.rawIMU['mz']), float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
stdout.flush()
from pymultiwii import MultiWii
import time
#serialPort = "/dev/ttyAMA0"
serialPort = "/dev/tty.SLAB_USBtoUART"
board = MultiWii(serialPort)
print("woken up")
#board.sendCMD(0,MultiWii.REBOOT,[],'')
time.sleep(1)
board.setVTX(5, 5, 1)
time.sleep(3)
test = board.getData(MultiWii.ATTITUDE)
print(test)
while True:
test = board.getData(MultiWii.ATTITUDE)
#print board.getData(MultiWii.STATUS)
print(board.getData(MultiWii.VTX_CONFIG))
test = 1
label='GX')
plt.plot([self.sb, self.senses], [self.gybuf, gy],
color='black',
label='GY')
plt.plot([self.sb, self.senses], [self.gzbuf, gz],
color='pink',
label='GZ')
self.fig.canvas.draw()
self.sb, self.axbuf, self.aybuf, self.azbuf, self.gxbuf, self.gybuf, self.gzbuf = self.senses, ax, ay, az, gx, gy, gz
if __name__ == "__main__":
chart = Chart()
#serialPort = "/dev/tty.usbserial-A801WZA1"
serialPort = "/dev/tty.SLAB_USBtoUART"
board = MultiWii(serialPort)
try:
while True:
board.getData(MultiWii.RAW_IMU)
#print (board.rawIMU)
t = float(board.rawIMU['timestamp'])
ax = board.rawIMU['ax']
ay = board.rawIMU['ay']
az = board.rawIMU['az']
gx = board.rawIMU['gx']
gy = board.rawIMU['gy']
gz = board.rawIMU['gz']
chart.plot(ax, ay, az, gx, gy, gz)
except Exception, error:
print("Error on Main: " + str(error))
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1.1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
import time
if __name__ == "__main__":
board = MultiWii("/dev/ttyACM0")
i=0
#board = MultiWii("/dev/serial0")
#board.sendCMD(8,MultiWii.RESET_CONF,[])
try:
while True:
board.getData(MultiWii.ATTITUDE)
#print board.attitude #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(float(board.attitude['angx']),float(board.attitude['angy']),float(board.attitude['heading']),float(board.attitude['elapsed']))
stdout.write("\r ATTITUDE: %s" % message )
print "\r"
board.getData(MultiWii.RAW_IMU)
message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t" .format(float(board.rawIMU['ax']),float(board.rawIMU['ay']),float(board.rawIMU['az']),float(board.rawIMU['gx']),float(board.rawIMU['gy']),float(board.rawIMU['gz']),float(board.rawIMU['mx']),float(board.rawIMU['my']),float(board.rawIMU['mz']),float(board.attitude['elapsed']))
stdout.write("\r RAW IMU: %s" % message )
"""show-attitude.py: Script to ask the MultiWii Board attitude and print it."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
while True:
board.getData(MultiWii.RAW_IMU)
#print (board.attitude) #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t" .format(float(board.rawIMU['ax']),float(board.rawIMU['ay']),float(board.rawIMU['az']),float(board.rawIMU['gx']),float(board.rawIMU['gy']),float(board.rawIMU['gz']),float(board.rawIMU['mx']),float(board.rawIMU['my']),float(board.rawIMU['mz']),float(board.attitude['elapsed']))
stdout.write("\r%s" % message )
stdout.flush()
# End of fancy printing
except Exception,error:
print ("Error on Main: "+str(error))
def getClockRate():
t1 = time.time()
for i in range(100):
getTheta()
print((time.time() - t1) / 100.)
def getLateralVelocity():
global data1, board
board.getData(MultiWii.RAW_IMU)
if __name__ == '__main__':
global data1, board
board = MultiWii("/dev/tty.SLAB_USBtoUART")
t0 = time.time()
deltaT = 0
velocity = 0
position = 0
for i in range(1000):
board.getData(MultiWii.RAW_IMU)
velocity += board.rawIMU['az'] * (time.time() - t0)
position += velocity * (time.time() - t0)
t0 = time.time()
if i % 10 == 0:
print("acceleration: ", board.rawIMU['az'], "velocity: ", velocity,
"position ", position)
#print(getTheta(), "degrees")
getTheta()
#print("rotationalV",get_dtheta_dt())
"""show-attitude.py: Script to ask the MultiWii Board attitude and print it."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__license__ = "GPL"
__version__ = "1.1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
while True:
board.getData(MultiWii.ATTITUDE)
#print (board.attitude) #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(float(board.attitude['angx']),float(board.attitude['angy']),float(board.attitude['heading']),float(board.attitude['elapsed']))
stdout.write("\r%s" % message )
stdout.flush()
# End of fancy printing
except Exception,error:
print ("Error on Main: "+str(error))