Точность джойстика

[wunderwaffe]

Точность определения джойстиком угла наклона - где-то градус, т.е. положение пляшет вокруг этого значения. В принципе, это ОК, т.к. джойстик задает ускорение вращения, положение - дважды интеграл, а интегрирование само по себе фильтр. Однако можно еще увеличить точность -

Время счета положения джойстиком 8 миллисекунд. Нужно опрашивать джойстик хотя бы 20 раз в секунду, значит, перерыв между измерениями - 50 миллисекунд. Это значит, что можно сделать 5-6 измерений в интервале между опросами, сгладить их скользящим окном и передать уже сглаженное значение.

Алгоритм работы с джостиком -
1) посылался байт запроса
2) по этому событию производится измерение, и отсылается результат

Надо сделать так -
1) результаты непрерывно пишутся в массив из пяти значений, самое новое измеренное записывается в самый старый элемент массива
2) посылается байт запроса
3) по этому событию считается среднее от массива, отсылается результат.

Прошивка для прикидки времени счета -

#include "Wire.h" //I2C Arduino Library
#define HMC5883L_ADDR 0x1E //0011110b, I2C 7bit address of HMC5883

bool haveHMC5883L = false;

void setup() {

 digitalWrite(4,LOW);
 digitalWrite(5,HIGH);
 
 Serial.begin(9600);
 Wire.begin();
 TWBR = 78;  // 25 kHz 
 TWSR |= _BV (TWPS0);  // change prescaler
 delay(500);
}

void loop() {

 int xm,ym,zm; 
 int x1,y1,z1; 
 
 bool detect = detectHMC5883L();
 if(!haveHMC5883L) {
  if(detect) {
   haveHMC5883L = true;
   Wire.beginTransmission(HMC5883L_ADDR); //open communication with HMC5883
   Wire.write(0x02); //select mode register
   Wire.write(0x00); //continuous measurement mode
   Wire.endTransmission();
  } else {
   Serial.println("No HMC5883L detected!");
   delay(2000);
   return;
  }
 } else {
  if(!detect) {
   haveHMC5883L = false;
   Serial.println("Lost connection to HMC5883L!");
   delay(2000);
   return;
  }
 }

if (Serial.available()) {
 byte bb=Serial.read();

 unsigned long t=micros();  

 Wire.beginTransmission(HMC5883L_ADDR);  //Tell the HMC5883 where to begin reading data
 Wire.write(0x03); //select register 3, X MSB register
 Wire.endTransmission();
 Wire.requestFrom(HMC5883L_ADDR, 6); //Read data from each axis, 2 registers per axis
 if(6<=Wire.available()){
   x1 = Wire.read()<<8; //X msb
   x1 |= Wire.read();   //X lsb
   y1 = Wire.read()<<8; //Y msb
   y1 |= Wire.read();   //Y lsb
   z1 = Wire.read()<<8; //Z msb
   z1 |= Wire.read();   //Z lsb
 } 

 Wire.beginTransmission(HMC5883L_ADDR);  //Tell the HMC5883 where to begin reading data
 Wire.write(0x03); //select register 3, X MSB register
 Wire.endTransmission();
 Wire.requestFrom(HMC5883L_ADDR, 6); //Read data from each axis, 2 registers per axis
 if(6<=Wire.available()){
   xm = Wire.read()<<8; //X msb
   xm |= Wire.read();   //X lsb
   ym = Wire.read()<<8; //Y msb
   ym |= Wire.read();   //Y lsb
   zm = Wire.read()<<8; //Z msb
   zm |= Wire.read();   //Z lsb
 } 

 double a=atan2(ym,x1)+atan2(x1,ym)+atan2(zm,ym)+atan2(1+xm,1-z1);
 a=sqrt(a*a+1);
 a=sqrt(a*a+2);

 t=micros()-t; 

 Serial.print(a);
 Serial.println("/x");
 Serial.println(t);
 
 delay(1); 
}
}

bool detectHMC5883L() {
 Wire.beginTransmission(HMC5883L_ADDR); //open communication with HMC5883
 Wire.write(10); //select Identification register A
 Wire.endTransmission();
 Wire.requestFrom(HMC5883L_ADDR, 3);
 if(3 == Wire.available()) {
  char a = Wire.read();
  char b = Wire.read();
  char c = Wire.read();
  if(a == 'H' && b == '4' && c == '3') return true;
 }
 return false;
}