Range finder vl53l1x as external sensor

[naorwaiss]

hi i try to connect vl53l1xto rasbri pi zero and then with mavlink to gave the FC the data
can someone do this? - i stack with the configuration of the FC because i think me pymavlink code is fine

 import time
from pymavlink import mavutil
import board
import digitalio
import adafruit_vl53l1x

class MAVLinkConnection:
    def __init__(self, connection_string, baud):
        self.master = mavutil.mavlink_connection(connection_string, baud=baud)
        self.wait_conn()
        self.configure_autopilot()

    def wait_conn(self):
        msg = None
        while not msg:
            self.master.mav.ping_send(
                int(time.time() * 1e6),  # Unix time in microseconds
                0,  # Ping number
                0,  # Request ping of all systems
                0  # Request ping of all components
            )
            msg = self.master.recv_match()
            time.sleep(0.5)

    def configure_autopilot(self):
        # Set rangefinder type to MAVLink (example setting)
        self.master.mav.param_set_send(
            1,
            1,
            b"RNGFND_TYPE",
            10,  # "MAVLink"
            mavutil.mavlink.MAV_PARAM_TYPE_INT8
        )

    def send_distance_sensor(self, time_boot_ms, min_dist, max_dist, current_dist, sensor_id):
        # Ensuring all parameters are integers
        time_boot_ms = int(time_boot_ms)
        min_dist = int(min_dist)
        max_dist = int(max_dist)
        current_dist = int(current_dist)
        sensor_id = int(sensor_id)

        # Ensure to provide defaults for optional parameters not used
        orientation = mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270
        covariance = 0  # Covariance, a value of 0 means unknown
        type = mavutil.mavlink.MAV_DISTANCE_SENSOR_UNKNOWN

        # Attempt to send the MAVLink distance sensor message
        try:
            self.master.mav.distance_sensor_send(
                time_boot_ms,
                min_dist,
                max_dist,
                current_dist,
                type,
                sensor_id,
                orientation,
                covariance
            )
        except Exception as e:
            print(f"Error sending MAVLink message: {e}")


class SensorManager:
    def __init__(self):
        self.i2c = board.I2C()
        self.sensors = []
        self.setup_sensors()

    def setup_sensors(self):
        xshut_pins = [board.D17, board.D27]
        for pin_number, pin in enumerate(xshut_pins):
            shutdown_pin = digitalio.DigitalInOut(pin)
            shutdown_pin.switch_to_output(value=False)
            shutdown_pin.value = True
            sensor = adafruit_vl53l1x.VL53L1X(self.i2c)
            self.sensors.append(sensor)

            # Change the address of the sensor if it's not the last one
            if pin_number < len(xshut_pins) - 1:
                sensor.set_address(pin_number + 0x30)

        for sensor in self.sensors:
            sensor.start_ranging()

    def read_sensors(self):
        readings = []
        for sensor_number, sensor in enumerate(self.sensors):
            if sensor.data_ready:
                distance = sensor.distance
                sensor.clear_interrupt()
                readings.append((sensor_number + 1, distance))
        return readings

class Application:
    def __init__(self):
        # Specify the serial port and baud rate for the real drone connection
        self.mavlink = MAVLinkConnection('/dev/ttyAMA0', 57600)
        self.sensors = SensorManager()

    def run(self):
        tstart = time.time()
        while True:
            time.sleep(0.5)
            readings = self.sensors.read_sensors()
            for sensor_id, distance in readings:
                time_boot_ms = int((time.time() - tstart) * 1000)
                self.mavlink.send_distance_sensor(time_boot_ms, 10, 40, distance, sensor_id)
                print(f"Distance sensor {sensor_id}: {distance}")

if __name__ == "__main__":
    app = Application()
    app.run()

i take part of this code from someone that do this before but he didnt explain how to config the FC
i do this operation due to the fact that this sensor didnt have static address a need the xsut pin to turn them off
thanks for the help