Low Pass Filter Question
Greg LaPolla
Posts: 324
in Propeller 2
I am porting this from Arduino. I have watched at least 20 You tube videos about the subject and I am still a little confused. I am not getting the correct data. I think that the coefficients are wrong for use with the propeller as it runs way faster than the Arduino. I don't have mathlab so I can't really figure out the coefficients.
On setting the return I have tried >> 15, SAR 15 and divide by 32768. None seem to work.
PUB lowPassFIRFilter(din): result | z, i
cbuf[offset] := din
z := mul16(FIRCoeffs[11], cbuf[(offset - 11) & $1F])
i:= 0
repeat while i < 11
z += mul16(FIRCoeffs[i], cbuf[(offset - i) & $1F] + cbuf[(offset - 22 + i) & $1F])
i++
offset++
offset //= 32 'Wrap condition
return result := (z >> 15)
' Integer multiplier
PUB mul16(x, y): result
return result := (x * y)
DAT
FIRCoeffs long 172, 321, 579, 927, 1360, 1858, 2390, 2916, 3391, 3768, 4012, 4096
Comments
Your offset use looks fishy to me. The increment and wrap indicate it’s a ringbuffer (which makes sense in this context), but within your loop you happily index into forbidden territories by doing index arithmetic. Basically for every offset based access you need to wrap it around before actually accessing the buffer. Like (untested)
cbuf[(offset - i) // 32]I am pretty sure the & $1F Takes care of that. I haven't seen the offset go above 31 which is what it is supposed to do.
I can confirm that
x &= $1Fis the same as
x //= 32The former is faster because it doesn't involve division.
@"Greg LaPolla" 🤦♂️You’re right. I somehow read that as applied to the value, not the index.
What does the original Arduino code look like? Have a link?
Most of the stuff I see on this is using floating point...
BTW: There's a super simple way to implement the digital form of a low pass RC filter, in case that doesn't work out...
http://www.dsplog.com/2007/12/02/digital-implementation-of-rc-low-pass-filter/
SAR 15 or divide by 32768 should work. The ">>" operator is unsigned, and it will give you the wrong answer.
How is cbuf defined? It should be defined as an array of 32 longs. So either as "long cbuf[32]" in a VAR section or "cbuf long 0[32]" in a DAT section. If you define it as a byte or word you will need to do sign extension since these are unsigned variables.
Here is the Arduino code
/* Optical Heart Rate Detection (PBA Algorithm) By: Nathan Seidle SparkFun Electronics Date: October 2nd, 2016 Given a series of IR samples from the MAX30105 we discern when a heart beat is occurring Let's have a brief chat about what this code does. We're going to try to detect heart-rate optically. This is tricky and prone to give false readings. We really don't want to get anyone hurt so use this code only as an example of how to process optical data. Build fun stuff with our MAX30105 breakout board but don't use it for actual medical diagnosis. Excellent background on optical heart rate detection: http://www.ti.com/lit/an/slaa655/slaa655.pdf Good reading: http://www.techforfuture.nl/fjc_documents/mitrabaratchi-measuringheartratewithopticalsensor.pdf https://fruct.org/publications/fruct13/files/Lau.pdf This is an implementation of Maxim's PBA (Penpheral Beat Amplitude) algorithm. It's been converted to work within the Arduino framework. */ /* Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved. * * Permission is hereby granted, free of charge, to any person obtaining a * copy of this software and associated documentation files (the "Software"), * to deal in the Software without restriction, including without limitation * the rights to use, copy, modify, merge, publish, distribute, sublicense, * and/or sell copies of the Software, and to permit persons to whom the * Software is furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included * in all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR * OTHER DEALINGS IN THE SOFTWARE. * * Except as contained in this notice, the name of Maxim Integrated * Products, Inc. shall not be used except as stated in the Maxim Integrated * Products, Inc. Branding Policy. * * The mere transfer of this software does not imply any licenses * of trade secrets, proprietary technology, copyrights, patents, * trademarks, maskwork rights, or any other form of intellectual * property whatsoever. Maxim Integrated Products, Inc. retains all * ownership rights. * */ #include "heartRate.h" int16_t IR_AC_Max = 20; int16_t IR_AC_Min = -20; int16_t IR_AC_Signal_Current = 0; int16_t IR_AC_Signal_Previous; int16_t IR_AC_Signal_min = 0; int16_t IR_AC_Signal_max = 0; int16_t IR_Average_Estimated; int16_t positiveEdge = 0; int16_t negativeEdge = 0; int32_t ir_avg_reg = 0; int16_t cbuf[32]; uint8_t offset = 0; static const uint16_t FIRCoeffs[12] = {172, 321, 579, 927, 1360, 1858, 2390, 2916, 3391, 3768, 4012, 4096}; // Heart Rate Monitor functions takes a sample value and the sample number // Returns true if a beat is detected // A running average of four samples is recommended for display on the screen. bool checkForBeat(int32_t sample) { bool beatDetected = false; // Save current state IR_AC_Signal_Previous = IR_AC_Signal_Current; //This is good to view for debugging //Serial.print("Signal_Current: "); //Serial.println(IR_AC_Signal_Current); // Process next data sample IR_Average_Estimated = averageDCEstimator(&ir_avg_reg, sample); IR_AC_Signal_Current = lowPassFIRFilter(sample - IR_Average_Estimated); // Detect positive zero crossing (rising edge) if ((IR_AC_Signal_Previous < 0) & (IR_AC_Signal_Current >= 0)) { IR_AC_Max = IR_AC_Signal_max; //Adjust our AC max and min IR_AC_Min = IR_AC_Signal_min; positiveEdge = 1; negativeEdge = 0; IR_AC_Signal_max = 0; //if ((IR_AC_Max - IR_AC_Min) > 100 & (IR_AC_Max - IR_AC_Min) < 1000) if ((IR_AC_Max - IR_AC_Min) > 20 & (IR_AC_Max - IR_AC_Min) < 1000) { //Heart beat!!! beatDetected = true; } } // Detect negative zero crossing (falling edge) if ((IR_AC_Signal_Previous > 0) & (IR_AC_Signal_Current <= 0)) { positiveEdge = 0; negativeEdge = 1; IR_AC_Signal_min = 0; } // Find Maximum value in positive cycle if (positiveEdge & (IR_AC_Signal_Current > IR_AC_Signal_Previous)) { IR_AC_Signal_max = IR_AC_Signal_Current; } // Find Minimum value in negative cycle if (negativeEdge & (IR_AC_Signal_Current < IR_AC_Signal_Previous)) { IR_AC_Signal_min = IR_AC_Signal_Current; } return(beatDetected); } // Average DC Estimator int16_t averageDCEstimator(int32_t *p, uint16_t x) { *p += ((((long) x << 15) - *p) >> 4); return (*p >> 15); } // Low Pass FIR Filter int16_t lowPassFIRFilter(int16_t din) { cbuf[offset] = din; int32_t z = mul16(FIRCoeffs[11], cbuf[(offset - 11) & 0x1F]); for (uint8_t i = 0 ; i < 11 ; i++) { z += mul16(FIRCoeffs[i], cbuf[(offset - i) & 0x1F] + cbuf[(offset - 22 + i) & 0x1F]); } offset++; offset %= 32; //Wrap condition return(z >> 15); } // Integer multiplier int32_t mul16(int16_t x, int16_t y) { return((long)x * (long)y); }@"Dave Hein"
It is long cbuf[32]
I think @"Dave Hein" is right... That "return result := (z >> 15)" looks wrong for signed integers...
Are all your variables declared as long? It looks like your code should work if you use divide or SAR.
All variables are longs. I switched them all to longs because only longs can have signs. It doesn't matter if I use >>, SAR or divide, the output is still incorrect.
Can you post all your code? The problem must be in some other area of the code.
There is not much more but here it is
CON CLK_FREQ = 200_000_000 ' system freq as a constant MS_001 = CLK_FREQ / 1_000 ' ticks in 1ms US_001 = CLK_FREQ / 1_000_000 ' ticks in 1us _clkfreq = CLK_FREQ ' set system clock #true, ON, OFF #false, NO, YES VAR long IR_AC_Max long IR_AC_Min long IR_AC_Signal_Current long IR_AC_Signal_Previous long IR_AC_Signal_min long IR_AC_Signal_max long IR_Average_Estimated long positiveEdge long negativeEdge long ir_avg_reg long cbuf[32] long offset ' Heart Rate Monitor functions takes a sample value and the sample number ' Returns true if a beat is detected ' A running average of four samples is recommended for display on the screen. PUB start() IR_AC_Signal_Current := 0 IR_AC_Signal_min := 0 IR_AC_Signal_max := 0 IR_AC_Max := 20 IR_AC_Min := -20 positiveEdge := 0 negativeEdge := 0 ir_avg_reg := 0 offset := 0 PUB checkForBeat(sample): beatDetected | diff beatDetected := false ' Save current state IR_AC_Signal_Previous := IR_AC_Signal_Current 'This is good to view for debugging 'debug(sdec(IR_AC_Signal_Current)) ' Process next data sample IR_Average_Estimated := averageDCEstimator(@ir_avg_reg, sample) IR_AC_Signal_Current := lowPassFIRFilter(sample - IR_Average_Estimated) 'debug(sdec(IR_Average_Estimated)) 'debug(sdec(sample)) debug(sdec(sample - IR_Average_Estimated)) debug(sdec(IR_AC_Signal_Current)) ' Detect positive zero crossing (rising edge) if ((IR_AC_Signal_Previous < 0) & (IR_AC_Signal_Current >= 0)) debug("**********************************") IR_AC_Max := IR_AC_Signal_max 'Adjust our AC max and min IR_AC_Min := IR_AC_Signal_min debug(sdec(IR_AC_Max - IR_AC_Min)) positiveEdge := 1 negativeEdge := 0 IR_AC_Signal_max := 0 'if ((IR_AC_Max - IR_AC_Min) > 100 & (IR_AC_Max - IR_AC_Min) < 1000) if ((IR_AC_Max - IR_AC_Min) > 20 & (IR_AC_Max - IR_AC_Min) < 1000) 'Heart beat!!! beatDetected := true ' Detect negative zero crossing (falling edge) if ((IR_AC_Signal_Previous > 0) & (IR_AC_Signal_Current <= 0)) positiveEdge := 0 negativeEdge := 1 IR_AC_Signal_min := 0 ' Find Maximum value in positive cycle if (positiveEdge & (IR_AC_Signal_Current > IR_AC_Signal_Previous)) IR_AC_Signal_max := IR_AC_Signal_Current ' Find Minimum value in negative cycle if (negativeEdge & (IR_AC_Signal_Current < IR_AC_Signal_Previous)) IR_AC_Signal_min := IR_AC_Signal_Current return beatDetected ' Average DC Estimator PUB averageDCEstimator(p, x): result long[p] += (((x << 15) - long[p]) SAR 4) return result := (long[p] >> 15) ' Low Pass FIR Filter PUB lowPassFIRFilter(din): result | z, i cbuf[offset] := din z := mul16(FIRCoeffs[11], cbuf[(offset - 11) & $1F]) {{ debug(sdec(offset - 11)) debug(sdec((offset - 11) & $1F)) if ((offset - 11) & $1F) > 31 abort }} i:= 0 repeat while i < 11 z += mul16(FIRCoeffs[i], cbuf[(offset - i) & $1F] + cbuf[(offset - 22 + i) & $1F]) {{ debug(sdec((offset - 22 + i)& $1F)) if ((offset - 22 + i) & $1F) > 31 abort debug(sdec((offset - i) & $1F)) if ((offset - i) & $1F) > 31 abort }} i++ offset++ offset //= 32 'Wrap condition ' debug(sdec(offset)) return result := (z >> 15) ' Integer multiplier PUB mul16(x, y): result 'debug(sdec(x)) 'debug(sdec(y)) 'debug(sdec(x * y)) return result := (x * y) DAT FIRCoeffs long 172, 321, 579, 927, 1360, 1858, 2390, 2916, 3391, 3768, 4012, 4096I haven't used Spin on the P2, but if it works like the P1 then it's going to run the start method and then stop. So I'm assuming this isn't all of the code, which means you are calling this code from other objects.
Your variables are declared in a VAR section. This means that each object that calls a method in the code you posted will use a different copy of the variables. Make sure you are calling all the methods from the same object. If not, then you should declare your variables in a DAT section so that you only have one set of variables.
This is an object. It is used in conjunction with a chip driver object. The LowPassFilter is not outputting the correct data.
It is supposed to detect positive zero crossing on the rising edge. It never happens at this point. I have narrowed the issue to the code in this method.
It will be called from another method and only called from that method.
There is no issues with the variables. This entire object only returns a true or false back to the calling method. There is no shared variables to get crossed. It is passed a value from the sensor and if it detects a heart beat it returns true if not it returns false. Thats it.
I think you can write this filter much simpler, without the mul16 methode:
' Low Pass FIR Filter PUB lowPassFIRFilter(din) : result | z, i cbuf[offset] := din z := FIRCoeffs[11] * cbuf[(offset - 11) & $1F] repeat i from 0 to 10 z += FIRCoeffs[i] * (cbuf[(offset - i) & $1F] + cbuf[(offset - 22 + i) & $1F]) offset := (offset + 1) & $1F return z SAR 15 DAT FIRCoeffs long 172, 321, 579, 927, 1360, 1858, 2390, 2916, 3391, 3768, 4012, 4096I have not looked at the Arduino code, just rewritten your FIR methode, so I don't know if the filter is ported correct.
Andy
OK, so you are only referencing the object from one other object, correct? As written, it won't work if you reference it from multiple objects.
There's an error in the following line:
if ((IR_AC_Max - IR_AC_Min) > 20 & (IR_AC_Max - IR_AC_Min) < 1000)
In Spin1 the "&" has precedence over the ">" operator. So this line is the same as:
if (((IR_AC_Max - IR_AC_Min) > (20 & (IR_AC_Max - IR_AC_Min))) < 1000)
The result is always be TRUE. In Spin1 a TRUE value was always -1. I don't know what it is in Spin2. Also, you probably don't want to use the "&" operator, which is a bitwise and. You probably want the logical and, which in Spin1 is AND. I don't know what it is in Spin2.
There is also a problem with the line:
return result := (z >> 15)
In Spin1 this should be
return result := (z ~> 15)
In Spin2 it should be
return result := (z SAR 15)
Also, do you even need the return. Can't you just do
result := (z SAR 15)
and it will automatically return the value of result?
[RANT] Too many arbitrary differences between Spin1 and Spin2. The differences are unnecessary! Program in C instead.[/RANT]
@"Dave Hein" Nice catches. I guess there are two main things tripping people up going between Spin2 and C; operator precedence and signed/unsigned integers (maybe mostly right shifting of these when less than 32 bits).
Personally, I like Spin2 much better than Spin1, mainly because >= is the right way (the way you say it) and functions requiring parenthesis makes code clearer. But, the C standard is hard to ignore.
I checked the Spin2 documentation, and the logical and operator is either AND or &&. So the IF statement could be written as
if ((IR_AC_Max - IR_AC_Min) > 20 AND (IR_AC_Max - IR_AC_Min) < 1000)
In this case the parentheses are not needed since the ">" operator will be evaluated before the AND operator.
I changed all the & to AND still not getting the right data out of the lowPassFilter.
@"Greg LaPolla" , try the attached file to see if that works.
@"Dave Hein" Thanks for the help!
Couple things I figured out today. Can use SAR 15 on the averageDCEstimator. I don't know why but it goes negative and the output of the sensor (sample) is never negative so I just use >> 15.
I am still pretty sure the coefficients need to change because in order to detect a heartbeat the current lowPassFIRFilter output has the be greater than or equal to zero and the previous output has to be less than zero. Holding my finger on the sensor for 1 minute I will see 1 maybe 2 hits.
Using your modified code IR_AC_Signal_Current and IR_AC_Signal_Previous never go negative when my finger is on the sensor.
Using my code the majority of the output is negative
From what I can gather the output should be positive and negative under 10 if I understand everything.
It seems like you are getting an overflow in the averageDCEstimator routine. The input value is scaled up by 32768, so to prevent overflow the input value must be less than 65536. Does the input ever exceed that value?
I also notice that the FIR filter has a gain of 1.45. If the input to the FIR filter was greater than 45,197 for 23 samples it would overflow. However, since the DC value is subtracted from the input I think this would prevent an overflow in the FIR filter.
Finger on reading is 97,000 to 101,000
Finger off reading 1100 to 1200
I think the lighting has something to do with the fluctuation
the estimator is spot on all the time. it runs slightly behind the sample usually by 20-30.
There's your problem. The input is exceeding the value of 65536. When averageDCEstimator shifts this up by 15 it overflows into a negative value. It looks like you are getting a lot of hum from the AC line.
Add the following line to the beginning of the checkForBeat method:
sample >>= 2
That should help. If that doesn't work try shifting down by 3 or 4 instead. You may also want to clip the maximum value of sample, as follows:
if (sample > 65535)
sample := 65535
That will ensure that averageDCEstimator doesn't overflow.