Подскажите с математикой обсчета компаса

clawham

Здравствуйте!
Делаю тут автопилот для кораблика.
В общем -то все получилось и работает/плавает нормально но есть проблема

и проблема эта - компас.

Использую модулек HMC5883

Естественно проблем со считыванием данных из него не имею. выдает числа от -160 до +120 по х и от -120 до +60 по Y

Ну я программно высчитываю середину и масштабирую к чистому выходу -100...0...+100
дальше делаю расчеты во флоате.

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

d = atan2(MAGFilt[0]*1.0 - MAGOffset[0]*1.0, MAGFilt[2]*1.0 - MAGOffset[2]*1.0);
    
    d = d * (180.0/PI);
    
    Course = d;
    
    Course += 90;
    
    Course += MagCalib.MagOffset;

ну вот собственно на выходе получаю нужный мне курс который иногда совпадает с реальным
но это все справедливо только для платы которая идеально паралельна поверхности земли...ну или наклонена так же как и при калибровке пределов и средних значений(кораблик на малом ходу делает пару кругов почета).

Так вот если наклонить кораблик сильнее - тоесть на корму поставить груз - кораблик проседает жопой и показания компаса уже улетают в хрень. ну скажем при наклоне на столе кораблика на 20-30" показания уходят на +-30 градусов в зависимости в какую сторону наклонять.

Вот и вопрос - как правильно обсчитать дополнительную третью ось магнитомера?
Может нужны ещё акселерометр 3осевой чтоб вектор гравитации получать. но какими формулами это все потом связать?
Кто сталкивался/делал/изучал - ничего не подскажет? Ато все что находил я - это примерно такие же реализации как и у меня или сложные матричные уравнения под матлаб.

Реклама

gessor

Из страшного курса по навигации вспоминается словосочетание "матрица перехода"...

Реклама

clawham

вот вот

ктото чтото подобное на мк делал?

Ser60

Почитайте статью "Compensate a Tilt-Compensating Electronic Compass" в апрельском выпуске журнала Circuit Cellar (#265) за 2012, стр. 16. Я делал на ARM компенсацию компаса с использованием библиотеки Sensor Fusion от Freescale/NXP.

Реклама

clawham

Я же правильно понимаю что без акселерометра не обойтись?

Реклама

Z_h_e

Сдается мне что тут все должно решаться тригонометрически. Например палуба корабля это плоскость XOY. Киль - ось OX. Считываете данные с компаса. Получаете вектор из начала координат. Проецируете его на плоскость XOY. Вычисляете угол между вектором и осью OX.

Сам я с таким устройством не знаком, но наверное как то так должно быть

.

Реклама

emax

а ось Z не используется?

clawham

да она есть но конечно же не используется. ибо я не знаю куда её приткнуть

Ser60

clawham писал(а):Я же правильно понимаю что без акселерометра не обойтись?

Правильно понимаете. И без третьей оси магнетометра тоже. Дело в том, что он показывает направление не на северный полюс, а на магнитный центр Земли, который находится глубоко от ее поверхности. Судя по вопросам, у меня впечатление, что рекомендованную выше статью не читали. Там все четко написано что к чему.

Vov123

А где Вы модуль приобретали?

clawham

да не читал. скачаю почитаю на досуге.

я знаю что он показывает вектор под углом а не паралельно поверхности - это логично . паралельно будет на экваторе . но это и не важно

но грубо говоря аксель тоже не поможет если наклонить кораблик так чтоб по ху были нули а по Z - вся шкала до упору. в таком виде как понять куда нос-то смотрит? крути нос как хош а на ху будут нули

У меня там впринципе и гпс есть - наверное есть формула примерная определения угла входа магнитных линий от широты текущей

Vov123

Не хотите отвечать? Некоторые продавцы прикрепляют ссылки на архивы с примерами работы с модулями. Обычно для трёх платформ (8051, AVR, и как-же без Arduino).
Ищите сами, может повезёт.

clawham

Ну кагбэ вопервых покупал не я а во вторых - ну собственно из примера под ардуино я и взял этот код и адаптировал. впринципе особо ничего сложного небыло. у нас до этого дургие были компасы и поменял я только регистры и биты при инициализации а модуль расчета из двух векторов результирующего курса - он так и остался...но эти новые компасы почему-то более сильно меняют курс от наклона. Собственно я уже понял что простой математики не будет. нашел некоторые фрагменты из ардупилота но там капец. похоже что по-другому это и не делается так что буду собирать упрощенную процедурку из кусков кода ардупилота.

Vov123

Я сам этот модуль только выписал, http://ru.aliexpress.com/item/Electroni ... 53748.html
жду, когда придёт потрогаю его за вымя.
Там в описании товара, внизу есть ссылка,про которую я писал выше, вроде там ничего ТАКОГО нет.
Большая просьба отписаться насколько алгоритм помог.
В принципе в качестве ответа ожидаю: "да" или "нет".

Z_h_e

emax писал(а):а ось Z не используется?

clawham писал(а):да она есть но конечно же не используется. ибо я не знаю куда её приткнуть

Я же предложил куда ее приткнуть. Для пробы не обязательно делать расчеты контроллером. Поставте Ваш корабль на стол, каким то образом получите данные с датчика, а расчеты сделайте "на бумаге". Ну потом пробуйте его крутить и наклонять.

clawham

есть 4 типа крена и при этом на каждый тип крена 4 типа направлений исходных...получается что в каждом отдельном случае из 16-ти эту ось 3 надо разными методами прибавлять или вычитать от одной из остальных осей - вот аксель типа и говорит насколько и как эту ось приплетать к остальным.

Z_h_e

У Вас есть некая точка, на которую указывает стрелка вектор датчика в трехмерном пространстве. Предположим что ось Х это киль, а ось Z это мачта. Вращая корабль(креня корабль), Вы вращаете систему координат, а вектор будет все равно указывать в туже точку. Вы же можете получить координаты вектора в трехмерном пространстве?
Не понимаю зачем Вам акселерометр, если у Вас и так датчик трехмерный.

Ser60

Курс корабля принято показывать в 2-мерной проекции на плоскость, тангенциально размещенную относительно поверхности Земли в точке нахождения наблюдателя. Плокость корабля с таковой не всегда совпадает при качке на воде. Акселерометр, коллинеарно расположенный с магнетометром, позволит определить насколько его плоскость отличается от плоскости измерения курса. По его показаниям компенсируются показания 3-мерного вектора магнетометра для пересчета его проекции на плоскость определения курса. Один акселерометр без магнетометра не позволит определить оринтацию объекта на поверхности Земли.
P.S. Даже на Экваторе вектор магнетометра не будет параллелен "плоскости" Земли.

Z_h_e

Я же говорю - проекция. Угол между проекцией вектора на палубу и килем будет постоянным при любом (если конечно корабль не перевернулся) крене при неизменности курса.

Ser60

У меня опять такое впечатление, что из моего последнего поста прочитали лишь слово "проекция". Прочитайте, наконец, рекомендованную выше статью. Там все подробно рассказано с картинками.