#!/usr/bin/perl -w
# 
# This file is licensed under the GPL.  Please see the file "LICENSE" in
# the root directory of this project for terms and conditions.
# (c) 2001 by Trammell Hudson <hudson@swcp.com>
#
use strict;
use FindBin;
use lib "$FindBin::Bin/lib";

use Tk;
use Joystick;
use Servo;
use Controls;

my $version = q$Id: flyer,v 1.5 2001/10/14 03:24:07 tramm Exp $;


my $mw		= MainWindow->new(
	-title		=> 'UAV Flyer',
);

my $js		= Joystick->new
	or die "Unable to open joystick: $!\n";

my $left_stick	= Controls->new( $mw->Frame->pack( -side => 'left' ) );
my $right_stick	= Controls->new( $mw->Frame->pack( -side => 'left' ) );

$mw->fileevent( $js->{dev}, readable => sub { $js->update } );

Servo->new( 255, '/dev/ttyS0' );

my %controls = (
	lr_cyclic	=> {
		servo		=> Servo->new( 0 ),
		trim		=> 0,
		axis		=> 0,
		range		=>  24000,
		scale		=> -1,
		label		=> 'Left/Right Cyclic',
		stick		=> $right_stick,
		stick_axis	=> 'x',
	},

	fa_cyclic	=> {
		servo		=> Servo->new( 1 ),
		trim		=> 0,
		axis		=> 1,
		range		=>  21000,
		scale		=> -1,
		label		=> 'Fore/Aft Cyclic',
		stick		=> $right_stick,
		stick_axis	=> 'y',
	},

	tr		=> {
		servo		=> Servo->new( 3 ),
		trim		=> 0,
		axis		=> 2,
		range		=>  18000,
		scale		=> -1,
		label		=> 'Anti-torque',
		stick		=> $left_stick,
		stick_axis	=> 'x',
	},

	throttle	=> {
		servo		=> Servo->new( 2 ),
		trim		=> 0,
		axis		=> 3,
		range		=>  20000,
		scale		=> -1,
		label		=> 'Throttle',
		stick		=> $left_stick,
		stick_axis	=> 'y',
	},
		
);

while( my ($control,$data) = each %controls )
{
	$js->watch_axis( $data->{axis}, sub {
		my $js		= shift;
		my $value	= shift;

		$data->{latch}	= 0 if $value == 0;

		return if $data->{latch};

		my $setting	= $data->{scale} * 128 * $value / $data->{range};
		$setting	= int( $setting + $data->{trim} );

		#print "$control: $value -> $setting\n";
		$data->{servo}->set( $setting );
		$data->{stick}->set( $data->{stick_axis} => $setting );
	} );
}

# Trigger "latches" cyclic and tail rotor
$js->watch_button( 0, sub {
	my $js		= shift;
	my $value	= shift;

	return if $value == 0;

	set_trim( $_, $js ) for qw/ tr lr_cyclic fa_cyclic /;

} );

# Button #2 "latches" just the tail rotor
$js->watch_button( 1, sub {
	my $js		= shift;
	my $value	= shift;

	return if $value == 0;
	set_trim( tr => $js );
} );


sub set_trim
{
	my $field	= shift;
	my $js		= shift;

	my $data	= $controls{$field};
	return if $js->axis( $data->{axis} ) == 0;

	$data->{latch}	= 1;
	$data->{trim}	= $data->{servo}->current;
	$data->{stick}->trim( $data->{stick_axis} => $data->{trim} );

	print "Latching $field at $data->{trim}\n";
}

# Now setup the cyclic trim settings
for ([ lr_cyclic => 4 ], [ fa_cyclic => 5 ])
{
	my ($c,$axis)	= @$_;
	my $data	= $controls{$c};

	$js->watch_axis( $axis, sub {
		my $js		= shift;
		my $value	= shift;

		return if $value == 0;

		my $incr	= $data->{scale} * ( $value <=> 0 );
		$data->{trim}	+= $incr;

		my $setting	= $incr + $data->{servo}->current;

		$data->{servo}->set( $setting );
		$data->{stick}->trim( $data->{stick_axis} => $data->{trim} );
		$data->{stick}->set( $data->{stick_axis} => $setting );
	} );
}


# Engine kill
$js->watch_button( 5, sub {
	my $js		= shift;
	my $value	= shift;

	$controls{throttle}->{servo}->set( 0 );
} );


MainLoop;
__END__
