Advertisement
raffaelegriecoit
Nov 20th, 2024
26
0
Never
This is comment for paste FRANCO
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. function parsedData = parseRadarData(rawData)
  2. % Definisci la struttura dei dati parsati
  3. parsedData = struct('x', [], 'y', [], 'z', [], 'velocity', []);
  4.  
  5. % Definisci la parola magica per sincronizzare i pacchetti
  6. magicWord = [2 1 4 3 6 5 8 7];
  7. magicWordBytes = length(magicWord);
  8.  
  9. % Cerca la parola magica nei dati grezzi
  10. startIdx = [];
  11. for i = 1:(length(rawData) - magicWordBytes + 1)
  12. if all(rawData(i:i+magicWordBytes-1) == magicWord)
  13. startIdx = i;
  14. break;
  15. end
  16. end
  17.  
  18. % Se la parola magica non viene trovata, esci dalla funzione
  19. if isempty(startIdx)
  20. warning('Parola magica non trovata nei dati ricevuti.');
  21. return;
  22. end
  23.  
  24. % Estrai l'header del frame
  25. frameHeaderLength = 40; % Lunghezza dell'header del frame in byte
  26. frameHeader = rawData(startIdx + magicWordBytes : startIdx + magicWordBytes + frameHeaderLength - 1);
  27.  
  28. % Estrai il numero di TLV nel frame
  29. numTLVs = typecast(uint8(frameHeader(35:38)), 'uint32');
  30.  
  31. % Imposta il puntatore all'inizio dei TLV
  32. pointer = startIdx + magicWordBytes + frameHeaderLength;
  33.  
  34. % Processa ciascun TLV
  35. for tlvIdx = 1:numTLVs
  36. % Leggi l'header del TLV
  37. tlvType = typecast(uint8(rawData(pointer:pointer+3)), 'uint32');
  38. tlvLength = typecast(uint8(rawData(pointer+4:pointer+7)), 'uint32');
  39. pointer = pointer + 8; % Salta l'header del TLV
  40.  
  41. % Verifica che ci siano abbastanza dati rimanenti
  42. if pointer + tlvLength > length(rawData)
  43. warning('Lunghezza del TLV supera i dati disponibili.');
  44. return;
  45. end
  46.  
  47. % Processa il TLV in base al tipo
  48. switch tlvType
  49. case 1 % Punti rilevati
  50. % Numero di punti rilevati
  51. numDetectedPoints = (tlvLength - 8) / 16; % 16 byte per punto
  52. if numDetectedPoints > 0
  53. % Estrai i dati dei punti
  54. pointData = rawData(pointer:pointer + numDetectedPoints * 16 - 1);
  55. pointData = typecast(uint8(pointData), 'single');
  56. pointData = reshape(pointData, [4, numDetectedPoints]);
  57.  
  58. % Assegna i dati parsati alla struttura
  59. parsedData.x = pointData(1, :);
  60. parsedData.y = pointData(2, :);
  61. parsedData.z = pointData(3, :);
  62. parsedData.velocity = pointData(4, :);
  63. end
  64. % Aggiungi altri casi per altri tipi di TLV se necessario
  65. otherwise
  66. % TLV non supportato
  67. warning('Tipo di TLV non supportato: %d', tlvType);
  68. end
  69.  
  70. % Aggiorna il puntatore alla fine del TLV corrente
  71. pointer = pointer + tlvLength - 8; % Sottrai 8 byte dell'header del TLV
  72. end
  73. end
  74.  
  75.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement