#!/usr/bin/perl
#!/usr/bin/perl -d:ptkdb

###############################################################################
#
# mkcomm.pl -  MK Communication Routines
#
# Copyright (C) 2009  Rainer Walther  (rainerwalther-mail@web.de)
#
# Creative Commons Lizenz mit den Zusaetzen (by, nc, sa)
#
# Es ist Ihnen gestattet: 
#     * das Werk vervielfältigen, verbreiten und öffentlich zugänglich machen
#     * Abwandlungen bzw. Bearbeitungen des Inhaltes anfertigen
# 
# Zu den folgenden Bedingungen:
#     * Namensnennung.
#       Sie müssen den Namen des Autors/Rechteinhabers in der von ihm festgelegten Weise nennen.
#     * Keine kommerzielle Nutzung.
#       Dieses Werk darf nicht für kommerzielle Zwecke verwendet werden.
#     * Weitergabe unter gleichen Bedingungen.
#       Wenn Sie den lizenzierten Inhalt bearbeiten oder in anderer Weise umgestalten,
#       verändern oder als Grundlage für einen anderen Inhalt verwenden,
#       dürfen Sie den neu entstandenen Inhalt nur unter Verwendung von Lizenzbedingungen
#       weitergeben, die mit denen dieses Lizenzvertrages identisch oder vergleichbar sind.
# 
# Im Falle einer Verbreitung müssen Sie anderen die Lizenzbedingungen, unter welche dieses
# Werk fällt, mitteilen. Am Einfachsten ist es, einen Link auf diese Seite einzubinden.
# 
# Jede der vorgenannten Bedingungen kann aufgehoben werden, sofern Sie die Einwilligung
# des Rechteinhabers dazu erhalten.
# 
# Diese Lizenz lässt die Urheberpersönlichkeitsrechte unberührt.
# 
# Weitere Details zur Lizenzbestimmung gibt es hier:
#   Kurzform: http://creativecommons.org/licenses/by-nc-sa/3.0/de/
#   Komplett: http://creativecommons.org/licenses/by-nc-sa/3.0/de/legalcode
#
###############################################################################
# 2009-02-21 0.0.1 rw created
# 2009-03-18 0.0.2 rw NC 0.14e
# 2009-04-01 0.1.0 rw RC1
# 2009-04-06 0.1.1 rw NC 0.15c
# 2009-05-16 0.1.2 rw External control
# 2009-08-15 0.1.3 rw SIG-Handler removed
# 2009-10-05 0.1.4 rw COM-Ports > 9 + PortSetSkip configuration
#                     Export Target-Hash to Simulator
# 2009-10-24 0.3.0 rw NC 0.17b
# 2010-02-10 0.4.0 rw Port read timout reduced from 100ms to 10ms
#                     Serial Channel
#                     Extern Control
#                     Controls via joystick or 3D-Mouse
#                     Navidata Current, Capacity
# 2010-02-18 0.4.1 rw LCD Stick/Poti^
# 2010-03-07 0.4.2 rw MKFLAG: LOWBAT
# 2010-07-01 0.5.0 rw WP/POI adjustments for NC 0.19/0.20
#                     Hardware Error Codes
#
###############################################################################

$Version{'mkcomm.pl'} = "0.5.0 - 2010-07-01";

# MK Protokoll
#   http://www.mikrokopter.de/ucwiki/en/SerialCommands?highlight=(command)
#   http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol)

#
# Parameter
#

# Com Port of MK Comm-Device (BT, WI.232)
if ( ! defined $Cfg->{'mkcomm'}->{'Port'} )
    {
    # set default
    $Cfg->{'mkcomm'}->{'Port'} = "COM5";
    }

$AddrFC     = "b";
$AddrNC     = "c";
$AddrMK3MAG = "d";


if ( $0 =~ /mkcomm.pl$/i )
    {
    # Program wurde direkt aufgerufen

    # change working directory to program path
    my $Cwd = substr ($0, 0, rindex ($0, "mkcomm.pl"));
    chdir $Cwd;

    # set path for local Perl libs
    push @INC, $Cwd . "perl/lib";
    }


# Packages
use threads;                # http://search.cpan.org/~jdhedden/threads-1.72/threads.pm
                            # http://perldoc.perl.org/threads.html
use threads::shared;        # http://search.cpan.org/~jdhedden/threads-shared-1.28/shared.pm
use Thread::Queue;          # http://search.cpan.org/dist/Thread-Queue-2.11/lib/Thread/Queue.pm
use Time::HiRes qw(usleep); # http://search.cpan.org/~jhi/Time-HiRes-1.9719/HiRes.pm
if ( $^O =~ /Win32/i )
    {
    require Win32::SerialPort;  # http://search.cpan.org/dist/Win32-SerialPort
    }
else
    {
    require Device::SerialPort; # http://search.cpan.org/~cook/Device-SerialPort-1.04/SerialPort.pm
    }

require "libmap.pl";

# Hashes exported to other threads and main-program
share (%MkOsd);
share (%MkTarget);
share (%MkNcDebug);
share (%Mk);
share (%MkSSim);     # Target info for simulator
share (%MkSerialChannel);


$Mk{'_RxCrcError'} = 0;    # statistics

# Queue for Sending to MK
$MkSendQueue = Thread::Queue->new();

sub MkCommExit()
    {	
    # close COM port
    &MkClose();

    if ( defined threads->self() )
        {
        threads->exit();
        }
    exit;
    }


sub MkInit()
    {
    if ( defined $MkPort )
        {
        return;   # already open
        }

    # open COM-Port		
     my $MkComPort = $Cfg->{'mkcomm'}->{'Port'};
     if ( $MkComPort =~ /^COM/i )
         {
         $MkComPort = "\\\\.\\" . $MkComPort;   # \\.\COMnn for nn > 9
         }
    undef $MkPort;
    if ( $^O =~ m/Win32/ )
        {
        $MkPort = Win32::SerialPort->new ($MkComPort) || die "Error open $MkComPort\n";
        }
    else
        {
        $MkPort = Device::SerialPort->new ($MkComPort) || die "Error open $MkComPort\n";
        }

    if ( ! ($Cfg->{'mkcomm'}->{'PortSetSkip'} =~ /y/i) )
        {
        # Set COM parameters, don't set for Bluetooth device
        $MkPort->baudrate(57600);
        $MkPort->parity("none");
        $MkPort->databits(8);
        $MkPort->stopbits(1);
        $MkPort->handshake('none');
        $MkPort->write_settings;
        }
	
    $MkPort->read_const_time(10); # total = (avg * bytes) + const (ms)
    }

# Read one line from MK
# Check send-queue
sub MkIOLine()
    {
    # Init serial port
    &MkInit();
	
    my $RxLine = "";		
    while ( 1 )
        {	
        # Check Send-Queue
        my $Items = $MkSendQueue->pending();
        if ( $Items >= 3 )  # Cmd, Addr, Data
            {
            my ($Id, $Addr, $Data) = $MkSendQueue->dequeue(3);
            &MkSend ($Id, $Addr, $Data);	
            }
		
        # Zeichenweise lesen, blockierend mit Timeout
        my ($RxLen, $RxChar) = $MkPort->read(1);   
        if ( $RxLen == 1 )
            {
            $Mk{'_BytesRx'} ++;    # Statistics

            if ( "$RxChar" eq "#" )       # 1st char of line
                {
                $RxLine = "#";
                }
            elsif ( "$RxChar" eq "\r" )   # last char of line
                {
                return ($RxLine);
                }
            else
                {
                $RxLine = "$RxLine" . "$RxChar";    # collect char
                }
            }
        }
    }


# Read and decode a command from MK
# process send queue in &MkIOLine()
sub MkIO()
    {	
    my $RxData = &MkIOLine();     # Blocking Read for complete line
	
    # Zeile decodieren
    if ( substr ($RxData, 0, 1) eq '#' )
        {			
        # Zeile decodieren
        $Header = substr($RxData, 0, 3);
        $Chksum = substr($RxData, -2);
        $Data = substr($RxData, 3, length ($RxData) -5);

# print "$Header\n";

        # CRC prüfen
        if ( &CrcCheck ("$Header" . "$Data", $Chksum ) )
            {
            # Base64 decodieren
            $Data = &Decode64($Data);

            # Daten auswerten und in shared Hash schreiben
            if ( &ProcessRx($Header, $Data) )
                {
                return 1;    # alles OK
                }
            }
        else
            {
            $Mk{'_RxCrcError'} ++;    # Statistics
            }
        }
		
    return 0;  # keine Daten empfangen
    }


# Send a command to MK
sub MkSend()
    {
    my ($Id, $Addr, $Data) = @_;

    # Init serial port
    &MkInit();

    my $Base64Data = &Encode64($Data);

    my $TxData = "#" . "$Addr" . "$Id" . "$Base64Data"; 
    my $Crc = &Crc($TxData);
    my $TxSend	= "$TxData" . "$Crc" . "\r";

    $Mk{'_BytesTx'} += length $TxSend;     # Statistics

    $MkPort->write($TxSend);
    }


# close COM-Port
sub MkClose()
    {
    undef $MkPort;
    }

	
# CRC Prüfung
sub CrcCheck ()
    {
    my ($Data, $Crc) = @_;

    my $Check = &Crc($Data);
    if ( $Check ne $Crc )
        {
        return 0;   # CRC passt nicht
        }	   
    return (1);    # CRC OK
    }

	
# CRC berechnen
sub Crc ()
    {
    my ($Data) = @_;
    my $TmpCrc = 0;
    my $Len = length $Data;

    for ($i=0; $i<$Len; $i++)
        {
        $TmpCrc += ord(substr($Data, $i, 1));
        }
		
    $TmpCrc %= 4096;
    my $Crc1 = ord ("=") + $TmpCrc / 64;
    my $Crc2 = ord ("=") + $TmpCrc % 64;
    $Crc = pack("CC", $Crc1, $Crc2);

    return ($Crc);
    }


# Empfangene Daten decodieren, modifiziertes Base64
sub Decode64()
    {
    my ($DataIn) = @_;

    my $ptrIn  = 0;
    my $DataOut  = "";
    my $len = length ($DataIn);

    while ( $len > 0 )
        {
        $a = ord (substr ($DataIn, $ptrIn ++, 1)) - ord ("=");
        $b = ord (substr ($DataIn, $ptrIn ++, 1)) - ord ("=");
        $c = ord (substr ($DataIn, $ptrIn ++, 1)) - ord ("=");
        $d = ord (substr ($DataIn, $ptrIn ++, 1)) - ord ("=");

        $x = ($a << 2) | ($b >> 4);
        $y = (($b & 0x0f) << 4) | ($c >> 2);
        $z = (($c & 0x03) << 6) | $d;

        foreach $i ( $x, $y, $z )
            {
            if ( $len--)
                {
                my $Tmp = pack ('C1', $i);
                $DataOut = "$DataOut" . "$Tmp";
                }
            else
                {
                last;
                }
            }
        }

    return ($DataOut);
    }

	
# zu sendende Daten codieren, modifiziertes Base64
sub Encode64()
    {
    my ($Data) = @_;

    my $Length = length $Data;
    my $TxBuf = "";
    my $ptr = 0;

    while( $Length > 0 )
        {
        my $a = 0;
        my $b = 0;
        my $c = 0;
        if ($Length) {$a = ord(substr ($Data, $ptr++, $Length--));}
        if ($Length) {$b = ord(substr ($Data, $ptr++, $Length--));}
        if ($Length) {$c = ord(substr ($Data, $ptr++, $Length--));}

        my $ac = ord("=") +    ($a >> 2);
        my $bc = ord("=") + ( (($a & 0x03) << 4) | (($b & 0xf0) >> 4) );
        my $cc = ord("=") + ( (($b & 0x0f) << 2) | (($c & 0xc0) >> 6) );
        my $dc = ord("=") +    ($c & 0x3f);
        $TxBuf = "$TxBuf" . pack ("C4", $ac, $bc, $cc, $dc);
        }
    return ($TxBuf);
    }

	
# Empfangenen Datensatz verarbeiten
sub ProcessRx()
    {
    my ($Header, $Data) = @_;

    my $Adr = substr ($Header, 1, 1);    # b=FC, c=NC, d=MK3MAG
    my $Id  = substr ($Header, 2, 1);

    if ( $Id eq "O" )
        {
        #
        # OSD-Daten nach %MkOsd einlesen
        #
		
        # Struktur Datensatz:
        #     u8  Version                                     // version of the data structure
        #     GPS_Pos_t CurrentPosition;
        #     GPS_Pos_t TargetPosition;
        #     GPS_PosDev_t TargetPositionDeviation;
        #     GPS_Pos_t HomePosition;
        #     GPS_PosDev_t HomePositionDeviation;
        #     u8  WaypointIndex;                              // index of current waypoints running from 0 to WaypointNumber-1
        #     u8  WaypointNumber;                             // number of stored waypoints
        #     u8  SatsInUse;                                  // no of satellites used for position solution
        #     s16 Altimeter;                                  // hight according to air pressure
        #     s16 Variometer;                                 // climb(+) and sink(-) rate
        #     u16 FlyingTime;                                 // in seconds
        #     u8  UBat;                                       // Battery Voltage in 0.1 Volts
        #     u16 GroundSpeed;                                // speed over ground in cm/s (2D)
        #     s16 Heading;                                    // current flight direction in deg as angle to north
        #     s16 CompassHeading;                             // current compass value
        #     s8  AngleNick;                                  // current Nick angle in 1°
        #     s8  AngleRoll;                                  // current Rick angle in 1°
        #     u8  RC_Quality;                                 // RC_Quality
        #     u8  MKFlags;                                    // Flags from FC
        #     u8  NCFlags;                                    // Flags from NC
        #     u8  Errorcode;                                  // 0 --> okay
        #     u8  OperatingRadius                             // current operation radius around the Home Position in m
        #     s16 TopSpeed;                                   // velocity in vertical direction in cm/s
        #     u8  TargetHoldTime;                             // time in s to stay at the given target, counts down to 0 if target has been reached
        #     u8  RC_RSSI;                                    // Receiver signal strength (since version 2 added)
        #     s16 SetpointAltitude;                           // setpoint for altitude
        #     u8  Gas;                                        // for future use
	#     u16 Current;                                    // actual current in 0.1A steps
	#     u16 UsedCapacity;                               // used capacity in mAh

        
        # GPS_Pos_t:
        #     s32 Longitude;   // in 1E-7 deg
        #     s32 Latitude;    // in 1E-7 deg
        #     s32 Altitude;    // in mm
        #     u8  Status;      // validity of data

        # GPS_PosDev_t:
        #     s16 Distance;                                   // distance to target in dm
        #     s16 Bearing;                                    // course to target in deg      

        # Status:
        #     INVALID         = 0
        #     NEWDATA         = 1
        #     PROCESSED       = 2
		
        # MKFlags  0x01: MOTOR_RUN, 0x02 FLY, 0x04: CALIBRATE, 0x08: START, 0x10: EMERGENCY_LANDING
        #          0x20: LOWBAT, 0x40: SPI_RX_ERR
        # NCFlags  0x01: FLAG_FREE, 0x02: FLAG_PH, 0x04: FLAG_CH, 0x08: FLAG_RANGE_LIMIT
        #          0x10: FLAG_NOSERIALLINK, 0x20: FLAG_TARGET_REACHED, FLAG_MANUAL_CONTROL: 0x40
        #          0x80: FLAG_8
	 	    
        lock (%MkOsd);  # until end of Block
		
        (
        $MkOsd{'Version'},
        $MkOsd{'CurPos_Lon'},
        $MkOsd{'CurPos_Lat'},
        $MkOsd{'CurPos_Alt'},
        $MkOsd{'CurPos_Stat'},
        $MkOsd{'TargetPos_Lon'},
        $MkOsd{'TargetPos_Lat'},
        $MkOsd{'TargetPos_Alt'},
        $MkOsd{'TargetPos_Stat'},
        $MkOsd{'TargetPosDev_Dist'},
        $MkOsd{'TargetPosDev_Bearing'},
        $MkOsd{'HomePos_Lon'},
        $MkOsd{'HomePos_Lat'},
        $MkOsd{'HomePos_Alt'},
        $MkOsd{'HomePos_Stat'},
        $MkOsd{'HomePosDev_Dist'},
        $MkOsd{'HomePosDev_Bearing'},
        $MkOsd{'WaypointIndex'},
        $MkOsd{'WaypointNumber'},
        $MkOsd{'SatsInUse'},
        $MkOsd{'Altimeter'},
        $MkOsd{'Variometer'},
        $MkOsd{'FlyingTime'},
        $MkOsd{'UBat'},
        $MkOsd{'GroundSpeed'},
        $MkOsd{'Heading'},
        $MkOsd{'CompassHeading'},
        $MkOsd{'AngleNick'},
        $MkOsd{'AngleRoll'},
        $MkOsd{'RC_Quality'},
        $MkOsd{'MKFlags'},
        $MkOsd{'NCFlags'},
        $MkOsd{'Errorcode'},
        $MkOsd{'OperatingRadius'},
        $MkOsd{'TopSpeed'},
        $MkOsd{'TargetHoldTime'},
        $MkOsd{'RC_RSSI'},
        $MkOsd{'SetPointAltitude'},
        $MkOsd{'Gas'},
        $MkOsd{'Current'},
        $MkOsd{'UsedCapacity'},
        ) = unpack ('ClllClllCsslllCssCCCssSCSssccCCCCCsCCsCSS', $Data); 
		
        $MkOsd{'CurPos_Lon'}     = sprintf("%.7f", $MkOsd{'CurPos_Lon'} / 10000000);
        $MkOsd{'CurPos_Lat'}     = sprintf("%.7f", $MkOsd{'CurPos_Lat'} / 10000000);
        $MkOsd{'CurPos_Alt'}     = sprintf("%.3f", $MkOsd{'CurPos_Alt'} / 1000);
        $MkOsd{'TargetPos_Lon'}  = sprintf("%.7f", $MkOsd{'TargetPos_Lon'} / 10000000);
        $MkOsd{'TargetPos_Lat'}  = sprintf("%.7f", $MkOsd{'TargetPos_Lat'} / 10000000);
        $MkOsd{'TargetPos_Alt'}  = sprintf("%.3f", $MkOsd{'TargetPos_Alt'} / 1000);
        $MkOsd{'HomePos_Lon'}    = sprintf("%.7f", $MkOsd{'HomePos_Lon'} / 10000000);
        $MkOsd{'HomePos_Lat'}    = sprintf("%.7f", $MkOsd{'HomePos_Lat'} / 10000000);
        $MkOsd{'HomePos_Alt'}    = sprintf("%.3f", $MkOsd{'HomePos_Alt'} / 1000);
        $MkOsd{'UBat'}           = sprintf("%.1f", $MkOsd{'UBat'} / 10);
        $MkOsd{'Current'}        = sprintf("%.1f", $MkOsd{'Current'} / 10);
		
        # Timestamp, wann der Datensatz geschtieben wurde
        $MkOsd{'_Timestamp'} = time;
        $MkOsd{'_FrameCount'} ++;
        }
		
    elsif ( $Id eq "s" )
        {
        #
        # NC Target position in %MkTarget
        #
        # Datenstruktur:
        #    GPS_Pos_t Position;     // the gps position of the waypoint, see ubx.h for details
        #    s16 Heading;            // orientation, future implementation
        #    u8  ToleranceRadius;    // in meters, if the MK is within that range around the target, then the next target is 
        #    u8  HoldTime;           // in seconds, if the MK was once in the tolerance area around a WP,
        #                            // this time defines the delay before the next WP is triggered
        #    u8  Event_Flag;         // future emplementation
        #    u8  reserve[12];        // reserved
		
        lock (%MkTarget);  # until end of block
		
        (
        $MkTarget{'Pos_Lon'},
        $MkTarget{'Pos_Lat'},
        $MkTarget{'Pos_Alt'},
        $MkTarget{'Pos_Stat'},
        $MkTarget{'Heading'},
        $MkTarget{'ToleranceRadius'},
        $MkTarget{'HoldTime'},
        $MkTarget{'EventFlag'},
        ) = unpack ('lllCsCCC', $Data); 

        $MkTarget{'Pos_Lon'} = sprintf("%.7f", $MkTarget{'Pos_Lon'} / 10000000);
        $MkTarget{'Pos_Lat'} = sprintf("%.7f", $MkTarget{'Pos_Lat'} / 10000000);
        $MkTarget{'Pos_Alt'} = sprintf("%.3f", $MkTarget{'Pos_Alt'} / 1000);
	
        # Timestamp, wann der Datensatz geschrieben wurde
        $MkTarget{'_Timestamp'} = time;
        $MkTarget{'_FrameCount'} ++;
        }
	
    elsif ( $Id eq "W" )
        {
        #
        # Request new waypoint
        #
        # Datenstruktur:
        #    u8 Number of waypoint
        
        ($WpNumber) = unpack ('C', $Data);
 
        # keine Ahnung wofuer das gut sein soll

        # print "Request new Waypoint Number: $WpNumber\n";
	    
        }
		
    elsif ( $Id eq "V" )
        {
        #
        # Version
        #
        # Datenstruktur:
        #    u8 SWMajor
        #    u8 SWMinor
        #    u8 ProtoMajor
        #    u8 ProtoMinor
        #    u8 SWPatch
        #    u8 HardwareError[5]

        (
        $Mk{'SWMajor'},
        $Mk{'SWMinor'},
        $Mk{'ProtoMajor'},
        $Mk{'ProtoMinor'},
        $Mk{'SWPatch'},
        $Mk{'HardwareError1'},
        $Mk{'HardwareError2'},
        $Mk{'HardwareError3'},
        $Mk{'HardwareError4'},
        $Mk{'HardwareError5'},
        ) = unpack ('C10', $Data); 

        $Mk{'_Timestamp'} = time;
        $Mk{'_FrameCount'} ++;
        }

    elsif ( $Id eq "E" )
        {
        #
        # Error Text
        #
        # Datenstruktur:
        #    s8 ErrorMsg[25]

        $Mk{'ErrorMsg'} = unpack ('Z25', $Data); 
        }
	
    elsif ( $Id eq "D" )
        {
        #
        # NC Debug %MkNcDebug
        #
        # Datenstruktur:
        #    u8 Digital[2];
        #    u16 Analog[32];

        lock (%MkNcDebug);    # until end of block

        (
        $MkNcDebug{'Digital_00'},
        $MkNcDebug{'Digital_01'},
        $MkNcDebug{'Analog_00'},
        $MkNcDebug{'Analog_01'},
        $MkNcDebug{'Analog_02'},
        $MkNcDebug{'Analog_03'},
        $MkNcDebug{'Analog_04'},
        $MkNcDebug{'Analog_05'},
        $MkNcDebug{'Analog_06'},
        $MkNcDebug{'Analog_07'},
        $MkNcDebug{'Analog_08'},
        $MkNcDebug{'Analog_09'},
        $MkNcDebug{'Analog_10'},
        $MkNcDebug{'Analog_11'},
        $MkNcDebug{'Analog_12'},
        $MkNcDebug{'Analog_13'},
        $MkNcDebug{'Analog_14'},
        $MkNcDebug{'Analog_15'},
        $MkNcDebug{'Analog_16'},
        $MkNcDebug{'Analog_17'},
        $MkNcDebug{'Analog_18'},
        $MkNcDebug{'Analog_19'},
        $MkNcDebug{'Analog_20'},
        $MkNcDebug{'Analog_21'},		
        $MkNcDebug{'Analog_22'},
        $MkNcDebug{'Analog_23'},
        $MkNcDebug{'Analog_24'},
        $MkNcDebug{'Analog_25'},
        $MkNcDebug{'Analog_26'},
        $MkNcDebug{'Analog_27'},
        $MkNcDebug{'Analog_28'},
        $MkNcDebug{'Analog_29'},
        $MkNcDebug{'Analog_30'},
        $MkNcDebug{'Analog_31'},
        ) = unpack ('C2s32', $Data); 

        # Timestamp, wann der Datensatz geschrieben wurde
        $MkNcDebug{'_Timestamp'} = time;
        $MkNcDebug{'_FrameCount'} ++;
        }

    elsif ( $Id eq "B" )
        {
        #
        # External Control
        #
        # Datenstruktur:
        #    u8 ConfirmFrame;

        my ($ConfirmFrame) = unpack ('C5', $Data); 

        }
    elsif ( $Id eq "L" )
        {
        #
        # LCD Screen
        #
        # Datenstruktur:
        #    u8 Menuitem
        #    u8 MaxMenuItem
        #    char[80] Display Text

        my ($MenuItem, $MaxMenuItem, $LcdLine) = unpack ('CCA80', $Data);
        if ( $LcdLine =~ /Po1:\s*(\d+)\s*Po2:\s*(\d+)\s*Po3:\s*(\d+)\s*Po4:\s*(\d+)/i )
            {
            $Stick{'RcPoti1'} = $1;
            $Stick{'RcPoti2'} = $2;
            $Stick{'RcPoti3'} = $3;
            $Stick{'RcPoti4'} = $4;
            }
        elsif ( $LcdLine =~ /Po5:\s*(\d+)\s*Po6:\s*(\d+)\s*Po7:\s*(\d+)\s*Po8:\s*(\d+)/i )
            {
            $Stick{'RcPoti5'} = $1;
            $Stick{'RcPoti6'} = $2;
            $Stick{'RcPoti7'} = $3;
            $Stick{'RcPoti8'} = $4;
            }
        elsif ( $LcdLine =~ /Ni:\s*(-*\d+)\s*Ro:\s*(-*\d+)\s*Gs:\s*(-*\d+)\s*Ya:\s*(-*\d+)/i )
            {
            $Stick{'RcStickNick'} = $1;
            $Stick{'RcStickRoll'} = $2;
            $Stick{'RcStickGas'} = $3;
            $Stick{'RcStickGier'} = $4;
            }
        $Stick{'_RcTimestamp'} = time;
        $Stick{'_RcFrameCount'} ++;
        }
    else
        {
        print "Unknown Command: $Header $Data\n";
        }
    }	


# send Target or Waypoint to MK
sub MkFlyTo()
    {
    my %Param = @_;

    my $x               = $Param{'-x'};
    my $y               = $Param{'-y'};
    my $Lat             = $Param{'-lat'};
    my $Lon             = $Param{'-lon'};
    my $Alt             = $Param{'-alt'};
    my $Heading         = $Param{'-heading'}; # 0..360: Heading, <0: POI-Index, >360: Invalid
    my $ToleranceRadius = $Param{'-toleranceradius'};
    my $Holdtime        = $Param{'-holdtime'};
    my $EventFlag       = $Param{'-eventflag'};
    my $Mode            = $Param{'-mode'};
    my $Index           = $Param{'-index'};   # 1..n (dummy ... will be overwritten in NC:uart1.c)
    my $Type            = $Param{'-type'};    # 0=WP, 1=POI

    if ( $x ne ""  and  $y ne ""  and  $Lat eq ""  and  $Lon eq "" )
        {
        ($Lat, $Lon) = &MapXY2Gps($x, $y);
        }
	
    if ( $Alt eq "" )             { $Alt             = $MkOsd{'CurPos_Alt'}; }
    if ( $Heading eq "" )         { $Heading         = $Cfg->{'waypoint'}->{'DefaultHeading'}; }
    if ( $ToleranceRadius eq "" ) { $ToleranceRadius = $Cfg->{'waypoint'}->{'DefaultToleranceRadius'}; }
    if ( $Holdtime eq "" )        { $Holdtime        = $Cfg->{'waypoint'}->{'DefaultHoldtime'}; }
    if ( $EventFlag eq "" )       { $EventFlag       = $Cfg->{'waypoint'}->{'DefaultEventFlag'}; }

    my $Status = 1;     # valid
    if ( $Mode =~ /delete/i )
        {
        $Status = 0;    # invalid -> delete NC WP-List
        $Index = -1;    # required from NC0.19 onward
        }
	
    my $Lat_i = sprintf "%d", $Lat * 10000000;
    my $Lon_i = sprintf "%d", $Lon * 10000000;
    my $Alt_i = sprintf "%d", $Alt * 1000;

    # Datenstruktur:
    #    GPS_Pos_t Position;     // the gps position of the waypoint, see ubx.h for details
    #    s16 Heading;            // orientation, future implementation
    #    u8  ToleranceRadius;    // in meters, if the MK is within that range around the target, then the next target is 
    #    u8  HoldTime;           // in seconds, if the MK was once in the tolerance area around a WP,
    #                            // this time defines the delay before the next WP is triggered
    #    u8  Event_Flag;         // future emplementation
    #    u8  Index;              // to indentify different waypoints, workaround for bad communications PC <-> NC
    #    u8  Type;               // typeof Waypoint (0=WP, 1=POI)
    #    u8  reserve[10];        // reserved

    my $Wp = pack ('lllCsC15', 
                   $Lon_i,
                   $Lat_i,
                   $Alt_i,
                   $Status,
                   $Heading,
                   $ToleranceRadius,
                   $Holdtime,
                   $EventFlag,
                   $Index + 1,
                   $Type,
                   0,0,0,0,0,0,0,0,0,0,
                  );

    if ( $Mode =~ /waypoint/i )
        {
        $MkSendQueue->enqueue( "w", "$AddrNC", $Wp );
        # &MkSend( "w", "$AddrNC", $Wp );
        }
    elsif ( $Mode =~ /target/i )
        {
        $MkSendQueue->enqueue( "s", "$AddrNC", $Wp );
        # &MkSend( "s", "$AddrNC", $Wp );

        # set Target information for Simulator
        $MkSim{'Target_Lat'} = $Lat;
        $MkSim{'Target_Lon'} = $Lon;
        $MkSim{'Target_Alt'} = $Alt;
        $MkSim{'Target_Status'} = $Status;
        $MkSim{'Target_Heading'} = $Heading;
        $MkSim{'Target_ToleranceRadius'} = $ToleranceRadius;
        $MkSim{'Target_Holdtime'} = $Holdtime;
        $MkSim{'Target_EventFlag'} = $EventFlag;

        # Timestamp, wann der Datensatz geschtieben wurde
        $MkSim{'_Timestamp'} = time;
        }
    else
        {
        # ignore
        }

    return 0;
    }


# send External control to MK
sub SendExternalControl()
    {
    my %Param = @_;

    my $RemoteButtons = $Param{'-remotebuttons'};
    my $Nick   = $Param{'-nick'};
    my $Roll   = $Param{'-roll'};
    my $Gier   = $Param{'-gier'};
    my $Gas    = $Param{'-gas'};
    my $Hight  = $Param{'-hight'};
    my $Free   = $Param{'-free'};
    my $Frame  = $Param{'-frame'};
    my $Config = $Param{'-config'};

    # Datenstruktur:
    #    u8      Digital[2];
    #    u8      RemoteButtons;
    #    s8      Nick;
    #    s8      Roll;
    #    s8      Yaw;
    #    u8      Gas;
    #    s8      Height;
    #    u8      free;
    #    u8      Frame;
    #    u8      Config;

    # Config/Bit 0 and FC-Parameter ExternControl > 128:
    #    Nich/Roll/Yaw added to RC-Channel
    #    Gas wird auf max. RC-Gas begrenzt

    my $Ec = pack ('CCCcccCcCCC',
                   0, 0, 
                   $RemoteButtons,
                   $Nick,
                   $Roll,
                   $Gier,
                   $Gas,
                   $Hight,
                   $Free,
                   $Frame,       # Frame/Command counter, ungleich 0
                   $Config,
                  );

    $MkSendQueue->enqueue( "b", "$AddrFC", $Ec );
    # &MkSend( "b", "$AddrFC", $Ec );

    return 0;
    }


# send serial Channel values from %MkSerialChannel to MK/FC
sub SendSerialChannel()
    {

    # Datenstruktur:
    #    s8      Channel[12];

    lock (%MkSerialChannel);    # until end of block
    
    my $SP = pack ('c12', 
                   $MkSerialChannel{'SerialChannel01'},
                   $MkSerialChannel{'SerialChannel02'},
                   $MkSerialChannel{'SerialChannel03'},
                   $MkSerialChannel{'SerialChannel04'},
                   $MkSerialChannel{'SerialChannel05'},
                   $MkSerialChannel{'SerialChannel06'},
                   $MkSerialChannel{'SerialChannel07'},
                   $MkSerialChannel{'SerialChannel08'},
                   $MkSerialChannel{'SerialChannel09'},
                   $MkSerialChannel{'SerialChannel10'},
                   $MkSerialChannel{'SerialChannel11'},
                   $MkSerialChannel{'SerialChannel12'},
                  );

    $MkSendQueue->enqueue( "y", "$AddrFC", $SP );
    # &MkSend( "y", "$AddrFC", $SP );

    return 0;
    }


# when called as thread
sub MkCommLoop()
    {
    while (1)
       {
       &MkIO();
       }
    }
	

#
# Hauptprgramm
#	

if ( $0 =~ /mkcomm.pl$/i )
    {
    # Program wurde direkt aufgerufen
    &MkCommLoop();

    # should never exit
    }
	
1;

__END__

