Алгоритм работы с HMC5883 компас

Land

Прошу помощи с алгоритмом работы с компасом на HMC5883. Изначально задача выглядит довольно простой. есть устройство, на котором стоит компас и шаговый двигатель. Шаговым двигателем управляется указатель, который выставляется на некий азимут. По нажатию кнопки запоминается количество шагов ШД и азимут компаса, после чего рассчитывается азимут указателя. В дальнейшем устройство вращается по/против часовой стрелки, а шаговик удерживает указатель на рассчитанном азимуте. Этакий псевдогироскоп. И все работает чудесно, до момента перехода компаса через 0 градусов. Т.к. дельта угла между направлением указателя и осью компаса высчитывается как DELTA= AZUMUT_NULL - DEGRESS, где AZIMUT_NULL -- это начальное положение указателя (нулевой азимут), а DEGRESS -- направление оси компаса в градусах, то в момент перехода через ноль градусов получается полная ерунда. Например, при нулевом азимуте в 270 градусов и направлением оси компаса 359 градусов, дельта равна -89 градусов. И это прекрасно. Но при нулевом азимуте в 270 градусов и направлении оси компаса в 0 градусов дельта равна уже 270 градусов.

И как с этим бороться -- ума не приложу

Буду очень признателен за дельный совет.

Реклама

maxlab

Привет! Стандартный алгоритм работы с HMC(QMC) выглядит так

Код: Выделить всё

void loop() {
  int x,y,z;
  compass.read(&x,&y,&z);

 // Calculate heading when the magnetometer is level, then correct for signs of axis.
  // Atan2() automatically check the correct formula taking care of the quadrant you are in
  float heading = atan2(y, x);

  float declinationAngle = 0.0404;
  heading += declinationAngle;
  // Find yours here: http://www.magnetic-declination.com/

   // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;

  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 

   delay(100);
}

В переменной headingDegrees истинный азимут. Не понятно для чего ШД и что за девайс изобретаете?
И если я правильно понял, дельту надо вычислять не как разницу между азимутом стрелки и истинным азимутом... а использовать предыдущее измерение истинного азимута.
Полученное приращение (в плюс или минус) корректирует количество шагов... или положение стрелки. Как то так...

Реклама