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 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)
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)
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
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"
"""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))
"""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))
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)
from pymultiwii import MultiWii
import struct
import time
serialPort = "/dev/tty.usbmodem1411"
board = MultiWii(serialPort)
while True:
# print board.sendCMD(4,MultiWii.SET_RAW_RC,[1500,1500,1500,1599]) #MSP_SET_RAW_RC
print board.getData(MultiWii.STATUS)
print board.getData(MultiWii.RAW_IMU)
print board.getData(MultiWii.ATTITUDE)
print board.getData(MultiWii.ALTITUDE)
print board.getData(MultiWii.MOTOR)
print board.getData(MultiWii.RC)
print board.getData(MultiWii.ANALOG)
print board.getData(MultiWii.RC_TUNING)
print board.getData(MultiWii.DEBUG)
# board.arm()
#
# board.sendCMD(8,214,[1500])
# data = [1000,2000]
# checksum = 0
# total_data = ['$', 'M', '<', 100, 214] + data
# for i in struct.pack('<2B%dH' % len(data), *total_data[3:len(total_data)]):
# checksum = checksum ^ ord(i)
# print checksum
# total_data.append(checksum)
# print total_data
# print len(data)
#
__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
try:
while True:
print "\r ####Boucle ", i
v = board.getData(MultiWii.RC)
r = board.rcChannels['roll']
p = board.rcChannels['pitch']
y = board.rcChannels['yaw']
t = board.rcChannels['throttle']
aux1 = board.rcChannels['aux1']
aux2 = board.rcChannels['aux2']
aux3 = board.rcChannels['aux3']
aux4 = board.rcChannels['aux4']
print "V==>", v
if (i > 5 and i < 15 and v != None):
print "\r\r#########\r"
datas = [1500, 1500, 1800, 1500]
print "\rsend =", datas
board.sendCMDreceiveATT(16, MultiWii.SET_RAW_RC, datas)
print "###########\r\r"
placa.arm() arm = 1 elif boton_up == 12 and arm ==1: placa.disarm() arm = 0 if arm ==1: data =[roll,pitch,yaw,trothle,1000,1000,1000,1000] placa.sendCMDreceiveATT(16,MultiWii.SET_RAW_RC,data) x = int(placa.attitude['angx']) y = int(placa.attitude['angy']) z = int(placa.attitude['heading']) array = [x,y,z,bat] msg = pickle.dumps(array) sock.sendto(msg, (UDP_HOST,UDP_PORT)) elif arm ==0: placa.getData(MultiWii.ATTITUDE) x = int(placa.attitude['angx']) y = int(placa.attitude['angy']) z = int(placa.attitude['heading']) array = [x,y,z,bat] msg = pickle.dumps(array) sock.sendto(msg, (UDP_HOST,UDP_PORT)) HayDatosSocket = select.select([sock],[],[],0.5) if HayDatosSocket[0]: Socketdata = sock.recv(bufer) mando = pickle.loads(Socketdata) roll = ROLL + mando[3] pitch = PITCH + mando[2] yaw = YAW + mando[1] trothle = TROTHLE + mando[0] boton_up = mando[5]
from pymultiwii import MultiWii
serialPort = "/dev/ttyACM0"
board = MultiWii(serialPort)
while True:
print(board.getData(MultiWii.ATTITUDE))
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"
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
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")
__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 )
print "\r GPS",board.getData(MultiWii.RAW_GPS)
#board.arm()
# End of fancy printing
#print "-->debut rc channel"
# ROLL/PITCH/YAW/THROTTLE/AUX1/AUX2/AUX3AUX4
#data = [1500,1550,1600,1560,1800,1800,1800,1800]
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)
"""----"""
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())
#print(getTime())
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