This week we have tried that our sensor measures the opening of the window using only the accelerometer, without using the magnetic linear encoder that we have used in the previous blog.
The measurement is carried out by making a double integration of the acceleration values.
We have developed a small python script to obtain the data from the Arduino through the serial port and be able to graph it.
Tracking System for Classroom Ventilation Routines
A STEM project for classrooms
the VenTTracker project - Blog #08 - Trying to make a measuring device with the accelerometer
The measuring device
We have converted our window sensor into a distance measuring device. This will allow us to know the opening status of a window for our project to monitor the ventilation routines that we are doing with the Arduino Nano 33 IoT.
We use the LSM6DS3, iNEMO inertial module sensor, from the the Arduino Nano 33 IoTArduino Nano 33 IoT, to estimate the distance from the window closed position.
More information about the sensor in our past blog: VenTTracker #04 - Playing with the IMU
The theory
We will try to get the positioning of the window using only the accelerometer and magnetic reed switch used to detect that the window is open or closed as described in our second blog VenTTracker #02 - Analyzing window types.
An acceleration sensor measures the acceleration applied to the device, including the force of gravity. The accelerometer returns acceleration force data for all three coordinate axes,
Motion sensors are useful for monitoring device movement, such as tilt, shake, rotate, or roll. Movement is usually a reflection of direct input from the user (for example, a user driving a car in a game or a user controlling a ball in a game), but it can also be a reflection of the physical environment in which it is find the device. (for example, moving with you while driving your car) In the first case, you are monitoring the movement in relation to the reference frame of the device or the reference frame of your application; in the second case, you are monitoring movement relative to the world's frame of reference. Motion sensors by themselves are not normally used to monitor the position of the device but as our requirements are not very demanding we are going to give it a try.
To obtain the position we must perform a double integral on the acceleration data. We apply a simple algorithm to implement a double integration of the data obtained from the sensor. The double integration consists of making a first integration to obtain the velocity and then another on the latter to obtain the position.
Graphic visualization of the double integration that we have obtained with the small python script that we include at the end of the blog
Calibrating the device
The signal is not noise-free, so it must be digitally filtered. The filter that we are going to use calculates an average of the acceleration; the value will be the result of averaging a certain number of samples.
We found that filtering only by averaging the acceleration is not enough. There is also environmental noise such as small vibrations caused by noise or air movement, so we carry out an extra filtering to discriminate the real acceleration that affects the movement of our window.
A "stopped" state is essential to obtain correct data. A calibration routine is needed when booting our sensor. This calibration value should be as accurate as possible. We have found that it is best to calibrate at various points on the window because the window may not be level properly. The actual value of the acceleration is the sample minus the calibration value.
In the calibration routine we eliminate static acceleration due to gravity. In this routine, we average the samples when the accelerometer is in a state of standstill. The more samples we have taken, the more accurate the calibration result.
Getting started
Initially we started taking samples with the official arduino library to see the behavior of our sensor by doing the double integral and applying an initial filter. The results are not very exact but they are valid for our purpose, which is to determine the position of the window without much precision. From this moment we decided to use the SparkFun library to have more control over the working parameters of the IMU.
Graphics made with the Arduino library. AccX in m/s^2, VelX in m/s, PosX in cm
Filtering Chain
To eliminate mechanical noise we use a low pass filter. We have to eliminate as much noise as possible to prevent errors from accumulating when integrating. Being a double integral the one that we are going to make, the errors grow quadratically.
An easy way to perform a low pass filter is to perform a moving average. We have simplified filtering and consists of obtaining the average of a set of samples. It is important to obtain the average of a balanced number of samples. If we take too many samples we can end up losing too much information and if we take too few we end up with an incorrect value.
We have carried out many tests to reach an adequate value. However, we observe that the number of samples required also depends on the average speed at which we move the window, so we have had to reach a compromise in which the sensor no longer acts correctly below a velocity threshold.
When we are in the stop condition, minor errors in acceleration can be interpreted as constant velocity due to the fact that non-zero samples are added. In the ideal case for a stopped state, all samples should be zero. If we do not zero these samples they will accumulate on the velocity producing a virtual continuous movement and, therefore, an unstable position.
Even with the above filtering, some data may be wrong, so we need a discrimination window between valid data and invalid data for the stopped state.
This is our filtering chain:
After filtering:
Motion completion detection.
Looking at the graph below, there is an initial acceleration or deceleration at maximum speed. So that acceleration changes direction to zero again. At this point we can consider that a complete displacement has been made and a new position has been reached.
In a real scenario, the area under the positive part should be equal to the area of the negative part, reaching zero velocity when the window stops. If we don't force that velocity to zero, the position will be in an unstable state. To force the steady state if we have several acceleration samples to zero or the velocity changes sign, we force the velocity to zero.
User interface
Our final version of the sensor will not have a graphic display to reduce power consumption and increase battery life.
For taking samples we have included the same graphic display that we used for the previous prototype of the sensor that used a linear magnetic encoder to determine the position of the moving part of the window.
The display shows the filtered data of the acceleration, speed and position. The last position data is shown in centimeters.
Absolute reference
We need a way to force an origin to our measurements. We use a magnetic reed switch to zero our sensor.
Resetting position to zero. Absolute reference.
Estimating distance from origin
Adjusting the LSM6DS3 sensor
With the LSM6DS3 we can adjust:
- the Linear acceleration measurement range
- the sample rate
- the antialiasing filter bandwidth
After taking many samples we decided to adjust the settings as shown in the following images.
The circuit
The circuit is similar to the window sensor prototype from the previous blog, VenTTracker #06 - Window Sensor Prototype, from which we have removed the indicator lods and the two magnetic reed switches that made up our magnetic linear encoder.
The code
Python Code
############################################################################ # This is an example for testing the VenTTracker Window Sensor # The sensor measures the opening of the window using only the accelerometer # When the device detects a complete movement it sends a stream of data # with the following structure. # This scrtp parse the data a graph it. # # START,accelSampleRate,accelRange,accelBandWidth, lpfSamples<LF> # accData,velData,posData,timestamp<LF> # accData,velData,posData,timestamp<LF> # ... # ... # ... # accData,velData,posData,timestamp<LF> # END<LF> # # # Author: Enrique Albertos # Date: 2021-04-23 ############################################################################ import serial import time import pandas as pd import matplotlib.pyplot as plt # if using a Jupyter notebook include %matplotlib inline # set up the serial line ser = serial.Serial('COM17', 9600) time.sleep(2) while True: # Read and record the data accData =[] # empty list to store the data velData =[] # empty list to store the data posData =[] # empty list to store the data accelSampleRate = "" accelRange = "" accelBandWidth ="" lpfSamples = "" timeStampData = [] readings = 0 while True: b = ser.readline() # read a byte string string_n = b.decode() # decode byte string into Unicode string = string_n.rstrip() # remove \n and \r datalist = string.split(",") if (datalist[0] == "START"): accelSampleRate = datalist[1] accelRange = datalist[2] accelBandWidth = datalist[3] lpfSamples = datalist[4] break csvData = [] while True: b = ser.readline() # read a byte string csvData.append(b) string_n = b.decode() # decode byte string into Unicode string = string_n.rstrip() # remove \n and \r if (string == "END"): break readings = readings + 1 datalist = string.split(",") accData.append(float(datalist[0])) # add to the end of data list velData.append(float(datalist[1])) # add to the end of data list posData.append(float(datalist[2])) # add to the end of data list timeStampData.append(int(datalist[3])) # add to the end of data list print(readings) if readings > 5: df = pd.DataFrame(csvData) df.to_csv("C:/Arduino/" + str(time.time()) + ".log") fig, axs = plt.subplots(1, 3, figsize=(14, 6), sharey=False) axs[0].plot(timeStampData, accData, label="Acceleration") axs[0].set_title('Acceleration') axs[0].set_xlabel('Time (ms)') axs[0].set_ylabel('Acceleration (m/s^2)') axs[0].grid( True, color='0.95') axs[1].plot(timeStampData,velData, label="Velocity") axs[1].set_title('Velocity') axs[1].set_xlabel('Time (ms)') axs[1].set_ylabel('Velocity (m/s)') axs[1].grid( True, color='0.95') axs[2].plot(timeStampData,posData, label= "Distance") axs[2].set_xlabel('Time (ms)') axs[2].set_ylabel('Distance (m)') axs[2].set_title('Distance') axs[2].grid(True, color='0.95') fig.suptitle('Distance. Sample Rate = ' + accelSampleRate + " Hz, Range +-" + accelRange + " G, Bandwidth " + accelBandWidth + " Hz" ) plt.show()
Arduino Code
Obtaining data in real time from the PC is not feasible due to the time it takes to send through the serial port and the speed at which we want to take samples from the accelerometer. Therefore, the log data is buffered and sent to the serial port when a complete movement is detected.
/************************************************************************** This is an example for testing the VenTTracker Window Sensor The sensor measures the opening of the window using only the accelerometer The measurement is carried out by making a double integration of the acceleration values. This example is for a 128x32 pixel display using I2C to communicate 3 pins are required to interface (two I2C and one reset). Hardware. Pinout 8 A4/SDA Analog ADC in; I2C SDA; SDA to SDA OLED Display 9 A5/SCL Analog ADC in; I2C SCL to SCK OLED Display 13 RST Digital In Active low reset input RESET to push button. Other end to ground. 14 GND Power Power Ground to Battery (-) 15 VIN Power In Vin Power input VIN to Baterry (+) 20 D2 Digital GPIO - to Open/Closed Reed Switches 27 D9/PWM Digital GPIO to Right Reed Switch. Other end to ground. 28 D10/PWM Digital GPIO; to Left Reed Switch. Other end to ground. Author: Enrique Albertos Date: 2021-04-23 **************************************************************************/ #include "SparkFunLSM6DS3.h" #include "SPI.h" #include "Wire.h" #include <Adafruit_GFX.h> #include <Adafruit_SSD1306.h> #include <Fonts/FreeMonoBold18pt7b.h> #define SCREEN_WIDTH 128 // OLED display width, in pixels #define SCREEN_HEIGHT 32 // OLED display height, in pixels #define G_CONSTANT 9.80665 // m/s^2 adjust to your location // Declaration for an SSD1306 display connected to I2C (SDA, SCL pins) #define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin) Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); long lastMillis = millis(); #define DISRIMINATION_WINDOW_X 0.019 #define DISRIMINATION_WINDOW_Y 0.019 #define DISRIMINATION_WINDOW_Z 0.1 #define LPF_SAMPLES 16.0 //16 LSM6DS3 IMU(I2C_MODE, 0x6A); uint16_t errorsAndWarnings = 0; float maximumX = 0; // last accelerometer reading float lastAccXsample; float lastAccYsample; float lastAccZsample; // buffer for logging float accSamples[1000]; float velSamples[1000]; float posSamples[1000]; long timestampSamples[1000]; int rawSamplesCount = 0; // last two readings for integration unsigned char countx; float accelerationX[2]; float velocityX[2]; float positionX[2]; float accelerationY[2]; float velocityY[2]; float positionY[2]; float accelerationZ[2]; float velocityZ[2]; float positionZ[2]; unsigned char direction; float stationaryReadingX; float stationaryReadingY; float stationaryReadingZ; // let's assume we are in closed position and window opens from left to right volatile bool openCloseChangePending = true; unsigned long lastLeftSwitchDebounceTime = 0; // the last time the input left encoder pin was toggled unsigned long OpenClosedSwitchDebounceDelay = 150; // the debounce time unsigned long lastDebouncingMeterMillis= 0; // last time the meter is stopped unsigned long accelerometerDebounceDelay = 300; // the debounce time enum WindowStateType {OPEN = HIGH, CLOSED = LOW}; int windowState = CLOSED; bool directionChanged = false; int openClosedSwitchPort = 2; // switch to ground + internal pull-up resistor. // Negative logic LOW when switch is closed void calibrateAccelerometer(void); void movementEndCheck(void); void position(void); void resetPosition(void); void displayPosition(void); void isrChangeOpenClosedSwitchPort() ; void setup() { setupSerialPort(); setupAccelerometer(); setupDisplay(); calibrateAccelerometer(); setUpOpenCLoseSwitch(); } void loop() { int16_t temp; windowState = digitalRead(openClosedSwitchPort); if(windowState == OPEN && openCloseChangePending ) { openCloseChangePending = false; if (rawSamplesCount > 3) { logDataToSerial(); } rawSamplesCount = 0; } if(windowState == CLOSED || openCloseChangePending ) { openCloseChangePending = false; rawSamplesCount = 0; resetPosition(); displayPosition(); } else { position(); } } void setupSerialPort() { Serial.begin(9600); delay(1000); } void setupDisplay() { if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32 for (;;) ; // Don't proceed, loop forever } } void setUpOpenCLoseSwitch() { pinMode(openClosedSwitchPort, INPUT_PULLUP); delay(20); // detect switch chages from open to closed. It is negative logic attachInterrupt(digitalPinToInterrupt(openClosedSwitchPort), isrChangeOpenClosedSwitchPort, CHANGE); } void setupAccelerometer() { IMU.settings.gyroEnabled = 0; IMU.settings.accelEnabled = 1; IMU.settings.accelRange = 2; //Max G force readable. Can be: 2, 4, 8, 16 IMU.settings.accelSampleRate = 416; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330 IMU.settings.accelBandWidth = 100; //Hz. Can be: 50, 100, 200, 400; IMU.settings.accelFifoEnabled = 0; //Set to include accelerometer in the FIFO IMU.settings.tempEnabled = 0; IMU.settings.fifoModeWord = 0; if (!IMU.begin() ) { Serial.print("Error at begin().\n"); } } void displayPosition(void) { display.setFont(); display.clearDisplay(); display.setCursor(0, 0); display.setTextColor(SSD1306_WHITE); display.print(accelerationX[1], 4); display.setCursor(0, 8); display.print(velocityX[1], 4); // show distance in cm float distance = sqrt(positionX[1]*positionX[1] + positionY[1] * positionY[1]+ positionZ[1] * positionZ[1]) * 100.0; display.setCursor(0, 16); display.print(distance, 4); display.setFont(&FreeMonoBold18pt7b); display.setCursor(64, 24); display.print((int)distance); display.setFont(); display.print("cm"); display.display(); } /** Interrupt Service Routine for Changing Edge in Open Close Switches */ void isrChangeOpenClosedSwitchPort() { windowState = digitalRead(openClosedSwitchPort); if ((millis() - lastLeftSwitchDebounceTime) > OpenClosedSwitchDebounceDelay) { openCloseChangePending = true; lastLeftSwitchDebounceTime = millis(); } } void resetPosition(void) { accelerationX[1] = 0; accelerationX[0] = 0; velocityX[1] = 0; velocityX[0] = 0; positionX[0] = 0; positionX[1] = 0; accelerationY[1] = 0; accelerationY[0] = 0; velocityY[1] = 0; velocityY[0] = 0; positionY[1] = 0; positionY[0] = 0; accelerationZ[1] = 0; accelerationZ[0] = 0; velocityZ[1] = 0; velocityZ[0] = 0; positionZ[1] = 0; positionZ[0] = 0; lastMillis = millis(); } // read accelerometer data void readAccelerometerData() { lastAccXsample = -IMU.readFloatAccelX(); lastAccYsample = IMU.readFloatAccelY(); lastAccZsample = IMU.readFloatAccelZ(); } void displayCalibrationInfo() { display.clearDisplay(); display.setCursor(10,10); display.fillScreen(BLACK); display.setTextColor(WHITE); display.println(F("Calibrating IMU...")); display.print(F("Sample rate: ")); display.print(IMU.settings.accelSampleRate); display.print(F(" Hz")); display.display(); delay(20); } /******************************************************************************* The purpose of the calibration routine is to obtain the value of the reference threshold. It consists on a 1024 samples average in no-movement condition. ********************************************************************************/ void calibrateAccelerometer(void) { unsigned int count1 =0; displayCalibrationInfo(); stationaryReadingX = 0.0; stationaryReadingY = 0.0; stationaryReadingZ = 0.0; // calculate mean stationary value do { readAccelerometerData(); stationaryReadingX = stationaryReadingX + lastAccXsample; // Accumulate Samples stationaryReadingY = stationaryReadingY + lastAccYsample; stationaryReadingZ = stationaryReadingZ + lastAccZsample; count1++; } while (count1 != 1024); // 1024 times stationaryReadingX = stationaryReadingX / 1024.0; stationaryReadingY = stationaryReadingY / 1024.0; stationaryReadingZ = stationaryReadingZ / 1024.0; } /* Send data buffers to serial */ void logDataToSerial() { Serial.print("START"); Serial.print(","); Serial.print(IMU.settings.accelSampleRate); Serial.print(","); Serial.print(IMU.settings.accelRange); Serial.print(","); Serial.print(IMU.settings.accelBandWidth); Serial.print(","); Serial.println(LPF_SAMPLES); for (int i = 0; i < rawSamplesCount; ++i) { Serial.print(accSamples[i], 8); Serial.print(","); Serial.print(velSamples[i], 8); Serial.print(","); Serial.print(posSamples[i], 8); Serial.print(","); Serial.print(timestampSamples[i]-timestampSamples[0]); Serial.println(); } Serial.println("END"); } /* This function allows movement end detection. If a certain number of acceleration samples are equal to zero or velocity direction has changed the sign we can assume movement has stopped. Velocity variables are reseted, this stops position increment and eliminates position error. */ void movementEndCheck(void) { if ( accelerationX[1] == 0 )// we can assume that velocity is cero // we count the number of acceleration samples that equals cero { countx++; } else { countx = 0; } if (countx > 2 || directionChanged ) { displayPosition(); countx = 0; resetAccelerationBuffer(); resetVelocityBuffer(); directionChanged = false; if (rawSamplesCount > 4) { logDataToSerial(); } rawSamplesCount = 0; lastDebouncingMeterMillis = millis(); } displayPosition(); } void resetAccelerationBuffer() { accelerationX[0] = 0.0; accelerationX[1] = 0.0; accelerationY[0] = 0.0; accelerationY[1] = 0.0; accelerationZ[0] = 0.0; accelerationZ[1] = 0.0; } void resetVelocityBuffer() { velocityX[0] = 0.0; velocityX[1] = 0.0; velocityY[0] = 0.0; velocityY[1] = 0.0; velocityZ[0] = 0.0; velocityZ[1] = 0.0; } void debounceAccelerometer() { while (millis() < lastDebouncingMeterMillis + accelerometerDebounceDelay){ readAccelerometerData(); resetAccelerationBuffer(); resetVelocityBuffer(); } } /*****************************************************************************************/ /****************************************************************************************** This function transforms acceleration to a proportional position by integrating the acceleration data twice. It also adjusts sensibility by multiplying the "positionX" and "positionY" variables. This integration algorithm carries error, which is compensated in the "movenemt_end_check" subroutine. Faster sampling frequency implies less error but requires more memory. Keep in mind that the same process is applied to the X and Y axis. *****************************************************************************************/ void position(void) { unsigned int count2; count2 = 0; debounceAccelerometer(); // filtering routine for noise attenuation average represents the acceleration of an instant do { readAccelerometerData(); accelerationX[1] = accelerationX[1] + lastAccXsample; accelerationY[1] = accelerationY[1] + lastAccYsample; accelerationZ[1] = accelerationZ[1] + lastAccZsample; count2++; } while (count2 != LPF_SAMPLES); // 64 sums of the acceleration sample // Low pass band filter accelerationX[1] = accelerationX[1] / LPF_SAMPLES; // division by 64 accelerationX[1] = accelerationX[1] - stationaryReadingX; // eliminating zero reference accelerationY[1] = accelerationY[1] / LPF_SAMPLES; // division by 64 accelerationY[1] = accelerationY[1] - stationaryReadingY; // eliminating zero reference accelerationZ[1] = accelerationZ[1] / LPF_SAMPLES; // division by 64 accelerationZ[1] = accelerationZ[1] - stationaryReadingZ; // eliminating zero reference // offset of the acceleration data // mechanical filter if ((accelerationX[1] <= DISRIMINATION_WINDOW_X) && (accelerationX[1] >=-DISRIMINATION_WINDOW_X)) // Discrimination window applied { accelerationX[1] = 0; // to the X axis acceleration } if ((accelerationY[1] <= DISRIMINATION_WINDOW_Y) && (accelerationY[1] >=-DISRIMINATION_WINDOW_Y)) // Discrimination window applied { accelerationY[1] = 0; // to the Y axis acceleration } if ((accelerationZ[1] <= DISRIMINATION_WINDOW_Z) && (accelerationZ[1] >=-DISRIMINATION_WINDOW_Z)) // Discrimination window applied { accelerationZ[1] = 0; // to the Z axis acceleration } // variable long actualTime = millis(); long ellapsedTime = actualTime - lastMillis; lastMillis = actualTime; // first X integration: velocityX[1] = velocityX[0] + (accelerationX[0] + ((accelerationX[1] - accelerationX[0]) / 2.0)) * G_CONSTANT * ellapsedTime /1000.0 ; velocityY[1] = velocityY[0] + (accelerationY[0] + ((accelerationY[1] - accelerationY[0]) / 2.0)) * G_CONSTANT * ellapsedTime /1000.0; velocityZ[1] = velocityZ[0] + (accelerationZ[0] + ((accelerationZ[1] - accelerationZ[0]) / 2.0)) * G_CONSTANT * ellapsedTime /1000.0; // second X integration: if ( (velocityX[0] * velocityX[1]) > 0) { positionX[1] = positionX[0] + (velocityX[0] + ((velocityX[1] - velocityX[0]) / 2.0)) * ellapsedTime/1000.0; positionY[1] = positionY[0] + (velocityY[0] + ((velocityY[1] - velocityY[0]) / 2.0)) * ellapsedTime/1000.0; positionZ[1] = positionZ[0] + (velocityZ[0] + ((velocityZ[1] - velocityZ[0]) / 2.0)) * ellapsedTime/1000.0; } if (rawSamplesCount == 0 ) { accSamples[rawSamplesCount] = accelerationX[0]; velSamples[rawSamplesCount] = velocityX[0]; posSamples[rawSamplesCount] = positionX[0]; timestampSamples[rawSamplesCount] = millis()-ellapsedTime; rawSamplesCount++; } accelerationX[0] = accelerationX[1]; // The current acceleration value must be sent to the previous acceleration directionChanged = (velocityX[0] * velocityX[1]) < 0; velocityX[0] = velocityX[1]; // Same done for the velocity variable positionX[0] = positionX[1]; // actual position data must be sent to the accelerationY[0] = accelerationY[1]; // The current acceleration value must be sent to the previous acceleration velocityY[0] = velocityY[1]; // Same done for the velocity variable positionY[0] = positionY[1]; // actual position data must be sent to the accelerationZ[0] = accelerationZ[1]; // The current acceleration value must be sent to the previous acceleration velocityZ[0] = velocityZ[1]; // Same done for the velocity variable positionZ[0] = positionZ[1]; // actual position data must be sent to the accSamples[rawSamplesCount] = accelerationX[1]; velSamples[rawSamplesCount] = velocityX[1]; posSamples[rawSamplesCount] = positionX[1]; timestampSamples[rawSamplesCount] = millis(); rawSamplesCount++; movementEndCheck(); direction = 0; // data variable to direction variable reset }
Next Steps
- Prepare our sensor to share your data in the cloud.
- Prepare our sensor to be able to configure it via bluetooth
- Train a Machine learning model that allows us to detect anomalies in the movement of the window to warn of the need for window maintenance.
- Get started with the design of our environmental sensor.
<< Previous VenTTracker Blog | Next VenTTracker Blog >> |
---|---|
VenTTracker #07 - Adjusting the window sensor | VenTTracker #09 - Checking and updating WiFiNINA Firmware |
Top Comments