LD06_python_viewer/main.py at main · FIPATECH/LD06_python_viewer · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
import os
import math
import serial
from serial.tools import list_ports
import matplotlib.pyplot as plt
from CalcLidarData import CalcLidarData

BAUD = 230400

# Détection automatique du port série du LD06
def find_ld06_port() -> str:
    env = os.getenv("LD06_PORT")
    if env:
        return env

    # Liste des ports détectés
    ports = list(list_ports.comports())
    ordered = sorted(
        ports,
        key=lambda p: (
            0
            if any(
                k in (p.device.lower() + " " + str(p.description).lower())
                for k in (
                    "usb",
                    "usbserial",
                    "usbmodem",
                    "cp210",
                    "ch340",
                    "ftdi",
                    "prolific",
                )
            )
            else 1
        ),
    )

    # Probe actif: recherche de l’entête 0x54 0x2C
    for p in ordered:
        try:
            with serial.Serial(p.device, baudrate=BAUD, timeout=0.5) as s:
                s.reset_input_buffer()
                data = s.read(512)
                if b"\x54\x2c" in data:
                    return p.device
        except Exception:
            continue

    # Fallback: premier port dispo
    if ports:
        return ports[0].device

    raise RuntimeError(
        "Aucun port LD06 détecté. Définir LD06_PORT ou vérifier le branchement"
    )


def main():
    fig = plt.figure(figsize=(8, 8))
    ax = fig.add_subplot(111, projection='polar')
    ax.set_title('lidar (quitter avec E)', fontsize=18)
    plt.connect('key_press_event', lambda event: exit(0) if event.key == 'e' else None)

    port = find_ld06_port()
    print(f"Port détecté: {port}")

    ser = serial.Serial(
        port=port,
        baudrate=BAUD,
        timeout=5.0,
        bytesize=8,
        parity='N',
        stopbits=1
    )

    tmpString = ""
    angles, distances = [], []
    i = 0

    try:
        while True:
            loopFlag = True
            flag2c = False

            if i % 40 == 39:
                if 'sc' in locals():
                    sc.remove()
                sc = ax.scatter(angles, distances, s=5)
                ax.set_theta_offset(math.pi / 2)
                plt.pause(0.01)
                angles.clear()
                distances.clear()
                i = 0

            while loopFlag:
                b = ser.read()
                if not b:
                    break
                tmpInt = int.from_bytes(b, 'big')

                if tmpInt == 0x54:
                    tmpString += b.hex() + " "
                    flag2c = True
                    continue

                elif tmpInt == 0x2C and flag2c:
                    tmpString += b.hex()

                    if not len(tmpString[0:-5].replace(' ', '')) == 90:
                        tmpString = ""
                        loopFlag = False
                        flag2c = False
                        continue

                    lidarData = CalcLidarData(tmpString[0:-5])
                    angles.extend(lidarData.Angle_i)
                    distances.extend(lidarData.Distance_i)

                    tmpString = ""
                    loopFlag = False
                else:
                    tmpString += b.hex() + " "

                flag2c = False

            i += 1
    finally:
        ser.close()

if __name__ == "__main__":
    main()