#!/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 EFIS;
use Servo;
use Controls;

my $version = q$Id: test-efis,v 1.6 2001/10/29 00:41:14 tramm Exp $;

my $mw = new MainWindow;

my $efis = EFIS->new( $mw->Frame->pack( -side => 'left' ) );

my $lr = Servo->new( 0 );

my $fa = Servo->new( 2 );

my $sticks	= Controls->new(
	$mw->Frame->pack( -side => 'right' ),
	$lr, $fa
);

my $tr		= Servo->new( 3 );

my $throttle		= Servo->new( 4 );


=head Cyclic controls

      Move:   Trim:
        k       K     
      h   l   H   L   
        j       J

p: Reset
P: Set all trim to the current position

=cut

$mw->bind( '<Key-H>', sub { $lr->trim( $lr->trim + 2 ) } );
$mw->bind( '<Key-J>', sub { $fa->trim( $fa->trim + 2 ) } );
$mw->bind( '<Key-K>', sub { $fa->trim( $fa->trim - 2 ) } );
$mw->bind( '<Key-L>', sub { $lr->trim( $lr->trim - 2 ) } );

$mw->bind( '<Key-p>', sub { $lr->reset; $fa->reset } );
$mw->bind( '<Key-P>', sub {
	$lr->trim( $lr->current );
	$fa->trim( $fa->current );
} );

$mw->bind( '<Key-h>', sub {
	$lr->left();
	$efis->set_roll( $lr->current );
} );

$mw->bind( '<Key-j>', sub {
	$fa->left();
	$efis->set_pitch( $fa->current );
} );

$mw->bind( '<Key-k>', sub {
	$fa->right();
	$efis->set_pitch( $fa->current );
} );

$mw->bind( '<Key-l>', sub {
	$lr->right();
	$efis->set_roll( $lr->current );
} );



=head Anti-torque controls

a: Trim nose to the left
s: Yaw nose to the left
d: Center tail rotor
f: Yaw nose to the right
g: Trim nose to the right

=cut

$mw->bind( '<Key-a>', sub { $tr->trim( $tr->trim - 1 ); $tr->sync } );
$mw->bind( '<Key-d>', sub { $tr->left( 1 ) } );
$mw->bind( '<Key-s>', sub { $tr->reset() } );
$mw->bind( '<Key-f>', sub { $tr->right( 1 ) } );
$mw->bind( '<Key-g>', sub { $tr->trim( $tr->trim + 1 ); $tr->sync } );


=head Throttle controls

q: Decrease throttle
w: Increase throttle
Q: Kill engine
=cut

$mw->bind( '<Key-q>', sub {
	$throttle->left();
	$efis->set_altitude( $throttle->current );
} );

$mw->bind( '<Key-w>', sub {
	$throttle->right();
	$efis->set_altitude( $throttle->current );
} );

$mw->bind( '<Key-Q>', sub {
	$throttle->reset();
	$efis->set_altitude( '0' );
} );

my $gyro = FileHandle->new( "</dev/ttyS0" )
	or die "Unable to open /dev/ttyS0: $!\n";
$gyro->autoflush();

$mw->fileevent( $gyro => readable => sub {
	local $_ = <$gyro>;
	my ($roll,$pitch,$yaw) =
		/R([0-9a-f]+) P([0-9a-f]+) Y([0-9a-f]+)/
		or return;

	$roll	= hex $roll;	$roll	-= 0x8000;	$roll	/= 64;
	$pitch	= hex $pitch;	$pitch	-= 0x8000;	$pitch	/= 64;
	$yaw	= hex $yaw;	$yaw	-= 0x8000;	$yaw	/= 64;

	$efis->set_roll( $roll );
	#$efis->set_pitch( $pitch );
	#$efis->set_yaw( $yaw );
} );

MainLoop;
