Возникла задача сделать наклоно-компенсированный компас. Поразбирался с различными датчиками и вроде бы получилось.
Итак, вначале об координатной системе.
Устанавливаем плату с датчиками в любое ортогональное положение (одна ось вдоль корпуса будет Х, вторая вертикально вниз когда корпус горизонтален будет Z и горизонтально на 90 по часовой от Х - ось Y).
Пишем (находим в и-нете) процедуру считывания показаний магнитометра и акселерометра. Выводим показания в монитор и определяем где у нас какая ось получилась.
Магнитометр.
Направляем нашу выбранную ось Х строго на север (корпус горизонтален). Покачиваем немного (градусов на 10-15) корпусом влево-право. Ось с наибольшими меняющимися показаниями будет осью Х магнитометра. Медленно поворачиваем по часовой (смотреть сверху) примерно на 90 градусов. Теперь наша ось Х должна выдавать показания около ноля, а другая ось увеличит свои показания до максимума. Вот эта другая ось и есть наш У. Ось Z при горизонтальном вращении почти не меняет значение. Но оно должно быть отрицательным.
Итого проверяем оси магнитометра:
0 градусов (на север): Х - максимум, У - около ноля, Z сильно в минусе
90 градусов (на север): Х - около ноля, У - максимум, Z сильно в минусе
180 градусов (на север): Х - минимум (отрицательное), У - около ноля, Z сильно в минусе
270 градусов (на север): Х - около ноля, У - минимум (отрицательное), Z сильно в минусе
Если перевернуть корпус вверх тормашками, то Z должна сменить знак и стать сильно положительной.
Если у вас не так, то пробуйте умножить значение осей на -1 пока не получите как выше в проверке.
Акселерометр.
В горизонтальном положении ось Z (смотрит вертикально вниз) выдает около сильно положительное значение (остальные около ноля). Затем наклоняем носом вниз - ось Х становится сильно отрицательной (остальные около ноля). Выравниваем и наклоняем правым крылом вниз - ось У становится сильно положительной (остальные около ноля). Если у вас не так - домножьте нужную ось на -1.
Математика.
Сначала нужно узнать (в радианах) как наклонилась плата с помощью акселерометра:
pitch = asin(-normAccel.XAxis);
roll = asin(normAccel.YAxis / cosPitch );
Значения берем нормализованные, т.е. приведенные к +- 1g. При подъеме носа питч становится отрицательным (до -1.5). При подъеме левого крыла ролл становится положительным до +1.5
Затем считаем направление на север (в радианах):
Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;
heading = atan2(Yh, Xh);
Значения магнитометра можно приводить к микротесла либо использовать сразу в единицах магнитометра - результат все равно частное от Xh и Yh и коэффициент нормализации сократится.
Калибровка.
Калибровка магнитометра и акселерометра очень нужна. Для обычного применения достаточно провести хард-калибровку. Медленно вращаем плату во всех направлениях и находим минимум и максимум показаний магнитометра по каждой оси. Затем вычисляем смещение как (минимум+максимум)/2. Результат отнимаем от каждого RAW значения магнитометра. Также необходимо выставить у всех осей коэффициент усиления, выравнивающий оси до наибольшей из них k=(Amax-Amin)/(Xmax-Xmin) или до средней длины.
Акселерометр калибруем аналогично, но данные снимаем только после остановки вращения (значение по оси не меняется в течении 100-200мс).
Итак, вначале об координатной системе.
Устанавливаем плату с датчиками в любое ортогональное положение (одна ось вдоль корпуса будет Х, вторая вертикально вниз когда корпус горизонтален будет Z и горизонтально на 90 по часовой от Х - ось Y).
Пишем (находим в и-нете) процедуру считывания показаний магнитометра и акселерометра. Выводим показания в монитор и определяем где у нас какая ось получилась.
Магнитометр.
Направляем нашу выбранную ось Х строго на север (корпус горизонтален). Покачиваем немного (градусов на 10-15) корпусом влево-право. Ось с наибольшими меняющимися показаниями будет осью Х магнитометра. Медленно поворачиваем по часовой (смотреть сверху) примерно на 90 градусов. Теперь наша ось Х должна выдавать показания около ноля, а другая ось увеличит свои показания до максимума. Вот эта другая ось и есть наш У. Ось Z при горизонтальном вращении почти не меняет значение. Но оно должно быть отрицательным.
Итого проверяем оси магнитометра:
0 градусов (на север): Х - максимум, У - около ноля, Z сильно в минусе
90 градусов (на север): Х - около ноля, У - максимум, Z сильно в минусе
180 градусов (на север): Х - минимум (отрицательное), У - около ноля, Z сильно в минусе
270 градусов (на север): Х - около ноля, У - минимум (отрицательное), Z сильно в минусе
Если перевернуть корпус вверх тормашками, то Z должна сменить знак и стать сильно положительной.
Если у вас не так, то пробуйте умножить значение осей на -1 пока не получите как выше в проверке.
Акселерометр.
В горизонтальном положении ось Z (смотрит вертикально вниз) выдает около сильно положительное значение (остальные около ноля). Затем наклоняем носом вниз - ось Х становится сильно отрицательной (остальные около ноля). Выравниваем и наклоняем правым крылом вниз - ось У становится сильно положительной (остальные около ноля). Если у вас не так - домножьте нужную ось на -1.
Математика.
Сначала нужно узнать (в радианах) как наклонилась плата с помощью акселерометра:
pitch = asin(-normAccel.XAxis);
roll = asin(normAccel.YAxis / cosPitch );
Значения берем нормализованные, т.е. приведенные к +- 1g. При подъеме носа питч становится отрицательным (до -1.5). При подъеме левого крыла ролл становится положительным до +1.5
Затем считаем направление на север (в радианах):
Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;
heading = atan2(Yh, Xh);
Значения магнитометра можно приводить к микротесла либо использовать сразу в единицах магнитометра - результат все равно частное от Xh и Yh и коэффициент нормализации сократится.
Калибровка.
Калибровка магнитометра и акселерометра очень нужна. Для обычного применения достаточно провести хард-калибровку. Медленно вращаем плату во всех направлениях и находим минимум и максимум показаний магнитометра по каждой оси. Затем вычисляем смещение как (минимум+максимум)/2. Результат отнимаем от каждого RAW значения магнитометра. Также необходимо выставить у всех осей коэффициент усиления, выравнивающий оси до наибольшей из них k=(Amax-Amin)/(Xmax-Xmin) или до средней длины.
Акселерометр калибруем аналогично, но данные снимаем только после остановки вращения (значение по оси не меняется в течении 100-200мс).
Комментариев нет:
Отправить комментарий