diff --git a/senseBox-bike-mcus2/initSensors.ino b/senseBox-bike-mcus2/initSensors.ino index 8280dcb..4eb4550 100644 --- 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 index aac64e3..d2fcef4 100644 --- a/senseBox-bike-mcus2/senseBox-bike-mcus2.ino +++ b/senseBox-bike-mcus2/senseBox-bike-mcus2.ino @@ -11,11 +11,13 @@ #include #include #include // http://librarymanager/All#NewPing +#include #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 index 4886460..1369257 100644 --- 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];