Update sensor configurations and improve data processing in Arduino sketches

Arduino/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575.ino

@@ -79,8 +79,8 @@ uint16_t sensoraddress8 = 0x38;
 
 uint16_t wait_for_i2c = 50;
 uint16_t imageWidth = 8;
-uint16_t ranging_frequency = 6;
-uint32_t i2c_freq_hz = 600000;
+uint16_t ranging_frequency = 15;
+uint32_t i2c_freq_hz = 1000000;
 uint32_t baudrate = 1000000;
 pin_size_t SDA_PIN0 = 4;
 pin_size_t SCL_PIN0 = 5;
@@ -216,8 +216,7 @@ void loop() {
   processSensorData(sensor13, Results13, "sensor13", LPN_PIN4);
   processSensorData(sensor14, Results14, "sensor14", LPN_PIN5);
   processSensorData(sensor15, Results15, "sensor15", LPN_PIN6);
-  processSensorData(sensor16, Results16, "sensor16", LPN_PIN7);
-  processSensorData(sensor17, Results17, "sensor17", LPN_PIN8);
+  processSensorData(sensor16, Results16, "sensor17", LPN_PIN8);
 
   // Serialize the JSON document and print to Serial
   serializeJson(doc, Serial);
@@ -292,7 +291,7 @@ void initializeSensor(VL53L7CX &sensor, uint16_t sensorAddress, int lpnPin, PCF8
   PCF.write16(0x0000);
 }
 
-void processSensorData(VL53L7CX &sensor, VL53L7CX_ResultsData &results, const JsonArray &data, const char *sensorKey, int lpn_pin) {
+void processSensorData(VL53L7CX &sensor, VL53L7CX_ResultsData &results, const char *sensorKey, int lpn_pin) {
   if (!sensorInitializedI2C1[lpn_pin]) return;
   uint8_t NewDataReady = 0;
   uint8_t status;
@@ -307,7 +306,9 @@ void processSensorData(VL53L7CX &sensor, VL53L7CX_ResultsData &results, const Js
 
   if ((!status) && (NewDataReady != 0)) {
     status = sensor.vl53l7cx_get_ranging_data(&results);
+
     JsonArray sensorData = doc[sensorKey].to<JsonArray>();
+
     for (int y = imageWidth * (imageWidth - 1); y >= 0; y -= imageWidth) {
       for (int x = 0; x <= imageWidth - 1; x++) {
         int index = VL53L7CX_NB_TARGET_PER_ZONE * (x + y);
@@ -319,6 +320,7 @@ void processSensorData(VL53L7CX &sensor, VL53L7CX_ResultsData &results, const Js
       }
     }
   }
+
   digitalWrite(LedPin, LOW);
 }
 

Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino

+/**
+ ******************************************************************************
+ * @file    VL53L7CX_Sat_HelloWorld.ino
+ * @author  STMicroelectronics
+ * @version V1.0.0
+ * @date    11 November 2021
+ * @brief   Arduino test application for the STMicrolectronics VL53L7CX
+ *          proximity sensor satellite based on FlightSense.
+ *          This application makes use of C++ classes obtained from the C
+ *          components' drivers.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2021 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *      without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+#include <Arduino.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <vl53l7cx_class.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <cmath>
+
+#define DEV_I2C0 Wire
+
+#define LedPin LED_BUILTIN
+
+#define FAKE_I2C_RST 99
+
+#define LPN_PIN0 28
+#define LPN_PIN1 27
+#define LPN_PIN2 26
+#define LPN_PIN3 22
+#define LPN_PIN4 19
+#define LPN_PIN5 18
+
+uint16_t sensoraddress0 = 0x30;
+uint16_t sensoraddress1 = 0x31;
+uint16_t sensoraddress2 = 0x32;
+uint16_t sensoraddress3 = 0x33;
+uint16_t sensoraddress4 = 0x34;
+uint16_t sensoraddress5 = 0x35;
+
+uint16_t wait_for_i2c = 50;
+uint16_t imageWidth = 8;
+uint16_t ranging_frequency = 15;
+uint32_t i2c_freq_hz = 1000000;
+uint32_t baudrate = 1000000;
+pin_size_t SDA_PIN0 = 20;
+pin_size_t SCL_PIN0 = 21;
+
+int sensor_count = 0;
+bool sensorInitialized[8] = { false };  // global oder static
+
+JsonDocument doc;
+
+// Components.
+VL53L7CX sensor0(&DEV_I2C0, LPN_PIN0, FAKE_I2C_RST);
+VL53L7CX sensor1(&DEV_I2C0, LPN_PIN1, FAKE_I2C_RST);
+VL53L7CX sensor2(&DEV_I2C0, LPN_PIN2, FAKE_I2C_RST);
+VL53L7CX sensor3(&DEV_I2C0, LPN_PIN3, FAKE_I2C_RST);
+VL53L7CX sensor4(&DEV_I2C0, LPN_PIN4, FAKE_I2C_RST);
+VL53L7CX sensor5(&DEV_I2C0, LPN_PIN5, FAKE_I2C_RST);
+
+void blink_led_loop(void);
+
+void blink_led_loop(void) {
+  do {
+    // Blink the led forever
+    digitalWrite(LedPin, HIGH);
+    delay(wait_for_i2c);
+    digitalWrite(LedPin, LOW);
+  } while (1);
+}
+
+/* Setup ---------------------------------------------------------------------*/
+
+void setup() {
+  delay(3000);
+  // Led.
+  pinMode(LedPin, OUTPUT);
+  digitalWrite(LedPin, HIGH);
+
+  // Initialize serial for output.
+  Serial.begin(baudrate);
+  Serial.println();
+  Serial.println("Please wait, it may take few seconds...");
+
+  // Initialize I2C bus.
+  DEV_I2C0.setSCL(SCL_PIN0);
+  DEV_I2C0.setSDA(SDA_PIN0);
+  DEV_I2C0.begin();
+  DEV_I2C0.setClock(i2c_freq_hz);
+
+  initializeSensor(sensor0, sensoraddress0, LPN_PIN0);
+  initializeSensor(sensor1, sensoraddress1, LPN_PIN1);
+  initializeSensor(sensor2, sensoraddress2, LPN_PIN2);
+  initializeSensor(sensor3, sensoraddress3, LPN_PIN3);
+  initializeSensor(sensor4, sensoraddress4, LPN_PIN4);
+  initializeSensor(sensor5, sensoraddress5, LPN_PIN5);
+
+  Serial.println("Initialized " + String(sensor_count) + " Sensors");
+}
+
+void loop() {
+  // Declare the result data variables for each sensor
+  VL53L7CX_ResultsData Results0;
+  VL53L7CX_ResultsData Results1;
+  VL53L7CX_ResultsData Results2;
+  VL53L7CX_ResultsData Results3;
+  VL53L7CX_ResultsData Results4;
+  VL53L7CX_ResultsData Results5;
+
+  // Process each sensor data and save to respective JSON arrays
+  processSensorData(sensor0, Results0, "sensor0", LPN_PIN0);
+  processSensorData(sensor1, Results1, "sensor1", LPN_PIN1);
+  processSensorData(sensor2, Results2, "sensor2", LPN_PIN2);
+  processSensorData(sensor3, Results3, "sensor3", LPN_PIN3);
+  processSensorData(sensor4, Results4, "sensor4", LPN_PIN4);
+  processSensorData(sensor5, Results5, "sensor5", LPN_PIN5);
+
+  // Serialize the JSON document and print to Serial
+  serializeJson(doc, Serial);
+  Serial.println();
+}
+
+void i2cScanner(TwoWire DEV_I2C) {
+  for (byte address = 1; address < 127; address++) {
+    DEV_I2C.beginTransmission(address);
+    if (DEV_I2C.endTransmission() == 0) {
+      Serial.print("Device found at address 0x");
+      if (address < 0x10)
+        Serial.print("0");
+      Serial.println(address, HEX);
+    }
+  }
+}
+
+void initializeSensor(VL53L7CX &sensor, uint16_t sensorAddress, int lpnPin) {
+  uint8_t status = VL53L7CX_STATUS_OK;
+  uint8_t isAlive = 0;
+
+  digitalWrite(lpnPin, HIGH);
+  // Set I2C address
+  status = sensor.vl53l7cx_set_i2c_address(sensorAddress << 1);
+  if (status != VL53L7CX_STATUS_OK) {
+    Serial.println(VL53L7CX_STATUS_ERROR);
+    return;
+  }
+
+  status = sensor.vl53l7cx_is_alive(&isAlive);
+  if (!isAlive || status != VL53L7CX_STATUS_OK) {
+    Serial.println(VL53L7CX_STATUS_ERROR);
+    return;
+  }
+
+  // Init VL53L7CX sensor
+  status = sensor.vl53l7cx_init();
+  if (status != VL53L7CX_STATUS_OK) {
+    Serial.println(VL53L7CX_STATUS_ERROR);
+    return;
+  }
+
+  delay(wait_for_i2c);
+
+  // Set resolution and frequency
+  sensor.vl53l7cx_set_resolution(VL53L7CX_RESOLUTION_8X8);
+  delay(wait_for_i2c);
+
+  sensor.vl53l7cx_set_ranging_frequency_hz(ranging_frequency);
+  delay(wait_for_i2c);
+
+  // Start ranging
+  sensor.vl53l7cx_start_ranging();
+  delay(wait_for_i2c);
+
+  sensorInitialized[lpnPin] = true;
+  sensor_count++;
+
+  digitalWrite(lpnPin, LOW);
+}
+
+void processSensorData(VL53L7CX &sensor, VL53L7CX_ResultsData &results, const char *sensorKey, int lpn_pin) {
+  if (!sensorInitialized[lpn_pin]) return;
+  uint8_t NewDataReady = 0;
+  uint8_t status;
+
+  // Wait for data to be ready
+  do {
+    status = sensor.vl53l7cx_check_data_ready(&NewDataReady);
+  } while (!NewDataReady);
+
+  // Turn LED on to indicate data processing
+  digitalWrite(LedPin, HIGH);
+
+  if ((!status) && (NewDataReady != 0)) {
+    status = sensor.vl53l7cx_get_ranging_data(&results);
+
+    JsonArray sensorData = doc[sensorKey].to<JsonArray>();
+
+    for (int y = imageWidth * (imageWidth - 1); y >= 0; y -= imageWidth) {
+      for (int x = 0; x <= imageWidth - 1; x++) {
+        int index = VL53L7CX_NB_TARGET_PER_ZONE * (x + y);
+
+        // Neu: JsonArray via add<JsonArray>()
+        JsonArray measurement = sensorData.add<JsonArray>();
+        measurement.add(results.distance_mm[index]);
+        measurement.add(results.target_status[index]);
+      }
+    }
+  }
+
+  digitalWrite(LedPin, LOW);
+}

workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py

@@ -27,7 +27,7 @@ class SerialListPublisher(Node):
     """
     A ROS 2 node that reads data from a serial port, processes it, and publishes it as a PointCloud2 message.
     """
-    def __init__(self, serial_port='/dev/ttyACM0', baudrate=115200):
+    def __init__(self, serial_port='/dev/ttyACM0', baudrate=1000000):
         super().__init__('serial_to_pcl_node')  # Initialize the ROS 2 node with a name
         self.declare_parameter('parent_frame', 'vl53l7cx_link')  # Declare a parameter for the parent frame
         self.parent_frame = self.get_parameter('parent_frame').get_parameter_value().string_value  # Get the parameter value
@@ -60,7 +60,7 @@ class SerialListPublisher(Node):
                 for key, array in data.items():
                     filtered_array = []
                     for pair in array:
-                        if pair[1] not in [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 14]:
+                        if pair[1] not in [5,6,9,12]:
                             filtered_array.append([99999])
                         else:
                             filtered_array.append([pair[0]])