-         
Ergebnis 1 bis 3 von 3

Thema: YDLIDAR X4 Problem Auswertung

  1. #1
    Benutzer Stammmitglied
    Registriert seit
    15.09.2012
    Beiträge
    71

    YDLIDAR X4 Problem Auswertung

    Anzeige

    LiFePo4 Akku selber bauen
    Hallo,

    ich verwende einen µC ATXMega ATXMega256A3BAU (32MHz), und versuche den Laser-Rotations-Scaner X4 von YDLIDAR auszuwerten.

    Dabei habe ich ein Problem bei dem ich einfach nicht weiter komme, und hoffe das mir hier jemand einen Tipp geben kann.
    Ich hatte bereits einen Tread bei https://www.mikrocontroller.net/topic/488656#new eröffnet. Leider blieb dieser unbeantwortet.

    Hallo,

    ich oute mich mal als Anfänger und Programmierleihe. Also bitte nicht
    erschlagen.

    Für ein Bastelprojekt versuche ich den Laser-Rotations-Scaner YDLIDAR X4
    auszuwerten. Leider klappt nicht alles so wie es soll. Das Programm
    läuft und es werden mir auch plausible Werte angezeigt. Auf das letzte
    halbe Grad Auflösung oder den letzten 1/4mm habe ich bewusst verzichtet.
    Zum ersten mal habe ich mich an einen Ringbuffer gewagt, dort vermute
    ich auch das Problem.

    Der Sensors Selbst sendet seine Daten kontinuierlich über UART (Baud
    128000).

    Vor jedem Schreibvorgang in den Buffer wird dieser auf 0 geprüft. Das
    ist manchmal nicht der Fall. Manchmal heißt: das es immer an einer
    anderen Stelle im Buffer vorkommt. Woher diese Werte kommen konnte ich
    nicht herausfinden. Da die Werte in dieser Reihenfolge oder an dieser
    Stelle zuvor nicht empfangen wurden. Den Empfang habe dafür mittels LA
    mitgeschrieben. Auch ein löschen, des Buffers, nach Abarbeitung eines
    vollständigen Datensatzes bis zum nächsten Header hat nichts geholfen.
    Auch habe mindestens 3 mal geprüft ob wirklich alle Pakete aus dem
    Datensatz abgearbeitet werden.
    Zweiter Punkt warum ich vermute das es am Ringbuffer liegt. Die
    CheckCode Auswertung (Prüfsumme) löst aus, aber nur wenn ich beim lesen
    aus dem Ringbuffer über das Bufferende komme. Und dann auch nur in ca.
    5% der Fälle.

    Ich Denke ich habe einen Fehler im Code, den ich einfach nicht finden
    kann.

    Anbei das Lauffähige Minimalprogramm. Ohne die Datei LCD, da hier nur
    ganz simple ein 0815 LCD angesteuert wird. Ohne LCD-Anzeige (ja mit
    delay...) tritt der Fehler häufiger auf...

    µC: ATXMega256A3BAU (32MHz)

    Ich hoffe jemand ist so nett und kann mir einen Tipp geben, wo ich den
    Fehler noch suchen kann.

    vielen Dank schonmal
    Mittlerweile habe ich noch etwas weiter gesucht. Wenn ich mir einen Fehlerhafte Auswertung einen Datensatzes mitschreibe und manuell auswerte, kann ich Feststellen das 6 Byte einfach verschwunden sind. Diese wurden aber eindeutig vom Sensor übertragen, dies habe ich mit einen LA mitgeschrieben. Bufferoverflow an der UART-Schnittstelle löst nicht aus.
    Für die Überprüfung habe ich mir eine Excel-Tabelle erstellt. Diese kann man hier leider nicht Hochladen. Daher muss ein Ausdruck reichen. Links ist der Ausgewertete Datensatz zu sehen, dieser ist eingerahmt. Rechts ist der empfangene Datensatz dargestellt, orange markiert sind die fehlenden Byte. Ich kann mir leider keinen Reim darauf machen wo diese hin sind.

    wäre schön wenn Jemand noch einen Tipp für mich hätte.

    Anbei das Datenblatt des Sensors, der Ausdruck (PDF) der Excel Auswertung, und das Programm. Wenn es irgendwo hilft...

    Danke schonmal





    YDLIDAR X4 Development Manual.pdf
    Winkelauswertung eines Datensatzes.pdf
    .c

  2. #2
    Erfahrener Benutzer Roboter Experte
    Registriert seit
    07.04.2015
    Beiträge
    756
    Wohlwollende Mecker:
    - Wenn Du so etwas herausgibst, tue dem Leser zumindest den Gefallen und schreibe bei Verwendung mehrerer UARTS, welche denn überhaupt das Problem ist (es scheint UART_C1 zu sein, hat mich 5 min. gekostet)
    - Man kann auch mehrere Codemodule verwenden. Ein einzelnes Modul bietet sich für so eine PeerToPeer-Kommunikation an. Ich habe gerade wieder drei mal durch den gesamten Sermon gescrollt, bis ich endlich irgendwo mittendrin die Ini-Routine der UART gefunden habe.
    - Du hast zwischen den eingehenden Zeichen (128kBaud) bei 32MHz ca. 2500 Befehlszyklen. Mit etwas Nebenbeschäftigung (andere Interrupts) wird eine Auswertung/ Statemachine in der RxD-ISR schnell eng. Nutze also entweder den DMA oder schreibe Deine eingehenden Daten in der RxD-ISR einfach nur in einen Fifo (einen Ringpuffer mit Lese- und Schreibindex), den Du mit einem langsamen (10..50ms) Low_Prio-Timer-Interrupt auswertest. Damit der RxD-Interrupt diese längerdauernde zyklische Poll-Auswertung unterbrechen kann, setzt Du ihn auf High_Prio.

    Letztlich lässt sich die Sache bei solchen Protokollen ohne die Abhängigkeit auf eine ISR besser und komfortabler im Debugger/Simulator testen. Ich z.B. habe den Sensor nicht und könnte Deinen Fehler maximal anhand eines Hexdumps Deiner Daten analysieren.

    Alte Programmiererweisheit: Länger als eine halbe Stunde auf Code zu starren, bringt nix. Meist hat man in der Zeit den passenden Test dazu bereits geschrieben.

  3. #3
    Benutzer Stammmitglied
    Registriert seit
    15.09.2012
    Beiträge
    71
    Danke für deine Antwort. Du hast Recht, ich hätte mich wirklich präziser ausdrücken sollen.
    Gern hätte ich jetzt die relevanten Teile aus dem Code hier mittels Codefenster dargestellt, aber leider ist die Eingabemaske so bescheiden das ich dann nichts mehr Tippen kann.

    Ja es ist die UART_C1 Schnittstelle. UART_F0 nutze ich nur um größere Datenmengen zu Fehlersuche am PC auswerten zu können.
    Einen Ringbuffer nutze ich bereits. In diesen wird per ISR geschrieben. In der Funktion "void X4_Auswertung (void)" wird ausgewertet, dies kann auch per Interrupt unterbrochen werden. Aufruf erfolgt in der Hauptschleife. Abspeicherung der Daten erfolgt Global.

    Relevanter Code aus der ISR für den Empfang:
    Code:
    if (X4_TypeCode == 0x81)
     {
     if (UART_X4_Buff[UART_X4_Rx_CNT] != 0)
     { X4_Err |= 0x01; //Buffer beim Schreiben nicht LEER, daher Fehler und Auswertung zu langsam
     }
     else
     { //UART_X4_Buff[UART_X4_Rx_CNT] = USARTC1.DATA; // da 5 Zeilen weiter unten, nicht genutzt
     }
    
     //gehört eigentlich in else
     UART_X4_Buff[UART_X4_Rx_CNT] = USARTC1.DATA;
    
     if (UART_X4_Rx_CNT >= (UART_X4_Buff_SIZE - 1)) //Zähler Überlauf verhindern
     { UART_X4_Rx_CNT = 0;
     }
     else
     { UART_X4_Rx_CNT++;
     }
    
     } //ENDE TypeCode == 0x81
    Der Sensor selbst sendet selbstständig nach dem Einschalten eine länger "welcomme Message" gefolgt vom "Helth Status". Es können 5 verschiedene Befehle gesendet werden. Antwort erfolgt in einen Übergeordneten Protokoll. Wurde der Befehl "start scanning" (0x60) gesendet, kann die Protokolllänge des Übergeordneten Protokolls ignoriert werden. Die nun folgenden Daten werden kontinuierlich vom Sensor übertragen, bis ein "stop" (0x65) gesendet wird. Markiert ist die kontinuierliche Datenübertragung durch "TypeCode= 0x81". Daher ist der Rest in der ISR auch irrelevant.
    Ein Datensatz, aus der kontinuierlichen Übertragung, besteht auch einen Header (10Byte) und verschieden vielen Datensätzen. Ein Datensatz besteht aus 2 Byte. Es gibt in der Regel 40 Datensätze, kann aber auch abweichend sein (z.B. 1, 20). Somit ist ein Durchschnittlicher Datensatz 90Byte lang.
    Diese werden in den Ringbuffer abgelegt. "UART_X4_Buff[UART_X4_Rx_CNT]"
    Die Funktion "void X4_Auswertung (void)" sucht nach dem Header (0xA5 & 0x5A) und fragt die Paketlänge ab "X4_AnzahlPakete".
    Als nächsten wird geprüft ob der Empfangszähler "UART_X4_Rx_CNT" bereits das Ende des Datensatzes erreicht hat (eigentlich größer ist als Empfangszähler, bzw kleiner als Empfangszähler "X4_AusWert_CNT").
    Die Einzelpakete werden in einer Schleife "while(((X4_Pakete_CNT + 1) < X4_AnzahlPakete) && (X4_AnzahlPakete > 1))" ausgewertet.
    Anschließend wird noch auf Plausibilität (CheckCode) geprüft und ggf. global abgespeichert.

    Mein Problem ist nun das es bei manchen Empfangenen Datensatz zu Fehlern kommt. Wenn dies der Fall ist, dann tritt es immer dann auf wenn der Datensatzanfang in der nähe des Bufferende liegt, und das Datensatzende in der nähe des Bufferanfang. Also der Datensatz mit einer länge von 90Byte das Ringbufferende überschreitet. Außerdem kommt es beim Schreiben in den Ringbuffer (ISR) zum Fehler das dieser nicht immer leer ist.
    Ich gehe davon aus das beides zusammen hängt.

    Als Test habe ich die gesamte Übertragung vom Sensor zum µC mitgeschrieben. Die Auswertung, inkl. des Lesens aus dem Buffer, wurde solange aufgezeichnet bis es zum Fehler im "checkCode" (Prüfsumme) kam. Im Excel konnte ich nun den Fehlerhaften Datensatz einzeln betrachten. Passend dazu habe ich den Datensatz aus der Aufzeichnung (gesamte Übertragung Sensor->µC) herausgefiltert.
    Man sieht das am Ende 6 Byte fehlen, die eigentlich übertragen wurden. Aber wohin sind diese verschwunden? Es kam zu keinen "Buffer Over Flow". Und warum sind diese Byte noch im Buffer und wurden nicht ausgewertet?
    Ich kann es mir nicht mehr erklären. Und Frage daher nach Hilfe.

    Anbei nochmal der Code zur Auswertung des Ringbuffer. Bereinigt um den Testcode.

    Code:
    		uint16_t	UART_X4_Massage_CNT = 0;					//Übergeordneter Counter für eine Nachricht, es kann Scan Command, Device Info und Healt Status unterschieden werden
    		uint16_t	UART_X4_Rx_CNT = 0;							//Counter für Empfangsschleife
    		uint8_t		UART_X4_Tx_CNT = 0;							//Zähler für Aufruf senden
    		
    		#define UART_X4_Buff_SIZE 320							//Größe des Empfangsbuffers /immer mit Ändern wenn Buffergröße geändert wird
    		uint8_t		UART_X4_Buff[UART_X4_Buff_SIZE];			//Empfangsbuffer
    		
    		#define X4_DATA_Size 100								//Buffergröße X4_DATA zwischenspeicher
    		signed int	X4_DATA_zwichenSP [2] [X4_DATA_Size];		//Empfangene Daten (Zwischenspeicher) [0] Winkel in 1° / [1] Entfernung in 1 mm
    
    		uint16_t	X4_DATA_Ausw [361];							//Auswertung der Entfernungsdaten. Stelle im Array entspricht Winkel in vollen Grad
    
    		uint8_t		X4_Final_WRT_CNT = 0;						//Zähler um errechnete Daten abzuspeichern. "UART_X4_Buff" muss immer um 2Byte geschoben werden, daher separater Zähler zum Abspeichern
    		
    		int16_t		X4_AusWert_CNT = 0;							//Zähler für Auswertung des Buffer der empfangenen Daten aus YDLidar X4 über UART (muss zur Prüfung auch kleiner 0 werden dürfen)
    
    		uint8_t		X4_Mode = 0;								//Empfangsmodus 0x00 = Single response / 0x01 = Continuous response / 0x02 & 0x03 gibt es nicht
    		uint8_t		X4_TypeCode = 0;							//Empfangen Typecode für Auswertung: 0x81 = Continuous response (Scan Command) / 0x04 = Single response (Device Info) / 0x06 = Single response (Health Status)
    		uint8_t		X4_StatusCode = 0;							//0x00 = running normally / 0x01 = device running / 0x02 = running incorrectly
    		uint16_t	X4_HW_Err = 0;								//Hardwarefehler vom X4 0x00 alles O.K. (ohne Spezifikation)
    		
    		uint8_t		X4_ZeroPacket = 0;							//erkanntes ZeroPacket der Nachricht
    		uint8_t		X4_AnzahlPakete = 0;						//Anzahl der Pakete in dieser Nachricht (bestimmt die Länge der Nachricht)	---offensichtlich liegt das MAXIMUM bei 40---
    		uint8_t		X4_Pakete_CNT = 0;							//erfasst die abgearbeiteten Pakete (nach dem Abarbeiten hochzählen, da erstes Paket bereits erledigt)
    		
    		uint16_t	X4_FSA = 0;									//first sample point in the sampled data
    		uint16_t	X4_LSA = 0;									//last sample point in the sampled data
    		uint16_t	X4_CheckCode = 0;							//check code of the current data packet uses a two-byte exclusive OR
    		uint16_t	X4_CheckCode_Ausw = 0;						//zur Berechnung der Empfangenen Daten mittels CheckCode.Hier werden die Daten XOR eingelesen
    		uint16_t	X4_Err = 0;									//Fehler 0x01 = Buffer RX Overflow (bereits bei Empfang Anzahl Pakete erkannt) / 0x02 = zu Empfangene Daten Pakete passen nicht in den Buffer / 0x04 Hardwarefehler (siehe X4_StatusCode & X4_HW_Err) / 0x08 UART Parity Error / 0x10 UART Buffer Overflow / 0x20 UART Frame Error / 0x40 CheckDIGIT Error FSA / 0x80 CheckDIGIT Error LSA / 0x100 X4_DATA zwischenspeicher zu klein, da Anzahl Pakete größer 80
    		float		X4_Zero_Winkel = 0;							//Speichert den Winkelwert aus ZeroPaket ab
    
    		float		X4_Korrekturwinkel = 0;						//Korrekturwinkel FSA aus Distancemessung
    		float		X4_Start_Winkel = 0;
    		float		X4_End_Winkel = 0;
    		float		X4_Dif_Winkel_LSN = 0;						//Differenzwinkel aus (Startwinkel und Endwinkel) geteielt durch Anzahl Pakete - 1
    void X4_Auswertung (void)
    {	//Fehler beim UART C1 Empfang erkennen (Hardwarfehler)
    	X4_Err |= ((USARTC1.STATUS & 0x1C) << 1);
    	
    	X4_ZeroPacket = 0;
    	X4_AnzahlPakete = 0;			//unnötige durchläufe verhindern
    	uint16_t	X4_Distance = 0;									//Zwischenspeicher für Distanzewert aus Entfangenen Daten
    
    		//diese schleife wird solange abgearbeitet bis die ganze Funktion verlassen wird "return". Aber nur wenn Typcode auf Entfernungsmessung steht
    		while(X4_TypeCode == 0x81)
    		{
    			uint16_t Dauerschleife = 0;
    
    			//Den Lesezähler so lange Hochzählen bis Startaufruf (170 & 85) (AA & 55) erkannt werden.
    			while( ! (	((X4_AusWert_CNT > 0) && (UART_X4_Buff[X4_AusWert_CNT - 1] == 0xAA)  && (UART_X4_Buff[X4_AusWert_CNT] == 0x55)) 
    					||   ((X4_AusWert_CNT == 0) && (UART_X4_Buff[UART_X4_Buff_SIZE - 1] == 0xAA) && (UART_X4_Buff[X4_AusWert_CNT] == 0x55))	)
    				 )
    			{
    				X4_AusWert_CNT_PLUS (1);	//im Buffer +1 um PaketHeader zu finden
    				
    				//Dauerschleife verhindern
    				Dauerschleife++;
    				if (Dauerschleife > (UART_X4_Buff_SIZE))
    				{
    					break;
    				}
    			}
    
    			//für nächste Abfrage des Buffer, noch um 1 Hochzählen
    			X4_AusWert_CNT_PLUS (1);
    
    			//Erkennung ob es ein ZeroPaket ist. Dann nur 1 Datenpaket und FSA=LSA=ZeroAngle
    			X4_ZeroPacket = UART_X4_Buff[X4_AusWert_CNT];
    			X4_AusWert_CNT_PLUS (1);
    
    			//LSN (Sample quantity) Anzahl Pakete erkennen
    			if (X4_AusWert_CNT != UART_X4_Rx_CNT)
    			{	X4_AnzahlPakete = UART_X4_Buff[X4_AusWert_CNT];
    			}
    			else
    			{	X4_AnzahlPakete = 0;							//Abbruch				
    			}			
    			X4_AusWert_CNT_PLUS (1);
    
    			//Prüfen ob alle Pakete im Buffer aufgenommen werden konnten (pro Paket 2 Byte + 10 Byte für den Header)
    			if (((X4_AnzahlPakete * 2) + 10) > UART_X4_Buff_SIZE)
    			{	
    				X4_Err |= 0x02;									//Bufferüberlauf da Anzahlpakete nicht vollständig aufgenommen werden kann/konnte
    			}
    
    			//Prüfung ob zu lesendes Paket schon vollständig empfangen wurde. Schreibzähler (UART_X4_Rx_CNT) muss um Anzahl Pakete + 10 (10 Byte Header -4 Byte Startaufruf/Auswertungen) höher liegen als Lesezähler (UART_X4_AusWert_CNT).
    			//Sonst sind die Daten nicht korrekt.
    			if ((X4_AusWert_CNT + (X4_AnzahlPakete * 2) + 6) <= (UART_X4_Buff_SIZE - 1))	//alle Pakete passen (ohne Überlauf) in den Buffer
    			{
    				if ( (((X4_AusWert_CNT + (X4_AnzahlPakete * 2) + 6) >= UART_X4_Rx_CNT) && (UART_X4_Rx_CNT >= X4_AusWert_CNT))
    					|| (X4_AnzahlPakete == 0)
    					)	//es wurden noch nicht alle Pakete empfangen (Ende des letzen Paketes liegt oberhalb des Empfangszähler UND Empfangszähler ist oberhalb Auswertezähler) 
    				{	
    					//Da Abbruch der Auswertung, wird der Auswertezähler wieder auf Ausgangswert für nächsten Startaufruf (170 & 85) zurückgesetzt
    					if (X4_AusWert_CNT >= 4)										//erkennung auf Zählerüberlauf 
    					{	X4_AusWert_CNT = X4_AusWert_CNT - 4;							//für nächsten Einsprung Zähler zurücksetzen um Startaufruf zu erkennen
    					} 
    					else
    					{	
    						X4_AusWert_CNT = UART_X4_Buff_SIZE - 1 + X4_AusWert_CNT - 4 + 1;	//für nächsten Einsprung Zähler zurücksetzen um Startaufruf zu erkennen
    					}
    
    					break;
    				}
    					
    			} 
    			else                                                                                    //alle Pakete passen nicht (ohne Überlauf) in den Buffer
    			{
    				//letzter Datensatz liegt hinter dem Empfangscounter (Ende des Datensatz nach Bufferüberlauf liegt hinter Empfangsbuffer) || Datensatzende ist 0 und Empfangszähler ist zwischen Datensatz Anfang/Ende
    				if ( (((X4_AusWert_CNT + (X4_AnzahlPakete * 2) + 6 - UART_X4_Buff_SIZE) >= UART_X4_Rx_CNT)
    					|| (((X4_AusWert_CNT + (X4_AnzahlPakete * 2) + 6 - UART_X4_Buff_SIZE) == 0) && (UART_X4_Rx_CNT >= X4_AusWert_CNT))
    					|| (X4_AnzahlPakete == 0))
    					) 
    				{		
    					//Da Abbruch der Auswertung, wird der Auswertezähler wieder auf Ausgangswert für nächsten Startaufruf (170 & 85) zurückgesetzt
    					if (X4_AusWert_CNT >= 4)										//erkennung auf Zählerüberlauf
    					{	X4_AusWert_CNT = X4_AusWert_CNT - 4;							//für nächsten Einsprung Zähler zurücksetzen um Startaufruf zu erkennen
    					}
    					else
    					{
    						X4_AusWert_CNT = UART_X4_Buff_SIZE - 1 + X4_AusWert_CNT - 4 + 1;	//für nächsten Einsprung Zähler zurücksetzen um Startaufruf zu erkennen
    					}
    
    					break;
    				}
    								
    			}
    
     			//Buffer (mit Inhalt Header) löschen, wenn kein return; / falls return; ausgelöst dann werden Daten für den nächsten Einsprung, die Headerdaten noch benötigt
    				//i=1 i<5 bedeutet: da Zähler bereits hochgezählt muss eine Stelle weiter vorn im Buffer gelöscht werden. 4 Durchläufe bis Startaufruf 170 nötig
    			for (int8_t i=1 ; i<5 ; i++)
    			{
    				if ((X4_AusWert_CNT - i) >= 0)
    				{	
    					UART_X4_Buff[(X4_AusWert_CNT - i)] = 0;						//Buffer löschen damit fehler beim beschreiben registriert werden				
    				} 
    				else
    				{	
    					UART_X4_Buff[(UART_X4_Buff_SIZE + X4_AusWert_CNT - i)] = 0;	//Buffer löschen damit fehler beim beschreiben registriert werden
    				}
    			}
    
    			//für CheckCode Auswertung Startaufruf eintragen (ist bei TypeCode=0x81 immer 0x55AA
    			X4_CheckCode_Ausw = 0x55AA;		//Startaufruf eintragen
    			
    			//Abspeichern der Winkeldaten
    			//FSA (FirstAngle) Startwinkel
    				//HighByte
    			X4_FSA = UART_X4_Buff[X4_AusWert_CNT];
    			UART_X4_Buff[X4_AusWert_CNT] = 0;						//Bufferlöschen damit fehler beim beschreiben registriert werden
    			X4_AusWert_CNT_PLUS (1);
    
    				//LowByte
    			X4_FSA |= (UART_X4_Buff[X4_AusWert_CNT] << 8);
    			UART_X4_Buff[X4_AusWert_CNT] = 0;						//Bufferlöschen damit fehler beim beschreiben registriert werden
    			X4_AusWert_CNT_PLUS (1);	
    
    				//Check DIGIT prüfen (muss 1 sein)
    				if (!(X4_FSA & 0x01))
    				{	X4_Err |= 0x40;
    				}
    
    				//CheckCode mit FSA Xor
    				X4_CheckCode_Ausw ^= X4_FSA;									//schreibe FSA für CheckCode Auswertung
    
    				//Anfangswinkel FSA (First_Angle) berechnen
    				X4_Start_Winkel = ((X4_FSA>>1)/64);
    
    
    			//LSA (LastAngle) Endwinkel
    				//HighByte
    			X4_LSA = UART_X4_Buff[X4_AusWert_CNT];
    			UART_X4_Buff[X4_AusWert_CNT] = 0;						//Bufferlöschen damit fehler beim beschreiben registriert werden
    			X4_AusWert_CNT_PLUS (1);
    
    				//LowByte
    			X4_LSA |= (UART_X4_Buff[X4_AusWert_CNT] << 8);
    			UART_X4_Buff[X4_AusWert_CNT] = 0;						//Bufferlöschen damit fehler beim beschreiben registriert werden
    			X4_AusWert_CNT_PLUS (1);
    
    				//Check DIGIT prüfen (muss 1 sein)
    				if (!(X4_LSA & 0x01))
    				{	X4_Err |= 0x80;
    				}
    
    				//Endwinkelberechnung (LSA)
    				X4_End_Winkel = ((X4_LSA>>1)/64);
    
    
    			//CheckCode empfangen
    				//HighByte
    			X4_CheckCode = UART_X4_Buff[X4_AusWert_CNT];
    			UART_X4_Buff[X4_AusWert_CNT] = 0;						//Bufferlöschen damit fehler beim beschreiben registriert werden
    			X4_AusWert_CNT_PLUS (1);
    
    				//LowByte
    			X4_CheckCode |= (UART_X4_Buff[X4_AusWert_CNT] << 8);
    			UART_X4_Buff[X4_AusWert_CNT] = 0;						//Bufferlöschen damit fehler beim beschreiben registriert werden
    			X4_AusWert_CNT_PLUS (1);
    
    			//Korrekturwinkel für Startwinkel aus Entfernung berechnen / nur beim ersten Datensatz / Wenn Distance = 0 dann ist auch der Winkel = 0
    				//erster Datensatz
    				X4_Distance = UART_X4_Buff[X4_AusWert_CNT];
    				UART_X4_Buff[X4_AusWert_CNT] = 0;												//Bufferlöschen damit fehler beim beschreiben registriert werden
    				X4_AusWert_CNT_PLUS (1);
    
    				X4_Distance |= (UART_X4_Buff[X4_AusWert_CNT] << 8);
    				UART_X4_Buff[X4_AusWert_CNT] = 0;												//Bufferlöschen damit fehler beim beschreiben registriert werden
    				X4_AusWert_CNT_PLUS (1);
    
    					//CheckCode mit ersten Datensatz Xor
    					X4_CheckCode_Ausw ^= X4_Distance;									//schreibe Datensatz 1 für CheckCode Auswertung
    			
    				if (X4_Distance == 0)
    				{	X4_Korrekturwinkel = 0;															//Wenn Distance Wert == 0 dann ist auch die Korrektur 0
    				} 
    				else
    				{	X4_Korrekturwinkel = atan(21.8*((155.3-X4_Distance)/(155.3 * X4_Distance)))*180/M_PI;	//Korrekturwinkel FSA (First_Angle) berechnen
    					//X4_Korrekturwinkel = (float)atan(21.8*((155.3-X4_Distance)/(155.3 * X4_Distance)))*180/M_PI;	//Korrekturwinkel FSA (First_Angle) berechnen
    				}
    
    			//Datenauswertung, wenn ZeroPaket dann nur einmal Daten
    			if (X4_ZeroPacket == 1)
    			{	//im Zeropaket werden die Daten für den Startwinkel übertragen. Wird zur Erkennung "start der Messung" benötigt. Ist Übersprung 359° (Startwinkle ist größer Endwinkel)
    				X4_Zero_Winkel = ((X4_LSA>>1)/64);
    			} 
    			else
    			{//kein ZeroPaket
     				X4_Pakete_CNT = 0;																	//vor der nutzung auf 0 setzen, sonst Fehler //erfasst die abgearbeiteten Pakete (nach dem Abarbeiten hochzählen, da erstes Paket bereits erledigt)
    				X4_Final_WRT_CNT = 0;			//für erste Paket auf 0 setzen
    
    				//Differenzwinkel geteielt durch Anzahl Pakete -1 (Grad Pro empfangenen Paket)
    					//hier nur einmal berechnen um nicht jedes mal (Anzahl Pakete / 40x) in der do..while zu berechnen
    				if ((X4_Start_Winkel == X4_Zero_Winkel)	&& (X4_End_Winkel < X4_Start_Winkel))											//Verhindert "blinden Felck" zwischen ~342° bis 359°
    				{	X4_Dif_Winkel_LSN = (360 - X4_Start_Winkel + X4_End_Winkel) / (X4_AnzahlPakete - 1);	//Startwinkel ist gleich Winkeldaten aus ZeroPaket (348°) und Endwinkel ist über 359°/ ist kleiner Startwinkel 
    				} 
    				else
    				{	X4_Dif_Winkel_LSN = (X4_End_Winkel - X4_Start_Winkel) / (X4_AnzahlPakete - 1);
    				}
    				
    				//Ersten Datensatz anders Berechnen, sonst Rechenfehler
    				X4_DATA_zwichenSP [1] [X4_Final_WRT_CNT] = (X4_Distance / 4);		//Entfernungsdaten in mm
    
    				X4_DATA_zwichenSP [0] [X4_Final_WRT_CNT] = (X4_Start_Winkel + X4_Korrekturwinkel);						//Winkeldaten in 1°
    				X4_Final_WRT_CNT++;
    
    				
    					//Datensätze bis zum Ende berechnen (Anzahl Pakete), danach Sprung nach oben und prüfen ob die nächsten Datensätze ausgewertet werden können (wenn vollständig empfangen)
    						//nicht durchlaufen wenn nur ein Datenpaket
    	
    					while(((X4_Pakete_CNT + 1) < X4_AnzahlPakete) && (X4_AnzahlPakete > 1))
    					{	
    						//zuerst die Entfernung berechnen
    						X4_Distance = UART_X4_Buff[X4_AusWert_CNT];
    						UART_X4_Buff[X4_AusWert_CNT] = 0;												//Bufferlöschen damit fehler beim beschreiben registriert werden
    						X4_AusWert_CNT_PLUS (1);
    					
    						X4_Distance |= (UART_X4_Buff[X4_AusWert_CNT] << 8);
    						UART_X4_Buff[X4_AusWert_CNT] = 0;												//Bufferlöschen damit fehler beim beschreiben registriert werden
    						X4_AusWert_CNT_PLUS (1);
    					
    							//CheckCode mit Datensatz XX Xor
    							X4_CheckCode_Ausw ^= X4_Distance;												//schreibe Datensatz # für CheckCode Auswertung
    						
    						//Berechnung des Korrekturwinkel
    						if (X4_Distance == 0)
    						{	X4_Korrekturwinkel = 0;														//Wenn Distance Wert == 0 dann ist auch die Korrektur 0
    						
    							X4_DATA_zwichenSP [1] [X4_Final_WRT_CNT] = 0;											//Entfernungsdaten in mm
    						}
    						else
    						{	X4_Korrekturwinkel = atan(21.8 *((155.3-X4_Distance)/(155.3 * X4_Distance))) * 57.295779;//*180/M_PI;	//Korrekturwinkel FSA (First_Angle) berechnen	(57.295779 = 180/M_PI)
    							//X4_Korrekturwinkel = (float)atan(21.8*((155.3-X4_Distance)/(155.3 * X4_Distance)))*180/M_PI;	//Korrekturwinkel FSA (First_Angle) berechnen
    						
    							X4_DATA_zwichenSP [1] [X4_Final_WRT_CNT] = (X4_Distance / 4);										//Entfernungsdaten in mm						
    						}
    
    						//Endwinkel wurde bereits berechnet, daher auswahl ob letzter Datensatz im Buffer
    							//Grad pro Paket:	((X4_End_Winkel - X4_Start_Winkel) / (X4_AnzahlPakete - 1))
    							//mal Aufsummieren:	* (X4_Pakete_CNT)
    						if ((X4_Pakete_CNT + 2) < X4_AnzahlPakete)
    						{	
    							X4_DATA_zwichenSP [0] [X4_Final_WRT_CNT] = ((X4_Dif_Winkel_LSN * (X4_Pakete_CNT)) + X4_Start_Winkel + X4_Korrekturwinkel);	//Winkeldaten in 1°
    						} 
    						else //Endwinkel
    						{	
    							X4_DATA_zwichenSP [0] [X4_Final_WRT_CNT] = (X4_End_Winkel + X4_Korrekturwinkel);											//Winkeldaten in 1°
    						}
    					
    						X4_Final_WRT_CNT++;
    					
    						if (X4_Final_WRT_CNT >= X4_DATA_Size)													//Anzahl Pakete größer 80
    						{	X4_Err |= 0x100;																	//Bufferüberlauf
    						}
    
    						//Paket abgearbeitet, hochzählen
    						X4_Pakete_CNT++;
    					}		//ENDE while (X4_Pakete_CNT + 1 weil ein Paket schon vor der Schleife ausgewertet wurde)
    
    				
    			//weiter berechnung CheckCode und Auswertung nur wenn alle Pakete verarbeitet wurden, nicht bei break;
    			if (((X4_Pakete_CNT + 1) == X4_AnzahlPakete) || (X4_AnzahlPakete == 1))
    			{	
    				X4_CheckCode_Ausw ^= (((int)X4_AnzahlPakete << 8) | (X4_ZeroPacket));					//schreibe CT und LSN für CheckCode Auswertung
    				X4_CheckCode_Ausw ^= X4_LSA;															//schreibe LSA für CheckCode Auswertung
    	
    				//auswerten ob CheckCode fehler im Empfang erkannt hat, oder CheckDigit FSA/LSA Fehlerhaft
    				if ((X4_CheckCode_Ausw != X4_CheckCode) || (X4_Err & 0xC0))
    				{	//Fehler
    
    					//lösche den gesameten Zwischenspeicher, da Fehlerhafter Empfang/Auswertung
    					for (uint8_t i = 0; i < X4_DATA_Size ; i++)
    					{	X4_DATA_zwichenSP [0] [i] = 0;
    						X4_DATA_zwichenSP [1] [i] = 0;
    					}
    				}
    				else
    				{	//kein Fehler Auswertung möglich
    					for (uint8_t i = 0; i < X4_AnzahlPakete ; i++)
    					{	//ArrayStelle enspricht vollem Grad und Entfernung in mm. Anzeige auflösung sowieso nur 1mm.
    						//Startwinkel kann wie normaler Winkel ausgewertet werden (Fehlt hier noch)
    						//Endwinkel ist anders zu berechnen (siehe weiter Oben)
    						//Alle Werte sind falsch, bis ZeroPaket empfangen wurde. Also Endwinkelkorrektur
    						//Prüfung bei Abspeichern auf < 0 und >360 noch machen
    
    						//Werte Größer 360 ° ausblenden
    						if (X4_DATA_zwichenSP [0] [i] < 360)
     						{
    							if (X4_DATA_zwichenSP [0] [i] < 0)										//negative Winkel (kleiner 0° = 360°-)
    							{	//dürfte es nicht mehr geben, daher prüfen und raus
    								X4_DATA_Ausw [(360 + (X4_DATA_zwichenSP [0] [i]))] = X4_DATA_zwichenSP [1] [i];		//Negative Winkel von 360° abziehen
    							} 
    							else
    							{	X4_DATA_Ausw [(X4_DATA_zwichenSP [0] [i])] = X4_DATA_zwichenSP [1] [i];				//Positive Winkelwerte
     							}						
     						}
    						else
    						{	//nur bei Startwinkel = 348°
    							X4_DATA_Ausw [(360 - (X4_DATA_zwichenSP [0] [i]))] = X4_DATA_zwichenSP [1] [i];
    						}
    				
    						//Daten aus Zwischenspeicher löschen
    						X4_DATA_zwichenSP [0] [i] = 0;
    						X4_DATA_zwichenSP [1] [i] = 0;
    					
    					} //ENDE for
    					
    				} //ENDE else (Empfang korrekt)		
    				
    			} //ENDE if (auswertung CheckCode nach vollständigen Empfang)
    			
    				X4_CheckCode_Ausw = 0;			//Rücksetzen
    				X4_Err &= 0xFF3F;				//CheckDIGIT Fehler Rücksetzen
    				
    		}//if (UART_X4_ZeroPacket == 1) else
    
    	}//ENDE Datenauswertung while(UART_X4_TypeCode == 0x81)
    
    }//ENDE Funktion "X4_Auswertung(void)"
    //ENDE Auswerten YDLidar-X4

Ähnliche Themen

  1. Problem mit PWM Auswertung am PIC12F509 in C
    Von Pilotzi im Forum PIC Controller
    Antworten: 11
    Letzter Beitrag: 12.02.2015, 20:16
  2. Auswertung von PIR
    Von PeruNova im Forum Sensoren / Sensorik
    Antworten: 8
    Letzter Beitrag: 04.03.2014, 20:46
  3. Auswertung
    Von oderlachs im Forum Software, Algorithmen und KI
    Antworten: 2
    Letzter Beitrag: 10.07.2012, 09:51
  4. PIR auswertung
    Von avus17 im Forum Sensoren / Sensorik
    Antworten: 5
    Letzter Beitrag: 29.08.2011, 13:06
  5. Problem bei Auswertung eines Ping))) Ultraschall Sensors
    Von daviid im Forum Basic-Programmierung (Bascom-Compiler)
    Antworten: 2
    Letzter Beitrag: 26.08.2010, 17:09

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •