/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

	# maximum channel value

	MAX_CHANNEL_VALUE = 1000

	# serial device to use

	SERIAL_DEVICE = '/dev/ttyUSB0'

	def __init__( self ):

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

		self.got_one = False

		self.channel_values = []

		self.quit = False

		self.buffer = bytearray()

	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 )

		# add widgets

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

		table.set_row_spacings( 20 )

		table.set_col_spacings( 10 )

		self.window.add( table )

		# add channels

		self.controls = []

		for a in range( 0, self.NUM_CHANNELS ):

			label = gtk.Label( "Channel %d" % ( a + 1 ) )

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

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

			control = gtk.HScale( adj )

			control.set_draw_value( False )

			control.set_property( 'width-request', 300 )

			control.set_sensitive( False )

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

			self.controls.append( control )

		# show it

		self.window.show_all()

	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( 0, 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 ):

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