Pages: [1]   Go Down
Author Topic: digitalSmooth problems  (Read 578 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I am trying to use the digitalSmooth function with multiple sensors but I get inaccurate numbers when I use more than two sensors?  If I use 1 sensor it works fine, if I use any two of the three sensors it works fine but if I try to use three sensors I get numbers much lower than they should be?

here is the code:
Code:
/* digitalSmooth
 Paul Badger 2007
 A digital smoothing filter for smoothing sensor jitter
 This filter accepts one new piece of data each time through a loop, which the
 filter inputs into a rolling array, replacing the oldest data with the latest reading.
 The array is then transferred to another array, and that array is sorted from low to high.
 Then the highest and lowest %15 of samples are thrown out. The remaining data is averaged
 and the result is returned.

 Every sensor used with the digitalSmooth function needs to have its own array to hold
 the raw sensor values. This array is then passed to the function, for it's use.
 This is done with the name of the array associated with the particular sensor.
 */

#include <Wire.h>

#include <L3G.h>
L3G gyro;

#include <LSM303.h>
LSM303 compass;


#define filterSamples   15              // filterSamples should  be an odd number, no smaller than 3
int sensSmoothArray1 [filterSamples];   // array for holding raw sensor values for sensor1
int sensSmoothArray2 [filterSamples];   // array for holding raw sensor values for sensor2
int sensSmoothArray3 [filterSamples];   // ADD A THIRD SENSOR VALUE

int rawData1, smoothData1;  // variables for sensor1 data
int rawData2, smoothData2;  // variables for sensor2 data
int rawData3, smoothData3;  // ADD THIRD SET OF VARIABLES

void setup(){
  Serial.begin(9600);
 
  Wire.begin();   // Sensor stuff
  gyro.init();
  gyro.enableDefault();
  gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
  gyro.writeReg(L3G_CTRL_REG4, 0x00); // 250 dps full scale 8.75mdps/digit
  compass.init();
  compass.enableDefault();
  compass.writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz
  compass.writeAccReg(LSM303_CTRL_REG4_A, 0x00); // 2 g full scale: FS = 00 on DLH, DLM

  compass.m_min.x = 24; compass.m_min.y = -187; compass.m_min.z = 370;  // calibrated compass values for heading
  compass.m_max.x = 31; compass.m_max.y = -176; compass.m_max.z = 379;  // calibrated compass values for heading
 
   
}
void loop(){       // test the digitalSmooth function
 
  compass.read();
 
  rawData1 = compass.a.x;                        // read sensor 1
  smoothData1 = digitalSmooth(rawData1, sensSmoothArray1);  // every sensor you use with digitalSmooth needs its own array

  rawData2 = compass.a.y;                        // read sensor 2
  smoothData2 = digitalSmooth(rawData2, sensSmoothArray2);  // every sensor you use with digitalSmooth needs its own array

  rawData3 = compass.a.z;                        // read sensor 3
  smoothData3 = digitalSmooth(rawData3, sensSmoothArray3);  // every sensor you use with digitalSmooth needs its own array


  Serial.print(smoothData1);
  Serial.print(",");
  Serial.print(smoothData2);
  Serial.print(",");
  Serial.println(smoothData3);

}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  long total;
  static int i;
 // static int raw[filterSamples];
  static int sorted[filterSamples];
  boolean done;

  i = (i + 1) % filterSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  // Serial.print("raw = ");

  for (j=0; j<filterSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting             
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (filterSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }

/*
  for (j = 0; j < (filterSamples); j++){    // print the array to debug
    Serial.print(sorted[j]);
    Serial.print("   ");
  }
  Serial.println();
*/

  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((filterSamples * 15)  / 100), 1);
  top = min((((filterSamples * 85) / 100) + 1  ), (filterSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++;
    // Serial.print(sorted[j]);
    // Serial.print("   ");
  }

//  Serial.println();
//  Serial.print("average = ");
//  Serial.println(total/k);
  return total / k;    // divide by number of samples
}

I have not used arrays very much and I don't know if it is something I am doing wrong or if the function is limited to two sensors, any help would be appreciated

Logged

Temple, Texas
Offline Offline
Sr. Member
****
Karma: 14
Posts: 360
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

sensor it works fine, if I use any two of the three sensors it works fine but if I try to use three sensors I get numbers much lower than they should be?
Are you saying that when the problem occurs, the numbers for all three sensors are lower, or the numbers for just the third sensor are too low.
Quote
Code:
  Serial.print(smoothData1);
  Serial.print(",");
  Serial.print(smoothData2);
  Serial.print(",");
  Serial.println(smoothData3);

Might be a good idea to similarly print out rawData1-3.  That may tell you whether the problem is with the "sensing" or with the "smoothing".

It doesn't look like digitalSmooth() is limited to any number of arrays since you are passing in the array to use.

John
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks for the reply;
The rawData1 - 3 is reading fine, but when all three are sent through the digitalSmooth function the data is ~ 25 - 30% lower than the rawData.
I have viewed both while smoothing all three:
Code:
  Serial.print(rawData1);
  Serial.print(",");
  Serial.println(smoothData1);


any thoughts .......
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 601
Posts: 48543
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Why is sorted static? It seems to me like you do want that array cleared every time the function is called.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Im not familiar with nuts and bolts of the function, i found the code here:

http://playground.arduino.cc/Main/DigitalSmooth

but i think it is used to keep track of where to put the new rawData#

sensSmoothArray = rawIn

but you  may be on to something, the i value is not unique for each sensor......
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 601
Posts: 48543
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
but you  may be on to something, the i value is not unique for each sensor.
That would be a problem. You'll need to make i an input to the function, and use three global variables.

Since the whole sorted array is re-written each time the function is called, I don't think static for it is appropriate.
Logged

Temple, Texas
Offline Offline
Sr. Member
****
Karma: 14
Posts: 360
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Right, the problem is with the static i. 

Mr. Badger's library function, though intended to support multiple "streams", is fatally flawed for more than one stream.

John
Logged

Temple, Texas
Offline Offline
Sr. Member
****
Karma: 14
Posts: 360
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I'm sure I have seen some circular-buffer-based libraries out there that would be suitable for moving average...

Alternately, maybe you could get by with simple exponential smoothing ?  Requires no data arrays and is fast.  IIRC Boils down to

Code:
sum -= avg;  // remove the oldest data value, which is approxmated by the current average
sum += new;  // add the newest data value
avg = sum/n;

You just have to choose an appropriate "n" (I think you were using 15 in the code you posted).

John


Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Yes it is the static i, using a filter of 15 with 3 sensors only every third array location would ever get a value and the rest would remain at 0 giving the reduced result, I could use a prime number for the filter?  I do like the idea of simple exponential smoothing, its simple and lightweight.  I would lose the benefit of throwing out the outliers though.

Thanks everyone for the input
   
Logged

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 212
Posts: 13531
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


check - http://playground.arduino.cc/Main/runningAverage - or - http://playground.arduino.cc/Main/runningMedian -

as these are implemented as classes you can instantiate multiple independent "smoothies" smiley-wink
Logged

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

Pages: [1]   Go Up
Jump to: