Merge pull request #21 from sensebox/tof-distance

use tof instead of ultrasonic
sensebox · Oct 9, 2024 · 6e19bd5 · 6e19bd5
2 parents 57095b2 + 11e29ef
commit 6e19bd5
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 2 deletions.
diff --git a/senseBox-bike-mcus2/initSensors.ino b/senseBox-bike-mcus2/initSensors.ino
@@ -21,6 +21,16 @@ void initUltrasonic() {
   // pinMode(ECHO_RIGHT, INPUT);
 };
 
+void initVL53L8CX() {
+  Wire.begin();
+  Wire.setClock(1000000); //Sensor has max I2C freq of 1MHz
+  sensor_vl53l8cx_top.begin();
+  sensor_vl53l8cx_top.init_sensor();
+  sensor_vl53l8cx_top.vl53l8cx_set_ranging_frequency_hz(30);
+  sensor_vl53l8cx_top.vl53l8cx_set_resolution(VL53L8CX_RESOLUTION_8X8);
+  sensor_vl53l8cx_top.vl53l8cx_start_ranging();
+}
+
 void initSPS() {
   int errorCount = 0;
   int16_t ret;

diff --git a/senseBox-bike-mcus2/senseBox-bike-mcus2.ino b/senseBox-bike-mcus2/senseBox-bike-mcus2.ino
@@ -11,11 +11,13 @@
 #include <NMEAGPS.h>
 #include <Adafruit_MPU6050.h>
 #include <NewPing.h>  // http://librarymanager/All#NewPing
+#include <vl53l8cx_class.h>
 
 #define TRIGGER_LEFT 1
 #define ECHO_LEFT 2
 #define MAX_DISTANCE_A 400
 NewPing sonarA(TRIGGER_LEFT, ECHO_LEFT, MAX_DISTANCE_A);
+VL53L8CX sensor_vl53l8cx_top(&Wire, -1, -1);
 static NMEAGPS gps;
 // https://github.com/SlashDevin/NeoGPS/blob/master/extras/doc/Data%20Model.md
 static gps_fix fix;
@@ -71,7 +73,8 @@ String name;
 // todo: give feedback through LED if all is working?
 void initsSensors() {
   Serial.print("Ultrasonic...");
-  initUltrasonic();
+  // initUltrasonic();
+  initVL53L8CX();
   // ATTENTION! SPS Disabled for essen-auf-raedern
   // Serial.print("SPS30...");
   // initSPS();  Serial.println("done!");
@@ -98,7 +101,7 @@ void initsSensors() {
 // set measurements for acceleration, distance, humidity and temperature
 void setMeasurements() {
   getAccAmplitudes(&sumAccX, &sumAccY, &sumAccZ);
-  handleDistance();
+  handleDistanceVL53L8CX();
   temp = HDC.readTemperature();
   humi = HDC.readHumidity();
 

diff --git a/senseBox-bike-mcus2/sensorFunctions.ino b/senseBox-bike-mcus2/sensorFunctions.ino
@@ -25,6 +25,35 @@ void handleDistance() {
   // }
 }
 
+/*  To give a confidence rating, a target with status 5 is considered as 100% valid. 
+A status of 6 or 9 can be considered with a confidence value of 50%. 
+All other statuses are below the 50% confidence level. */
+bool validTargetStatus(int status) {
+  return status == 5 || status == 6 || status == 9;
+}
+
+void handleDistanceVL53L8CX() {
+  VL53L8CX_ResultsData Results;
+  uint8_t NewDataReady = 0;
+  uint8_t status;
+
+  status = sensor_vl53l8cx_top.vl53l8cx_check_data_ready(&NewDataReady);
+
+  if ((!status) && (NewDataReady != 0)) {
+    sensor_vl53l8cx_top.vl53l8cx_get_ranging_data(&Results);
+    float min = 1000.0; // larger than what the sensor could possibly see
+    for(int i = 0; i < VL53L8CX_RESOLUTION_8X8*VL53L8CX_NB_TARGET_PER_ZONE; i++) {
+      if(validTargetStatus((int)(&Results)->target_status[i])){
+        float distance = ((&Results)->distance_mm[i])/10;
+        if(min > distance) {
+          min = distance;
+        }
+      }
+    }
+    dist_l = (min==1000.0) ? 400.0 : min; // in theory the sensor can measure up to 4m distance
+  }
+}
+
 void callSPS() {
   //struct sps30_measurement m;
   char serial[SPS30_MAX_SERIAL_LEN];