Компас с компенсацией наклона Adafruit_LSM303

Question

Компас с компенсацией наклона Adafruit_LSM303

У меня возникли проблемы с компенсацией наклона моего компаса.

Я использую акселерометр/компас LSM303DLHC в сочетании с Adafruit_LSM303_U.h.

Я смог направить свой компас в правильном направлении с помощью этого небольшого фрагмента:

  float heading = (atan2(event.magnetic.y,event.magnetic.x) * 180) / Pi;

Я также смог получить угол крена и наклона с помощью преобразования X и Y моего акселерометра в углы:

   headingNew = asin(eventA.acceleration.x/10);
   float tilt = headingNew*180/PI/90*100;
   headingNew = asin(eventA.acceleration.y/10);
   float roll = headingNew*180/PI/90*100;

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

    //Xh = event.magnetic.x*cos(tilt)+event.magnetic.z*sin(tilt);
    //Yh = event.magnetic.x*sin(roll)*sin(tilt)+event.magnetic.y*cos(roll)-event.magnetic.z*sin(roll)*cos(tilt);

или в самом коде:

   Xh = event.magnetic.x*(eventA.acceleration.z/10)+event.magnetic.z*(eventA.acceleration.x/10);
   Yh = event.magnetic.x*(eventA.acceleration.y/10)*(eventA.acceleration.x/10)+event.magnetic.y*(eventA.acceleration.z/10)-event.magnetic.z*(eventA.acceleration.y/10)*(eventA.acceleration.z/10);

Я действительно получаю правильный угол на плоской поверхности. Но он еще более неточен под углом, чем обычный курс. У кого-нибудь есть опыт работы с этим датчиком? Уже большое спасибо за помощь.

@Roel, 👍1

2 ответа

Лучший ответ:

▲ 3

Функции sin() и cos() ожидают, что их аргументы будут в радиан. Если вы кормите их чем-то еще, будь то градусы, процент наклона или что-то еще, ожидайте, что результат будет просто дерьмом.

Правка: Операция изменила вопрос, аннулировав мой ответ. Итак, вот вам новый ответ...

Вы ошибочно приняли EventA.acceleration.y за cos(наклон), EventA.acceleration.x за sin(наклон)и так далее. Они не одно и то же. Вы можете подумать, что они одинаковы до некоторого мультипликативного коэффициента, и это правильно. Однако коэффициент не отменяется, как только вы выполняете вычисления, такие как sin(крен)*sin(наклон). Не спешите с математикой: если вы хотите преобразовать известные хорошие формулы в более практичные, тщательно проверяйте каждый шаг преобразования.

21 ноя 19, @Edgar Bonet

На самом деле для синуса наклона я использую свой акселерометр.x, для косинуса наклона акселерометра.y. Также для греха крена я использую акселерометр.Y. Это значения от 0 до 1, которые совпадают с cos/sin единичного круга, @Roel

@Roel: Re “ _ для косинуса наклона [я использую] акселерометр.y_”: Я не вижу этого в коде, который вы опубликовали. Если вы задаете вопрос о _different_ фрагменте кода, пожалуйста, отредактируйте вопрос и убедитесь, что отображаемый вами код соответствует вашему вопросу., @Edgar Bonet

Я отредактировал вопрос, чтобы показать вам, как на самом деле выглядит код в моем проекте., @Roel

Я просто хочу знать, что не так с конечной функцией, потому что я не могу получить правильный результат., @Roel

@Roel: **Что не так**: он использует EventA.ускорение.y вместо cos(наклон) и т. Д. **Решение**: используйте опубликованную формулу с углами в радианах., @Edgar Bonet

Я изменил его в своем коде и в вопросе. Это мне не помогло. cos(наклон) = событие.ускорение.z сейчас. Это мне не помогло, @Roel

Пересмотрел все. Если бы вы могли проверить еще раз. Я действительно получаю правильный угол на плоской поверхности. Но он еще более неточен под углом, чем обычный курс., @Roel

@*Roel* · Accepted Answer · 2019-11-22 15:30-00:00

С помощью этого фильма на Youtube я нашел другую библиотеку и создал этот код:

#include <SPI.h>
#include <Wire.h>
#include <LSM303.h>
const int centreX  = 160;
const int centreY  = 120;
const int diameter = 100;
#define STMPE_CS 8
#define TFT_CS 10
#define TFT_DC 9

int last_dx, last_dy, dx, dy;
LSM303 compass;
void setup(){
Serial.begin(9600);
Serial.println("meters Test"); Serial.println("");
Wire.begin();
compass.init();
compass.enableDefault();


compass.m_min = (LSM303::vector<int16_t>){-602, -704, -511}; //значения из
cailbration.
compass.m_max = (LSM303::vector<int16_t>){+557, +436, +529}; //значения из
cailbration.
last_dx = centreX;
last_dy = centreY;
}
void loop() {
compass.read();
double angle;
float heading = compass.heading();


angle= heading; //use heading from compass
dx = (diameter * cos((angle-90)*3.14/180)) + centreX;    // вычисление X позиции
dy = (diameter * sin((angle-90)*3.14/180)) + centreY;    // вычисление Y позиции
last_dx = dx;
last_dy = dy;
Serial.print("angle:"); Serial.println(angle);
Serial.println("tilt:"); Serial.println(compass.a.x);
delay(25);
}

Теперь все работает. Иногда вам просто нужно выбрать свои сражения.