From 21ae9b15820eaa378e70b93f23b114a7329c2f0b Mon Sep 17 00:00:00 2001
From: Rene Ebeling <rene.ebeling@alumni.fh-aachen.de>
Date: Tue, 20 May 2025 14:14:22 +0200
Subject: [PATCH] Update sensor configurations and improve data processing in
Arduino sketches
---
...clean_both_I2C_with_STMlibrary_PCF8575.ino | 12 +-
.../V1_6_Sensoren_mit_LPn_Control_Vincent.ino | 242 ++++++++++++++++++
.../serial_to_pcl/serial_to_pcl_node.py | 4 +-
3 files changed, 251 insertions(+), 7 deletions(-)
create mode 100644 Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino
diff --git a/Arduino/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575.ino b/Arduino/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575.ino
index bb83a0f..3c5c72e 100644
--- a/Arduino/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575/Final_18_vl53l7cx_clean_both_I2C_with_STMlibrary_PCF8575.ino
+++ b/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);
}
diff --git a/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino b/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino
new file mode 100644
index 0000000..51f87f4
--- /dev/null
+++ b/Arduino/V1_6_Sensoren_mit_LPn_Control_Vincent/V1_6_Sensoren_mit_LPn_Control_Vincent.ino
@@ -0,0 +1,242 @@
+/**
+ ******************************************************************************
+ * @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>© 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);
+}
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py
index d90094a..c90f25e 100644
--- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py
+++ b/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]])
--
GitLab