359 lines
11 KiB
C
359 lines
11 KiB
C
#ifndef _WATERLEVEL_H_
|
|
#define _WATERLEVEL_H_
|
|
|
|
#include <Wire.h>
|
|
#include <VL53L0X.h> //pololu/VL53L0X@^1.3.1
|
|
|
|
|
|
|
|
|
|
|
|
// +++++++++++++++ Common Parameters ++++++++++
|
|
|
|
#define READINTERVAL_WATERLEVEL 500
|
|
#define WATERLEVELMEAN_SIZE 16
|
|
#define WATERLEVELMEAN_FILTER_CUTOFF 4 //max value is around WATERLEVELMEAN_SIZE/2
|
|
|
|
|
|
#define WATERLEVEL_UNAVAILABLE -1 //-1 is also timeout value
|
|
|
|
|
|
// +++++++++++++++ VL53L0X +++++++++++++++
|
|
VL53L0X sensorA;
|
|
#define PIN_VL53L0X_XSHUT_A 19
|
|
// Uncomment this line to use long range mode. This
|
|
// increases the sensitivity of the sensor and extends its
|
|
// potential range, but increases the likelihood of getting
|
|
// an inaccurate reading because of reflections from objects
|
|
// other than the intended target. It works best in dark
|
|
// conditions.
|
|
|
|
//#define LONG_RANGE
|
|
|
|
// Uncomment ONE of these two lines to get
|
|
// - higher speed at the cost of lower accuracy OR
|
|
// - higher accuracy at the cost of lower speed
|
|
|
|
//#define HIGH_SPEED
|
|
#define HIGH_ACCURACY
|
|
|
|
|
|
|
|
|
|
float waterlevelAMean_array[WATERLEVELMEAN_SIZE];
|
|
uint16_t waterlevelAMean_array_pos=0;
|
|
float waterlevelA=WATERLEVEL_UNAVAILABLE; //distance from floor to water surface [mm]
|
|
float watervolumeA=WATERLEVEL_UNAVAILABLE; //calculated Volume in Reservoir
|
|
|
|
|
|
//Calibration
|
|
float waterlevelA_calib_offset=532.78; //c
|
|
float waterlevelA_calib_factor=-1.179; //m
|
|
|
|
|
|
float waterlevelA_calib_reservoirArea=20*20*3.1416; //area in cm^2. barrel diameter inside is 400mm
|
|
|
|
uint16_t distanceA_unsuccessful_count=0;
|
|
|
|
// +++++++++++++++ VL53L0X +++++++++++++++
|
|
VL53L0X sensorB;
|
|
|
|
#define PIN_VL53L0X_XSHUT_A 19
|
|
// Uncomment this line to use long range mode. This
|
|
// increases the sensitivity of the sensor and extends its
|
|
// potential range, but increases the likelihood of getting
|
|
// an inaccurate reading because of reflections from objects
|
|
// other than the intended target. It works best in dark
|
|
// conditions.
|
|
|
|
//#define LONG_RANGE
|
|
|
|
// Uncomment ONE of these two lines to get
|
|
// - higher speed at the cost of lower accuracy OR
|
|
// - higher accuracy at the cost of lower speed
|
|
|
|
//#define HIGH_SPEED
|
|
#define HIGH_ACCURACY
|
|
|
|
|
|
|
|
|
|
float waterlevelBMean_array[WATERLEVELMEAN_SIZE];
|
|
uint16_t waterlevelBMean_array_pos=0;
|
|
float waterlevelB=WATERLEVEL_UNAVAILABLE; //distance from floor to water surface [mm]
|
|
float watervolumeB=WATERLEVEL_UNAVAILABLE; //calculated Volume in Reservoir
|
|
|
|
|
|
//Calibration
|
|
float waterlevelB_calib_offset=532.78; //c
|
|
float waterlevelB_calib_factor=-1.179; //m
|
|
|
|
|
|
float waterlevelB_calib_reservoirArea=56.5*36.5; //area in cm^2
|
|
|
|
uint16_t distanceB_unsuccessful_count=0;
|
|
|
|
|
|
|
|
float waterlevelA_heightToVolume(float distance);
|
|
float waterlevelB_heightToVolume(float distance);
|
|
|
|
|
|
mqttValueTiming timing_waterlevelA;
|
|
mqttValueTiming timing_waterlevelB;
|
|
|
|
void waterlevel_setup() {
|
|
|
|
|
|
pinMode(PIN_VL53L0X_XSHUT_A, OUTPUT);
|
|
digitalWrite(PIN_VL53L0X_XSHUT_A, LOW); //pull to GND
|
|
|
|
|
|
|
|
|
|
/*
|
|
Wire.begin();
|
|
|
|
byte error, address;
|
|
int nDevices;
|
|
|
|
delay(500);
|
|
Serial.println("Scanning...");
|
|
|
|
nDevices = 0;
|
|
for(address = 1; address < 127; address++ )
|
|
{
|
|
// The i2c_scanner uses the return value of
|
|
// the Write.endTransmisstion to see if
|
|
// a device did acknowledge to the address.
|
|
Wire.beginTransmission(address);
|
|
error = Wire.endTransmission();
|
|
|
|
if (error == 0)
|
|
{
|
|
Serial.print("I2C device found at address 0x");
|
|
if (address<16)
|
|
Serial.print("0");
|
|
Serial.print(address,HEX);
|
|
Serial.println(" !");
|
|
|
|
nDevices++;
|
|
}
|
|
else if (error==4)
|
|
{
|
|
Serial.print("Unknown error at address 0x");
|
|
if (address<16)
|
|
Serial.print("0");
|
|
Serial.println(address,HEX);
|
|
}
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
|
|
timing_waterlevelA.minchange=0.0;
|
|
timing_waterlevelA.maxchange=3.0;
|
|
timing_waterlevelA.mintime=30*000;
|
|
timing_waterlevelA.maxtime=60*60*1000;
|
|
|
|
timing_waterlevelB.minchange=0.0;
|
|
timing_waterlevelB.maxchange=3.0;
|
|
timing_waterlevelB.mintime=30*000;
|
|
timing_waterlevelB.maxtime=60*60*1000;
|
|
|
|
|
|
|
|
Wire.begin();
|
|
Serial.print("I2C Clock Speed=");
|
|
Serial.println(Wire.getClock());
|
|
|
|
|
|
//Initialize SensorB first
|
|
sensorB.setTimeout(1000);
|
|
if (!sensorB.init())
|
|
{
|
|
Serial.println("Failed to detect and initialize sensorA!");
|
|
publishInfo("error/waterlevel","Failed to detect and initialize sensorA");
|
|
delay(1000);
|
|
}
|
|
|
|
Serial.println("set addr 0x2A");
|
|
sensorB.setAddress(0x2A); //change address
|
|
Serial.println("conf Default");
|
|
|
|
|
|
|
|
#if defined LONG_RANGE
|
|
// lower the return signal rate limit (default is 0.25 MCPS)
|
|
sensorB.setSignalRateLimit(0.1);
|
|
// increase laser pulse periods (defaults are 14 and 10 PCLKs)
|
|
sensorB.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
|
|
sensorB.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
|
|
#endif
|
|
|
|
#if defined HIGH_SPEED
|
|
// reduce timing budget to 20 ms (default is about 33 ms)
|
|
sensorB.setMeasurementTimingBudget(20000);
|
|
#elif defined HIGH_ACCURACY
|
|
// increase timing budget to 200 ms
|
|
sensorB.setMeasurementTimingBudget(200000);
|
|
#endif
|
|
|
|
|
|
|
|
// Stop driving this sensor's XSHUT low. This should allow the carrier
|
|
// board to pull it high. (We do NOT want to drive XSHUT high since it is
|
|
// not level shifted.) Then wait a bit for the sensor to start up.
|
|
pinMode(PIN_VL53L0X_XSHUT_A, INPUT);
|
|
delay(50);
|
|
|
|
//Initialize Sensor A after SensorB's address was changed
|
|
sensorA.setTimeout(1000);
|
|
if (!sensorA.init())
|
|
{
|
|
Serial.println("Failed to detect and initialize sensorA!");
|
|
publishInfo("error/waterlevel","Failed to detect and initialize sensorA");
|
|
delay(1000);
|
|
}
|
|
|
|
|
|
|
|
#if defined LONG_RANGE
|
|
// lower the return signal rate limit (default is 0.25 MCPS)
|
|
sensorA.setSignalRateLimit(0.1);
|
|
// increase laser pulse periods (defaults are 14 and 10 PCLKs)
|
|
sensorA.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
|
|
sensorA.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
|
|
#endif
|
|
|
|
#if defined HIGH_SPEED
|
|
// reduce timing budget to 20 ms (default is about 33 ms)
|
|
sensorA.setMeasurementTimingBudget(20000);
|
|
#elif defined HIGH_ACCURACY
|
|
// increase timing budget to 200 ms
|
|
sensorA.setMeasurementTimingBudget(200000);
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (uint16_t i=0;i<WATERLEVELMEAN_SIZE;i++) {
|
|
waterlevelAMean_array[i]=WATERLEVEL_UNAVAILABLE; //-1 is also timeout value
|
|
waterlevelBMean_array[i]=WATERLEVEL_UNAVAILABLE; //-1 is also timeout value
|
|
}
|
|
}
|
|
|
|
void waterlevel_loop(unsigned long loopmillis) {
|
|
static uint8_t waterlevel_loop_select=0;
|
|
|
|
switch(waterlevel_loop_select)
|
|
{
|
|
case 0:
|
|
// ############ A
|
|
|
|
static unsigned long last_read_waterlevelA;
|
|
if (loopmillis>=last_read_waterlevelA+READINTERVAL_WATERLEVEL) {
|
|
last_read_waterlevelA=loopmillis;
|
|
|
|
|
|
uint16_t distance=sensorA.readRangeSingleMillimeters(); //error=65535
|
|
|
|
//Serial.print("Distance reading A="); Serial.print(distance);Serial.println();
|
|
|
|
|
|
if (distance!=WATERLEVEL_UNAVAILABLE && distance!=65535) { //successful
|
|
waterlevelAMean_array[waterlevelAMean_array_pos]=distance;
|
|
waterlevelAMean_array_pos++;
|
|
waterlevelAMean_array_pos%=WATERLEVELMEAN_SIZE;
|
|
distanceA_unsuccessful_count=0;
|
|
}else{
|
|
distanceA_unsuccessful_count++;
|
|
if (distanceA_unsuccessful_count%20==0) {
|
|
String _text="Distance A unsuccessful count=";
|
|
_text.concat(distanceA_unsuccessful_count);
|
|
_text.concat(" distance=");
|
|
_text.concat(distance);
|
|
publishInfo("error/waterlevel",_text);
|
|
}
|
|
}
|
|
|
|
|
|
if (isValueArrayOKf(waterlevelAMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
|
|
float _filteredDistance=getFilteredf(waterlevelAMean_array,WATERLEVELMEAN_SIZE,WATERLEVELMEAN_FILTER_CUTOFF);
|
|
Serial.print("Filtered reading A="); Serial.print(_filteredDistance);Serial.println();
|
|
|
|
//Invert distance and offset
|
|
waterlevelA=constrain(waterlevelA_calib_offset+waterlevelA_calib_factor*_filteredDistance,0,1000);
|
|
watervolumeA=waterlevelA_heightToVolume(waterlevelA);
|
|
|
|
//float _meanWaterlevel=getMeanf(waterlevelMean,WATERLEVELMEAN_SIZE);
|
|
//Serial.print("\t Dist="); Serial.print(_filteredWaterlevel); Serial.print("mm"); Serial.print("(+- "); Serial.print((getMaxf(waterlevelMean,WATERLEVELMEAN_SIZE)-getMinf(waterlevelMean,WATERLEVELMEAN_SIZE))/2.0); Serial.print(")"); Serial.print(" [mean="); Serial.print(_meanWaterlevel); Serial.print("]");
|
|
}
|
|
}
|
|
waterlevel_loop_select++;
|
|
break;
|
|
|
|
case 1:
|
|
|
|
// ############ B
|
|
|
|
static unsigned long last_read_waterlevelB;
|
|
if (loopmillis>=last_read_waterlevelB+READINTERVAL_WATERLEVEL) {
|
|
last_read_waterlevelB=loopmillis;
|
|
|
|
|
|
uint16_t distance=sensorB.readRangeSingleMillimeters(); //out of range =255
|
|
|
|
//Serial.print("Distance reading B="); Serial.print(distance);Serial.println();
|
|
|
|
|
|
if (distance!=WATERLEVEL_UNAVAILABLE) { //successful
|
|
waterlevelBMean_array[waterlevelBMean_array_pos]=distance;
|
|
waterlevelBMean_array_pos++;
|
|
waterlevelBMean_array_pos%=WATERLEVELMEAN_SIZE;
|
|
distanceB_unsuccessful_count=0;
|
|
}else{
|
|
distanceB_unsuccessful_count++;
|
|
if (distanceB_unsuccessful_count%20==0) {
|
|
String _text="Distance B unsuccessful count=";
|
|
_text.concat(distanceB_unsuccessful_count);
|
|
_text.concat(" distance=");
|
|
_text.concat(distance);
|
|
publishInfo("error/waterlevel",_text);
|
|
}
|
|
}
|
|
|
|
|
|
if (isValueArrayOKf(waterlevelBMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
|
|
float _filteredDistance=getFilteredf(waterlevelBMean_array,WATERLEVELMEAN_SIZE,WATERLEVELMEAN_FILTER_CUTOFF);
|
|
|
|
Serial.print("Filtered reading B="); Serial.print(_filteredDistance);Serial.println();
|
|
//Invert distance and offset
|
|
waterlevelB=constrain(waterlevelB_calib_offset+waterlevelB_calib_factor*_filteredDistance,0,1000);
|
|
watervolumeB=waterlevelB_heightToVolume(waterlevelB);
|
|
|
|
|
|
//float _meanWaterlevel=getMeanf(waterlevelMean,WATERLEVELMEAN_SIZE);
|
|
//Serial.print("\t Dist="); Serial.print(_filteredWaterlevel); Serial.print("mm"); Serial.print("(+- "); Serial.print((getMaxf(waterlevelMean,WATERLEVELMEAN_SIZE)-getMinf(waterlevelMean,WATERLEVELMEAN_SIZE))/2.0); Serial.print(")"); Serial.print(" [mean="); Serial.print(_meanWaterlevel); Serial.print("]");
|
|
}
|
|
|
|
waterlevel_loop_select=0;
|
|
break;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
float waterlevelA_heightToVolume(float distance){
|
|
return waterlevelA_calib_reservoirArea/100 * distance/100; //area[cm^2] in dm^2 * height in dm = dm^3= L
|
|
}
|
|
|
|
|
|
float waterlevelB_heightToVolume(float distance){
|
|
return waterlevelB_calib_reservoirArea/100 * distance/100; //area[cm^2] in dm^2 * height in dm = dm^3= L
|
|
}
|
|
|
|
#endif |