/elec/quadcopter

#!/usr/bin/python

#

# rc-interface-test

import pygtk

pygtk.require( '2.0' );

import gtk, serial, struct

from io import BytesIO

class MainDialogue:

	# number of channels to read

	NUM_CHANNELS = 8

	# number of motor channels

	NUM_MOTORS = 4

	# maximum channel value

	MAX_CHANNEL_VALUE = 1000

	# serial device to use

	SERIAL_DEVICE = '/dev/ttyUSB0'

	def __init__( self ):

		try:

			self.link = serial.Serial( self.SERIAL_DEVICE, 9600 )

			self.link_error = None

		except serial.serialutil.SerialException as e:

			self.link = None

			self.link_error = e

		self.got_one = False

		self.motor_values = [ 0 ] * self.NUM_MOTORS

		self.quit = False

		self.buffer = bytearray()

	def add_info( self, vbox, heading = False, text = '' ):

		label = gtk.Label()

		if( heading ):

			label.set_markup( "<b>%s</b>" % text )

		else:

			label.set_label( text )

#		label.set_justify( gtk.JUSTIFY_LEFT )

		label.set_alignment( 0, 0 )

		vbox.pack_start( label, False, False )

	def setup_window( self ):

		# create and set up window

		self.window = gtk.Window( gtk.WINDOW_TOPLEVEL )

		self.window.connect( "delete_event", self.destroy )

		self.window.connect( "destroy", self.destroy )

		self.window.set_title( "Testing rc-interface serial comms" );

		self.window.set_border_width( 30 )

		self.window.set_modal( True )

		self.window.connect( "key-press-event", self.key_press_event )

		hbox = gtk.HBox( False, 10 )

		self.window.add( hbox )

		# controls frame

		frame = gtk.Frame( "Channels" )

		frame.set_shadow_type( gtk.SHADOW_IN )

		hbox.pack_start( frame )

		table = gtk.Table( 2, self.NUM_CHANNELS )

		table.set_border_width( 25 )

		table.set_row_spacings( 10 )

		table.set_col_spacings( 15 )

		frame.add( table )

		# add channels

		self.controls = []

		for a in range( self.NUM_CHANNELS ):

			label = gtk.Label( a + 1 )

			table.attach( label, a, a + 1, 1, 2 )

			adj = gtk.Adjustment( 0, 0, self.MAX_CHANNEL_VALUE, 1, 0, 0 )

			control = gtk.VScale( adj )

			control.set_draw_value( False )

			control.set_inverted( True )

			control.set_size_request( -1, 200 )

			control.set_sensitive( False )

			table.attach( control, a, a + 1, 0, 1 )

			self.controls.append( control )

		# motors frame

		frame = gtk.Frame( "Motors" )

		frame.set_shadow_type( gtk.SHADOW_IN )

		hbox.pack_start( frame )

		table = gtk.Table( 2, self.NUM_MOTORS )

		table.set_border_width( 25 )

		table.set_row_spacings( 10 )

		table.set_col_spacings( 15 )

		frame.add( table )

		# add motor controls

		self.motors = []

		for a in range( self.NUM_MOTORS ):

			label = gtk.Label( a + 1 )

			table.attach( label, a, a + 1, 1, 2 )

			adj = gtk.Adjustment( 0, 0, self.MAX_CHANNEL_VALUE, 1, 0, 0 )

			control = gtk.VScale( adj )

			control.set_draw_value( False )

			control.set_inverted( True )

			control.set_size_request( -1, 200 )

			table.attach( control, a, a + 1, 0, 1 )

			control.connect( "value-changed", self.on_motor_change, a )

			self.motors.append( control )

		# info frame

		frame = gtk.Frame( "Info" )

		hbox.pack_start( frame )

		vbox = gtk.VBox()

		vbox.set_border_width( 10 )

		frame.add( vbox )

		self.add_info( vbox, True, "USB Device:" )

		self.add_info( vbox, False, self.SERIAL_DEVICE )

		self.add_info( vbox )

		if( self.link_error is not None ):

			self.add_info( vbox, True, "USB Error:" )

			label = gtk.Label( self.link_error )

			label.set_line_wrap( True )

			label.set_size_request( 150, -1 )

			vbox.pack_start( label, False )

		else:

			pass

		vbox.pack_start( gtk.Label( "" ) )

		# show it

		self.window.show_all()

	def on_motor_change( self, widget, motor ):

		self.motor_values[ motor ] = int( widget.get_value() )

		self.write_serial()

	def key_press_event( self, widget, event ):

		if event.keyval == gtk.keysyms.Escape:

			self.window.destroy()

		else:

			return False

	def destroy( self, widget, data = None ):

		self.quit = True

	def process_serial_data_at( self, pos ):

		for a in range( self.NUM_CHANNELS ):

			at = 2 * a + pos

			value = struct.unpack( "h", self.buffer[ at : at + 2 ] )[ 0 ]

			# update control

			self.controls[ a ].set_value( value )

	def read_serial( self ):

		# serial problem?

		if self.link is None: return

		# try and read

		in_waiting = self.link.inWaiting()

		if( in_waiting ):

			self.buffer.extend( self.link.read( in_waiting ) )

			# check buffer for double -1

			got = bytearray()

			i = 0

			while i < len( self.buffer ):

				# check foir double -1

				if i > 0 and self.buffer[ i ] == 255 and \

						self.buffer[ i - 1 ] == 255:

					# if we've got enough data, process it

					start_index = i - ( self.NUM_CHANNELS * 2 + 1 )

					if start_index >= 0:

						self.process_serial_data_at( start_index )

					# remove everything up to and including this double -1 from

					# the serial buffer

					self.buffer = self.buffer[ i + 1 : ]

					i = 0

				else:

					i += 1

	def write_serial( self ):

		# serial problem?

		if self.link is None: return

		# construct message

		bytes = bytearray()

		bytes.extend( struct.pack( "h", -1 ) )

		for i in range( self.NUM_MOTORS ):

			bytes.extend( struct.pack( "h", self.motor_values[ i ] ) )

		# send

		self.link.write( bytes )

	def main( self ):

		self.setup_window()

		while not self.quit:

			# update gtk

			while gtk.events_pending():

				gtk.main_iteration( False )

			# check serial interface

			self.read_serial()

MainDialogue().main()