| 0,0 → 1,918 |
|---|
| #!/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__ |
| |