├── LICENSE ├── README.md ├── esp32-ac-voltage-logger.ino └── libraries └── Filters ├── FilterDerivative.cpp ├── FilterDerivative.h ├── FilterOnePole.cpp ├── FilterOnePole.h ├── FilterTwoPole.cpp ├── FilterTwoPole.h ├── Filters.h ├── FloatDefine.h ├── RunningStatistics.cpp └── RunningStatistics.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Natthawut Suradet 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # esp32-ac-voltage-logger 2 | AC voltage event logger with ESP32 and ZMPT101B module. 3 | 4 | Filters library from >>> https://github.com/JonHub/Filters -------------------------------------------------------------------------------- /esp32-ac-voltage-logger.ino: -------------------------------------------------------------------------------- 1 | #include "FS.h" 2 | #include "SD.h" 3 | #include "SPI.h" 4 | #include 5 | #include 6 | 7 | const char* ssid = "xxxxxxx"; 8 | const char* password = "xxxxxxx"; 9 | 10 | const char* ntpServer = "pool.ntp.org"; 11 | const long gmtOffset_sec = 25200; 12 | const int daylightOffset_sec = 0; 13 | 14 | String dataMessage; 15 | String timeStamp; 16 | 17 | // Run function every PERIOD time (mS) 18 | // logSDCard 19 | unsigned long target_time = 0L; 20 | #define PERIOD (1000L) 21 | // getTimeServer 22 | unsigned long target_time2 = 0L; 23 | #define PERIOD2 (12*60*60*1000L) 24 | 25 | /* This code works with ESP8266 12E or Arduino and ZMPT101B AC voltage sensor up to 250 VAC 50/60Hz 26 | * It permits the measure of True RMS value of any AC signal, not only sinewave 27 | * The code uses the Sigma "Standard deviation" method and displays the value every "printPeriod" 28 | * check www.SurtrTech.com for more details 29 | */ 30 | 31 | #include //Library to use 32 | 33 | #define ZMPT101B 36 //Analog input 34 | 35 | float testFrequency = 50; // test signal frequency (Hz) 36 | float windowLength = 5/testFrequency; // how long to average the signal, for statistist, changing this can have drastic effect 37 | // Test as you need 38 | 39 | int RawValue = 0; 40 | float Volts_TRMS; // estimated actual voltage in Volts 41 | float Volts_TRMS_Prev; 42 | 43 | float intercept = -5; // to be adjusted based on calibration testin 44 | float slope = 0.54353; 45 | 46 | /* How to get the intercept and slope? First keep them like above, intercept=0 and slope=1, 47 | * also below keep displaying Calibrated and non calibrated values to help you in this process. 48 | * Put the AC input as 0 Volts, upload the code and check the serial monitor, normally you should have 0 49 | * if you see another value and it is stable then the intercept will be the opposite of that value 50 | * Example you upload first time and then you see a stable 1.65V so the intercept will be -1.65 51 | * To set the slope now you need to put the voltage at something higher than 0, and measure that using your reference TRMS multimeter 52 | * upload the new code with the new intercept and check the value displayed as calibrated values 53 | * Slope = (Measured values by multimeter)/(Measured values by the code) 54 | * Place your new slope and reupload the code, if you have problems with calibration try to adjust them both 55 | * or add a new line to calibrate further 56 | * Slope and intercept have nothing to do with the TRMS calculation, it's just an adjustement of the line of solutions 57 | */ 58 | 59 | 60 | unsigned long printPeriod = 50; //Measuring frequency, every 1s, can be changed 61 | unsigned long previousMillis = 0; 62 | 63 | RunningStatistics inputStats; //This class collects the value so we can apply some functions 64 | 65 | #define LED_READVOLTAGE 2 66 | #define LED_SDCARD 32 67 | #define LED_AC_OK 27 68 | #define LED_AC_FAIL 25 69 | #define LED_HEARTBEAT 12 70 | int LED_BLINK_State = LOW; // Variable to store the LED state 71 | 72 | struct tm timeinfo; 73 | char timeStringBuff[50]; // timestamp string buffer for logSDCard function 74 | 75 | bool AC_FAIL = false; 76 | 77 | int ledStateHB = LOW; 78 | unsigned long previousMillisHB = 0; 79 | const long intervalHB = 1000; 80 | 81 | // ================================================================================================================ 82 | 83 | void setup() { 84 | pinMode(LED_READVOLTAGE, OUTPUT); 85 | pinMode(LED_SDCARD, OUTPUT); 86 | pinMode(LED_AC_OK, OUTPUT); 87 | pinMode(LED_AC_FAIL, OUTPUT); 88 | pinMode(LED_HEARTBEAT, OUTPUT); 89 | 90 | inputStats.setWindowSecs( windowLength ); 91 | Serial.begin(115200); // start the serial port 92 | Serial.println("Serial started"); 93 | 94 | // ================== 95 | Serial.println("===================================================="); 96 | Serial.println("===================================================="); 97 | 98 | //connect to WiFi and get time from NTP server 99 | getTimeServer(); 100 | 101 | if(!SD.begin()){ 102 | Serial.println("Card Mount Failed"); 103 | return; 104 | } 105 | uint8_t cardType = SD.cardType(); 106 | 107 | if(cardType == CARD_NONE){ 108 | Serial.println("No SD card attached"); 109 | return; 110 | } 111 | 112 | Serial.print("SD Card Type: "); 113 | if(cardType == CARD_MMC){ 114 | Serial.println("MMC"); 115 | } else if(cardType == CARD_SD){ 116 | Serial.println("SDSC"); 117 | } else if(cardType == CARD_SDHC){ 118 | Serial.println("SDHC"); 119 | } else { 120 | Serial.println("UNKNOWN"); 121 | } 122 | 123 | uint64_t cardSize = SD.cardSize() / (1024 * 1024); 124 | Serial.printf("SD Card Size: %lluMB\n", cardSize); 125 | 126 | // If the data.csv file doesn't exist 127 | // Create a file on the SD card and write the data labels 128 | File file = SD.open("/data.csv"); 129 | if(!file) { 130 | Serial.println("File doens't exist"); 131 | Serial.println("Creating file..."); 132 | writeFile(SD, "/data.csv", "Timestamp,Voltage,Status\r\n"); 133 | } 134 | else { 135 | Serial.println("File already exists"); 136 | } 137 | file.close(); 138 | 139 | for (int i=0; i<10000; i++) readVoltage(); 140 | } 141 | 142 | // ================================================================================================================ 143 | 144 | void loop() { 145 | readVoltage(); //The only function I'm running, be careful when using with this kind of boards 146 | //Do not use very long delays, or endless loops inside the loop 147 | 148 | if(Volts_TRMS > 200) { // 10% of 220V 149 | digitalWrite(LED_AC_OK, HIGH); // Update the LED state 150 | digitalWrite(LED_AC_FAIL, LOW); // Update the LED state 151 | Volts_TRMS_Prev = Volts_TRMS; 152 | if(AC_FAIL == true) { 153 | logSDCard(Volts_TRMS_Prev, false); // Write to SD card 154 | AC_FAIL = false; 155 | } 156 | } else { 157 | digitalWrite(LED_AC_OK, LOW); // Update the LED state 158 | digitalWrite(LED_AC_FAIL, HIGH); // Update the LED state 159 | AC_FAIL = true; 160 | if(millis() - target_time >= PERIOD) { 161 | target_time += PERIOD; 162 | if(Volts_TRMS - Volts_TRMS_Prev < -50) { // Different between current voltage and previous voltage more than 50 V 163 | Volts_TRMS_Prev = Volts_TRMS; 164 | logSDCard(Volts_TRMS_Prev, true); // Write to SD card 165 | } 166 | } 167 | } 168 | 169 | if(millis() - target_time2 >= PERIOD2) { 170 | target_time2 += PERIOD2; // change scheduled time exactly, no slippage will happen 171 | getTimeServer(); // Update time from NTP every 12 hours 172 | } 173 | 174 | // Heartbeat LED blink 175 | unsigned long currentMillisHB = millis(); 176 | if (currentMillisHB - previousMillisHB >= intervalHB) { 177 | previousMillisHB = currentMillisHB; 178 | if (ledStateHB == LOW) { 179 | ledStateHB = HIGH; 180 | } else { 181 | ledStateHB = LOW; 182 | } 183 | digitalWrite(LED_HEARTBEAT, ledStateHB); 184 | } 185 | } 186 | 187 | // ================================================================================================================ 188 | 189 | void getTimeServer(){ 190 | Serial.printf("Connecting to %s ", ssid); 191 | WiFi.begin(ssid, password); 192 | while (WiFi.status() != WL_CONNECTED) { 193 | delay(500); 194 | Serial.print("."); 195 | } 196 | Serial.println(" CONNECTED"); 197 | Serial.print("IP address: "); 198 | Serial.println(WiFi.localIP()); 199 | delay(500); 200 | 201 | //init and get the time 202 | configTime(gmtOffset_sec, daylightOffset_sec, ntpServer); 203 | printLocalTime(); 204 | 205 | //disconnect WiFi as it's no longer needed 206 | WiFi.disconnect(true); 207 | WiFi.mode(WIFI_OFF); 208 | Serial.println("WIFI DISCONNECTED"); 209 | } 210 | 211 | void printLocalTime(){ 212 | Serial.print("Connecting to NTP server ..."); 213 | if(!getLocalTime(&timeinfo)){ 214 | Serial.println("Failed to obtain time"); 215 | return; 216 | } 217 | Serial.println(" CONNECTED"); 218 | Serial.print("DateTime: "); 219 | Serial.println(&timeinfo, "%Y-%m-%d %H:%M:%S"); 220 | } 221 | 222 | float readVoltage(){ 223 | LED_BLINK_State = !LED_BLINK_State; // Toggle the LED state 224 | digitalWrite(LED_READVOLTAGE, LED_BLINK_State); // Update the LED state 225 | 226 | RawValue = analogRead(ZMPT101B); // read the analog in value: 227 | inputStats.input(RawValue); // log to Stats function 228 | 229 | if((unsigned long)(millis() - previousMillis) >= printPeriod) { // We calculate and display every 1s 230 | previousMillis = millis(); // update time 231 | 232 | Volts_TRMS = inputStats.sigma()* slope + intercept; 233 | // Volts_TRMS = Volts_TRMS*0.979; // Further calibration if needed 234 | 235 | // Serial.print("Non Calibrated: "); 236 | // Serial.print("\t"); 237 | // Serial.print(inputStats.sigma()); 238 | // Serial.print("\t"); 239 | // Serial.print("Calibrated: "); 240 | Serial.print("Voltage: "); 241 | Serial.println(Volts_TRMS); 242 | } 243 | } 244 | 245 | // Write the sensor readings on the SD card 246 | void logSDCard(float Volts_TRMS_SD, bool AC_FAIL_SD) { 247 | getLocalTime(&timeinfo); 248 | strftime(timeStringBuff, sizeof(timeStringBuff), "%Y-%m-%d %H:%M:%S", &timeinfo); 249 | timeStamp = String(timeStringBuff); 250 | 251 | if(AC_FAIL_SD == false){ 252 | dataMessage = timeStamp + "," + String(Volts_TRMS_SD) + ",OK" + "\r\n"; 253 | } else { 254 | dataMessage = timeStamp + "," + String(Volts_TRMS_SD) + ",Fault" + "\r\n"; 255 | } 256 | Serial.print("Save data: "); 257 | Serial.print(dataMessage); 258 | appendFile(SD, "/data.csv", dataMessage.c_str()); 259 | } 260 | 261 | void writeFile(fs::FS &fs, const char * path, const char * message){ 262 | Serial.printf("Writing file: %s\n", path); 263 | 264 | File file = fs.open(path, FILE_WRITE); 265 | if(!file){ 266 | Serial.println("Failed to open file for writing"); 267 | return; 268 | } 269 | if(file.print(message)){ 270 | Serial.println("File written"); 271 | } else { 272 | Serial.println("Write failed"); 273 | } 274 | file.close(); 275 | } 276 | 277 | void appendFile(fs::FS &fs, const char * path, const char * message){ 278 | Serial.printf("Appending to file: %s\n", path); 279 | 280 | File file = fs.open(path, FILE_APPEND); 281 | if(!file){ 282 | Serial.println("Failed to open file for appending"); 283 | return; 284 | } 285 | if(file.print(message)){ 286 | Serial.println("Message appended"); 287 | digitalWrite(LED_SDCARD, HIGH); 288 | } else { 289 | Serial.println("Append failed"); 290 | } 291 | file.close(); 292 | digitalWrite(LED_SDCARD, LOW); 293 | } -------------------------------------------------------------------------------- /libraries/Filters/FilterDerivative.cpp: -------------------------------------------------------------------------------- 1 | #include "FilterDerivative.h" 2 | #include "Arduino.h" 3 | 4 | float FilterDerivative::input( float inVal ) { 5 | long thisUS = micros(); 6 | float dt = 1e-6*float(thisUS - LastUS); // cast to float here, for math 7 | LastUS = thisUS; // update this now 8 | 9 | Derivative = (inVal-LastInput) / dt; 10 | 11 | LastInput = inVal; 12 | return output(); 13 | } 14 | 15 | float FilterDerivative::output() { return Derivative; } 16 | 17 | void testFilterDerivative() { 18 | FilterDerivative der; 19 | 20 | while(true) { 21 | float t = 1e-6 * float( micros() ); 22 | float value = 100*sin(TWO_PI*t); 23 | 24 | der.input(value); 25 | 26 | Serial.print( "\n" ); 27 | Serial.print( value ); Serial.print( "\t"); 28 | Serial.print( der.output() ); 29 | 30 | delay(10); 31 | } 32 | } -------------------------------------------------------------------------------- /libraries/Filters/FilterDerivative.h: -------------------------------------------------------------------------------- 1 | #ifndef FilterDerivative_h 2 | #define FilterDerivative_h 3 | 4 | // returns the derivative 5 | struct FilterDerivative { 6 | long LastUS; 7 | float LastInput; 8 | 9 | float Derivative; 10 | 11 | float input( float inVal ); 12 | 13 | float output(); 14 | }; 15 | 16 | void testFilterDerivative(); 17 | 18 | #endif -------------------------------------------------------------------------------- /libraries/Filters/FilterOnePole.cpp: -------------------------------------------------------------------------------- 1 | #include "FilterOnePole.h" 2 | #include "FloatDefine.h" 3 | 4 | FilterOnePole::FilterOnePole( FILTER_TYPE ft, float fc, float initialValue ) { 5 | setFilter( ft, fc, initialValue ); 6 | } 7 | 8 | void FilterOnePole::setFilter( FILTER_TYPE ft, float fc, float initialValue ) { 9 | FT = ft; 10 | setFrequency( fc ); 11 | 12 | Y = initialValue; 13 | Ylast = initialValue; 14 | X = initialValue; 15 | 16 | LastUS = micros(); 17 | } 18 | 19 | float FilterOnePole::input( float inVal ) { 20 | long time = micros(); 21 | ElapsedUS = float(time - LastUS); // cast to float here, for math 22 | LastUS = time; // update this now 23 | 24 | // shift the data values 25 | Ylast = Y; 26 | X = inVal; // this is now the most recent input value 27 | 28 | // filter value is controlled by a parameter called X 29 | // tau is set by the user in microseconds, but must be converted to samples here 30 | TauSamps = TauUS / ElapsedUS; 31 | 32 | float ampFactor; 33 | #ifdef ARM_FLOAT 34 | ampFactor = expf( -1.0 / TauSamps ); // this is 1 if called quickly 35 | #else 36 | ampFactor = exp( -1.0 / TauSamps ); // this is 1 if called quickly 37 | #endif 38 | 39 | Y = (1.0-ampFactor)*X + ampFactor*Ylast; // set the new value 40 | 41 | return output(); 42 | } 43 | 44 | void FilterOnePole::setFrequency( float newFrequency ) { 45 | setTau( 1.0/(TWO_PI*newFrequency ) ); // τ=1/ω 46 | } 47 | 48 | void FilterOnePole::setTau( float newTau ) { 49 | TauUS = newTau * 1e6; 50 | } 51 | 52 | float FilterOnePole::output() { 53 | // figure out which button to read 54 | switch (FT) { 55 | case LOWPASS: 56 | // return the last value 57 | return Y; 58 | break; 59 | case INTEGRATOR: 60 | // using a lowpass, but normaize 61 | return Y * (TauUS/1.0e6); 62 | break; 63 | case HIGHPASS: 64 | // highpass is the _difference_ 65 | return X-Y; 66 | break; 67 | case DIFFERENTIATOR: 68 | // like a highpass, but normalize 69 | return (X-Y)/(TauUS/1.0e6); 70 | break; 71 | default: 72 | // should never get to here, return 0 just in case 73 | return 0; 74 | } 75 | } 76 | 77 | void FilterOnePole::print() { 78 | Serial.println(""); 79 | Serial.print(" Y: "); Serial.print( Y ); 80 | Serial.print(" Ylast: "); Serial.print( Ylast ); 81 | Serial.print(" X "); Serial.print( X ); 82 | Serial.print(" ElapsedUS "); Serial.print( ElapsedUS ); 83 | Serial.print(" TauSamps: "); Serial.print( TauSamps ); 84 | //Serial.print(" ampFactor " ); Serial.print( ampFactor ); 85 | Serial.print(" TauUS: "); Serial.print( TauUS ); 86 | Serial.println(""); 87 | } 88 | 89 | void FilterOnePole::test() { 90 | float tau = 10; 91 | float updateInterval = 1; 92 | float nextupdateTime = millis()*1e-3; 93 | 94 | float inputValue = 0; 95 | FilterOnePole hp( HIGHPASS, tau, inputValue ); 96 | FilterOnePole lp( LOWPASS, tau, inputValue ); 97 | 98 | while( true ) { 99 | float now = millis()*1e-3; 100 | 101 | // switch input values on a 20 second cycle 102 | if( round(now/20.0)-(now/20.0) < 0 ) 103 | inputValue = 0; 104 | else 105 | inputValue = 100; 106 | 107 | hp.input(inputValue); 108 | lp.input(inputValue); 109 | 110 | if( now > nextupdateTime ) { 111 | nextupdateTime += updateInterval; 112 | 113 | Serial.print("inputValue: "); Serial.print( inputValue ); 114 | Serial.print("\t high-passed: "); Serial.print( hp.output() ); 115 | Serial.print("\t low-passed: "); Serial.print( lp.output() ); 116 | Serial.println(); 117 | } 118 | } 119 | } 120 | 121 | void FilterOnePole::setToNewValue( float newVal ) { 122 | Y = Ylast = X = newVal; 123 | } 124 | 125 | 126 | // stuff for filter2 (lowpass only) 127 | // should be able to set a separate fall time as well 128 | FilterOnePoleCascade::FilterOnePoleCascade( float riseTime, float initialValue ) { 129 | setRiseTime( riseTime ); 130 | setToNewValue( initialValue ); 131 | } 132 | 133 | void FilterOnePoleCascade::setRiseTime( float riseTime ) { 134 | float tauScale = 3.36; // found emperically, by running test(); 135 | 136 | Pole1.setTau( riseTime / tauScale ); 137 | Pole2.setTau( riseTime / tauScale ); 138 | } 139 | 140 | float FilterOnePoleCascade::input( float inVal ) { 141 | Pole2.input( Pole1.input( inVal )); 142 | return output(); 143 | } 144 | 145 | // clears out the values in the filter 146 | void FilterOnePoleCascade::setToNewValue( float newVal ) { 147 | Pole1.setToNewValue( newVal ); 148 | Pole2.setToNewValue( newVal ); 149 | } 150 | 151 | float FilterOnePoleCascade::output() { 152 | return Pole2.output(); 153 | } 154 | 155 | void FilterOnePoleCascade::test() { 156 | // make a filter, how fast does it run: 157 | 158 | float rise = 1.0; 159 | FilterOnePoleCascade myFilter( rise ); 160 | 161 | // first, test the filter speed ... 162 | long nLoops = 1000; 163 | 164 | Serial.print( "testing filter with a rise time of "); 165 | Serial.print( rise ); Serial.print( "s" ); 166 | 167 | Serial.print( "\n running filter speed loop ... "); 168 | 169 | float startTime, stopTime; 170 | 171 | startTime = millis()*1e-3; 172 | for( long i=0; i 0.1 && !crossedTenPercent ) { 191 | // filter first crossed the 10% point 192 | startTime = millis()*1e-3; 193 | crossedTenPercent = true; 194 | } 195 | } 196 | stopTime = millis()*1e-3; 197 | 198 | Serial.print( "done, rise time: " ); Serial.print( stopTime-startTime ); 199 | 200 | Serial.print( "testing attenuation at f = 1/risetime" ); 201 | 202 | myFilter.setToNewValue( 0.0 ); 203 | 204 | float maxVal = 0; 205 | float valWasOutputThisCycle = true; 206 | 207 | float lastFilterVal = 0; 208 | 209 | while( true ) { 210 | float now = 1e-3*millis(); 211 | 212 | float currentFilterVal = myFilter.input( sin( TWO_PI*now) ); 213 | 214 | if( currentFilterVal < 0.0 ) { 215 | if( !valWasOutputThisCycle ) { 216 | // just crossed below zero, output the max 217 | Serial.print( maxVal*100 ); Serial.print( " %\n" ); 218 | valWasOutputThisCycle = true; 219 | } 220 | 221 | } 222 | 223 | } 224 | 225 | 226 | 227 | } 228 | -------------------------------------------------------------------------------- /libraries/Filters/FilterOnePole.h: -------------------------------------------------------------------------------- 1 | #ifndef FilterOnePole_h 2 | #define FilterOnePole_h 3 | 4 | #include 5 | 6 | enum FILTER_TYPE { 7 | HIGHPASS, 8 | LOWPASS, 9 | INTEGRATOR, 10 | DIFFERENTIATOR 11 | }; 12 | 13 | // the recursive filter class implements a recursive filter (low / pass / highpass 14 | // note that this must be updated in a loop, using the most recent acquired values and the time acquired 15 | // Y = a0*X + a1*Xm1 16 | // + b1*Ylast 17 | struct FilterOnePole { 18 | FILTER_TYPE FT; 19 | float TauUS; // decay constant of the filter, in US 20 | float TauSamps; // tau, measued in samples (this changes, depending on how long between input()s 21 | 22 | // filter values - these are public, but should not be set externally 23 | float Y; // most recent output value (gets computed on update) 24 | float Ylast; // prevous output value 25 | 26 | float X; // most recent input value 27 | 28 | // elapsed times are kept in long, and will wrap every 29 | // 35 mins, 47 seconds ... however, the wrap does not matter, 30 | // because the delta will still be correct (always positive and small) 31 | float ElapsedUS; // time since last update 32 | long LastUS; // last time measured 33 | 34 | FilterOnePole( FILTER_TYPE ft=LOWPASS, float fc=1.0, float initialValue=0 ); 35 | 36 | // sets or resets the parameters and state of the filter 37 | void setFilter( FILTER_TYPE ft, float tauS, float initialValue ); 38 | 39 | void setFrequency( float newFrequency ); 40 | 41 | void setTau( float newTau ); 42 | 43 | float input( float inVal ); 44 | 45 | float output(); 46 | 47 | void print(); 48 | 49 | void test(); 50 | 51 | void setToNewValue( float newVal ); // resets the filter to a new value 52 | }; 53 | 54 | // two pole filter, these are very useful 55 | struct FilterOnePoleCascade { 56 | 57 | FilterOnePole Pole1; 58 | FilterOnePole Pole2; 59 | 60 | FilterOnePoleCascade( float riseTime=1.0, float initialValue=0 ); // rise time to step function, 10% to 90% 61 | 62 | // rise time is 10% to 90%, for a step input 63 | void setRiseTime( float riseTime ); 64 | 65 | void setToNewValue( float newVal ); 66 | 67 | float input( float inVal ); 68 | 69 | float output(); 70 | 71 | void test(); 72 | }; 73 | 74 | 75 | #endif 76 | 77 | -------------------------------------------------------------------------------- /libraries/Filters/FilterTwoPole.cpp: -------------------------------------------------------------------------------- 1 | #include "FilterTwoPole.h" 2 | 3 | #ifdef ARDUINO_SAM_DUE 4 | #define ARM_FLOAT 5 | #endif 6 | 7 | // The driven, damped harmonic oscillator equation is: 8 | // http://en.wikipedia.org/wiki/Harmonic_oscillator) 9 | // 10 | // a + 2*ζ*ω0*v + ω0²*x = F(t)/m 11 | // 12 | // where the quality factor is related to the damping factor by 13 | // 14 | // Q = ½ζ 15 | // 16 | // It is useful to normalize the force to the spring constant, 17 | // so a force of F will result in the oscillator resting at a position x=F 18 | // The allows the oscillator to be used as a lowpass filter, 19 | // where the position X functions as the output voltage V. 20 | // Setting a=v=0, and solving for m, gives 21 | // 22 | // m = 1/w0 23 | // 24 | // and the final equation is 25 | // 26 | // a + 2*ζ*ω0*v + ω0²*x = ω0²*F(t) 27 | // 28 | // for determining the energy, you must know the effective spring constant, which is 29 | // found from the equation (for undampened, where w0 = w) 30 | // 31 | // w0² = k/m 32 | // w0 = k 33 | // 34 | // this makes the energy 35 | // 36 | // E = 0.5*k*x² + 0.5*m*v² 37 | // = 0.5*w0*x² + 0.5*v²/w0 38 | // 39 | // 40 | // Filter types (such as Bessel or Butterworth) are defined by 41 | // specific quality factors ... the quality factor also defines a 42 | // relationship between f0 (the undamped resonance frequency) 43 | // and the -3 dB frequency of the filter 44 | // 45 | // For a Butterworth filter, these values are 46 | // (see figures 8.26 and 8.32 Bessel) 47 | // (See Analog Devices note AN-649) 48 | // http://www.analog.com/static/imported-files/application_notes/447952852AN-649_0.pdf 49 | // http://www.analog.com/library/analogDialogue/archives/43-09/EDCh%208%20filter.pdf) 50 | // 51 | // Butterworth 52 | // F_0=1 53 | // F_(-3dB)=1 54 | // Q=1/√2 55 | // 56 | // Bessel 57 | // F_0=1.2754 58 | // F_(-3dB)=1 59 | // Q=1/√3 60 | // 61 | // (note – ramp time for a Bessel filter is about 1/(2F_0 ) ) 62 | 63 | 64 | FilterTwoPole::FilterTwoPole( float frequency0, float qualityFactor, float xInit ) { 65 | X = xInit; // start it some arbitrary position 66 | Vprev = 0; // initially stopped 67 | IsHighpass = false; // by default, a normal oscillator 68 | 69 | setQ( qualityFactor ); 70 | setFrequency0( frequency0 ); 71 | 72 | LastTimeUS = micros(); 73 | } 74 | 75 | void FilterTwoPole::setQ( float qualityFactor ) { 76 | // zero will result in divide by zero, upper value keeps it stable 77 | qualityFactor = constrain( qualityFactor, 1e-3, 1e3 ); 78 | 79 | Q = qualityFactor; 80 | } 81 | 82 | void FilterTwoPole::setFrequency0( float f ) { 83 | W0 = TWO_PI*abs(f); 84 | } 85 | 86 | void FilterTwoPole::setAsFilter( OSCILLATOR_TYPE ft, float frequency3db, float initialValue ) { 87 | // if this is a highpass filter, set to invert the transfer function on the output 88 | //if( ft == HIGHPASS_BESSEL || ft == HIGHPASS_BUTTERWORTH ) { 89 | // IsHighpass = true; 90 | //} 91 | //else { 92 | IsHighpass = false; 93 | //} 94 | 95 | X = initialValue; 96 | 97 | if( ft == LOWPASS_BESSEL ) { 98 | setFrequency0( frequency3db * 1.28 ); 99 | setQ( 0.5774 ); 100 | } 101 | //else if( ft == HIGHPASS_BESSEL ) { 102 | // setFrequency0( frequency3db * 1.28 ); 103 | // setQ( 0.5774 ); 104 | //} 105 | 106 | else if( ft == LOWPASS_BUTTERWORTH ) { 107 | // set as butterworth 108 | setFrequency0( frequency3db ); 109 | setQ( 0.7071 ); 110 | } 111 | //else if( ft == HIGHPASS_BUTTERWORTH ) { 112 | // set as butterworth 113 | // setFrequency0( frequency3db ); 114 | // setQ( 0.7071 ); 115 | //} 116 | 117 | 118 | } 119 | 120 | float FilterTwoPole::input( float drive ) { 121 | Fprev = drive; // needed when using filter as a highpass 122 | 123 | long now = micros(); // get current time 124 | float dt = 1e-6*float(now - LastTimeUS); // find dt 125 | LastTimeUS = now; // save the last time 126 | 127 | // constrain the dt 128 | // if input has not been called frequently enough 129 | // the velocity and position can fly off to extremly high values 130 | // ... constraining the dt effectively "pauses" the motion during delays in updating 131 | // note this will result in an incorrect answer, but if dt is too large 132 | // the answer will be incorrect, regardless. 133 | dt = constrain( dt, 0, 1.0/W0 ); 134 | 135 | float A = sq(W0)*drive - W0/Q*Vprev - sq(W0)*X; // *** compute acceleration 136 | float V = Vprev + A * dt; // step velocity 137 | Vavg = .5*(V+Vprev); 138 | X += Vavg * dt; // step position, using average V to reduce error 139 | // (trapezoidal integration) 140 | 141 | Vprev = V; // save the last V 142 | 143 | // normally, this returns output 144 | // use it here to figure out how to return highpass 145 | 146 | //return Q/W0*Vavg; 147 | return output(); 148 | } 149 | 150 | float FilterTwoPole::output() { 151 | // if( IsHighpass ) 152 | // return Fprev-X-Q/W0*Vavg; // this is almost correct ... 153 | // else 154 | return X; // the filtered value (position of oscillator) 155 | } 156 | 157 | // as a measure for the energy of the oscillator, returns the maxium amplitude 158 | float FilterTwoPole::getMaxAmp() { 159 | // first, calculate the energy 160 | // E = 0.5*w0*x² + 0.5*v²/w0 161 | 162 | float E = 0.5 * W0 * sq(X) + 0.5 * sq(Vprev) / W0; 163 | 164 | // calculate use this to calculate max amplitude 165 | // E = 0.5*w0*x² 166 | // x = sqrt(2*E/w0) 167 | #ifdef ARM_FLOAT 168 | return sqrtf(2.0*E/W0); 169 | #else 170 | return sqrt(2.0*E/W0); 171 | #endif 172 | } 173 | 174 | void FilterTwoPole::print() { 175 | Serial.print(" X: "); Serial.print( X ); 176 | Serial.print(" Vprev: "); Serial.print( Vprev ); 177 | Serial.println(""); 178 | } 179 | 180 | void FilterTwoPole::test() { 181 | float updateInterval = .1; 182 | float nextupdateTime = 1e-6*float(micros()); 183 | 184 | float inputValue = 0; 185 | FilterTwoPole osc( 0.2, 4, 0); 186 | 187 | while( true ) { 188 | float now = 1e-6*float(micros()); 189 | 190 | // switch input values on a 20 second cycle 191 | if( round(now/50.0)-(now/50.0) < 0 ) 192 | inputValue = 100; 193 | else 194 | inputValue = 150; 195 | 196 | osc.input(inputValue); 197 | 198 | // analogWrite(10,osc.output() ); // hardcoded the dial pin 199 | 200 | if( now > nextupdateTime ) { 201 | nextupdateTime += updateInterval; 202 | 203 | Serial.print("inputValue: "); Serial.print( inputValue ); 204 | Serial.print("\t output: "); Serial.print( osc.output() ); 205 | Serial.println(); 206 | } 207 | } 208 | } 209 | -------------------------------------------------------------------------------- /libraries/Filters/FilterTwoPole.h: -------------------------------------------------------------------------------- 1 | #ifndef FilterTwoPole_h 2 | #define FilterTwoPole_h 3 | #include 4 | #define ATTODUINO 5 | 6 | enum OSCILLATOR_TYPE { 7 | LOWPASS_BESSEL, 8 | //HIGHPASS_BESSEL, 9 | LOWPASS_BUTTERWORTH, 10 | //HIGHPASS_BUTTERWORTH, 11 | }; 12 | 13 | // implements a driven harmonic oscillator, which can be used as a filter 14 | // mass is normalized to unity (does not appear explicity in the equations), 15 | // and the driving force is writtin in units of spring constant, so a static force 16 | // of X will cause the oscillator come to rest at X 17 | // 18 | // The oscillator parameters are 19 | // W0 (undampened resonant frequency) ... the user sets this in Hz (as F0) 20 | // Q (quality factor) 21 | // 22 | // In addition, the ocillator can be configured as a (two-pole) lowpass or high filter, 23 | // since these circuits are directly analogous to harmonic oscillators. 24 | 25 | // note that the two-pole lowpass Bessel, the rise time (for a step) and 26 | // and impulse response width are approx tau/2.0, or 1/(2*w0): 27 | // a tau = 1 is w0 (and wc) of TWO_PI 28 | // this has a rise time / impulse response of about 0.4 29 | // (i.e., the intergration time is about 0.4 * tau) 30 | 31 | struct FilterTwoPole { 32 | 33 | //OSCILLATOR_TYPE FT; 34 | float X; // position 35 | float Vprev; // previously computed velocity 36 | float Vavg; // average of the last two calculated velocities 37 | float Fprev; // previous driving force (not frequency!) 38 | 39 | float Q; // quality factor, must be > 0 40 | float W0; // undamped resonance frequency 41 | 42 | bool IsHighpass; // false for normal output, true will make a lowpass into a highpass 43 | 44 | long LastTimeUS; // last time measured 45 | 46 | FilterTwoPole( float frequency0 = 1, float qualityFactor = 1, float xInit = 0); 47 | 48 | void setQ( float qualityFactor ); 49 | 50 | void setFrequency0( float f ); 51 | 52 | void setAsFilter( OSCILLATOR_TYPE ft, float frequency3db, float initialValue=0 ); 53 | 54 | float input( float drive = 0 ); 55 | 56 | float output(); 57 | 58 | // as a measure for the energy of the oscillator, returns the maxium amplitude 59 | float getMaxAmp(); 60 | 61 | void print(); 62 | 63 | void test(); 64 | 65 | }; 66 | 67 | 68 | #endif 69 | 70 | -------------------------------------------------------------------------------- /libraries/Filters/Filters.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014 Jonathan Driscoll 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef Filters_h 16 | #define Filters_h 17 | 18 | #include "FilterOnePole.h" 19 | #include "FilterTwoPole.h" 20 | #include "FilterDerivative.h" 21 | #include "RunningStatistics.h" 22 | 23 | #endif -------------------------------------------------------------------------------- /libraries/Filters/FloatDefine.h: -------------------------------------------------------------------------------- 1 | #ifndef FloatDefine_h 2 | #define FloatDefine_h 3 | 4 | 5 | 6 | #endif -------------------------------------------------------------------------------- /libraries/Filters/RunningStatistics.cpp: -------------------------------------------------------------------------------- 1 | #include "RunningStatistics.h" 2 | #include "FloatDefine.h" 3 | 4 | RunningStatistics::RunningStatistics() { 5 | setWindowSecs( 1 ); // setup with one second average 6 | setInitialValue( 0 ); // start with zero 7 | } 8 | 9 | void RunningStatistics::setWindowSecs( float windowSecs ) { 10 | AverageSecs = windowSecs; 11 | 12 | averageValue.setRiseTime( AverageSecs ); 13 | averageSquareValue.setRiseTime( AverageSecs ); 14 | } 15 | 16 | void RunningStatistics::setInitialValue( float initialMean, float initialSigma ) { 17 | averageValue.setToNewValue( initialMean ); 18 | averageSquareValue.setToNewValue( sq(initialMean) + sq(initialSigma ) ); 19 | } 20 | 21 | void RunningStatistics::input( float inVal ) { 22 | averageValue.input(inVal); // calculates running average 23 | averageSquareValue.input(inVal*inVal); // calculates running average of square 24 | } 25 | 26 | float RunningStatistics::mean() { 27 | return averageValue.output(); 28 | } 29 | 30 | float RunningStatistics::variance() { 31 | float var = averageSquareValue.output() - averageValue.output()*averageValue.output(); 32 | 33 | // because of the filtering, it's possible that this could be negative ... check! 34 | if( var < 0 ) var = 0; 35 | 36 | return var; 37 | } 38 | 39 | float RunningStatistics::sigma() { 40 | 41 | #ifdef ARM_FLOAT 42 | return sqrtf(variance()); 43 | #else 44 | return sqrt(variance()); 45 | #endif 46 | 47 | } 48 | 49 | float RunningStatistics::CV() { 50 | static const float maxCV = 1e3; 51 | float meanTmp = mean(); 52 | 53 | // prevent divide by zero 54 | if( meanTmp == 0 ) return maxCV; 55 | else return sigma() / meanTmp; 56 | } 57 | 58 | void testRunningStatistics() { 59 | // a speed test for running statistics 60 | 61 | RunningStatistics myStats; 62 | 63 | myStats.setInitialValue( (1.0/1024)*float(analogRead( A0 )) ); 64 | 65 | float updateInterval = 1.0; 66 | float nextUpdateTime = 1e-6*float(micros()) + updateInterval; 67 | 68 | long nLoops = 0; 69 | 70 | while( true ) { 71 | myStats.input( (1.0/1024)*float(analogRead( A0 )) ); 72 | nLoops++; 73 | float t = 1e-6*float(micros()); 74 | 75 | if( t > nextUpdateTime ) { 76 | nextUpdateTime += updateInterval; 77 | 78 | Serial.print( "mean: "); Serial.print( myStats.mean() ); 79 | Serial.print( "\tsigma: " ); Serial.print( myStats.sigma() ); 80 | Serial.print( "\tHz: "); Serial.print( nLoops ); 81 | 82 | nLoops = 0; 83 | Serial.print("\n"); 84 | } 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /libraries/Filters/RunningStatistics.h: -------------------------------------------------------------------------------- 1 | #ifndef RunningStatistics_h 2 | #define RunningStatistics_h 3 | 4 | #include "FilterOnePole.h" 5 | 6 | struct RunningStatistics { 7 | // in statistics, SigmaSqr is: 8 | // σ^2 = - ^2 9 | // averages can be taken by low-pass smoothing with a (two-pole) filter 10 | 11 | float AverageSecs; // seconds to average over 12 | 13 | FilterOnePoleCascade averageValue; 14 | FilterOnePoleCascade averageSquareValue; 15 | 16 | void input( float inVal ); 17 | 18 | // constructor 19 | RunningStatistics(); 20 | 21 | void setWindowSecs( float windowSecs ); 22 | 23 | void setInitialValue( float initialMean, float initialSigma = 0 ); 24 | 25 | float mean(); 26 | 27 | float variance(); 28 | 29 | float sigma(); 30 | 31 | float CV(); 32 | }; 33 | 34 | void testRunningStatistics(); 35 | 36 | #endif --------------------------------------------------------------------------------