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:
/* 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
