This shows you the differences between two versions of the page.
en:mag_calibration [2019/07/31 15:36] golikov created |
en:mag_calibration [2020/03/25 16:28] |
||
---|---|---|---|
Line 1: | Line 1: | ||
- | FIXME **This page is not fully translated, yet. Please help completing the translation.**\\ //(remove this paragraph once the translation is finished)// | ||
- | **Определение угла по магнитометру** \\ | ||
- | В представленном ниже видео спутник OrbiCraft разворачивается Web-камерой в сторону макета Земли. | ||
- | Информацию об ориентации спутник получает по [[:w_subsys|датчику угловой скорости]] и [[mag_subsys|магнитометру]]. | ||
- | Информация об угле нужна для того чтобы разворачивать спутник в необходимую сторону, а информация об угловой скорости | ||
- | необходима для того чтобы стабилизировать спутник, т.е. погасить угловое вращение. | ||
- | Управляющий момент создается с помощью двигателя [[wheel_subsys|маховика]]. | ||
- | {{ youtube>5vUo6r-nvrw?medium }} | ||
- | |||
- | По умолчанию магнитометр не откалиброван, т.е. выдает неточные значения. | ||
- | Уточненные значения можно получить из сырых данных путем калибровки, которая заключается в нахождении матрицы преобразования и вектора смещения. | ||
- | |||
- | <note tip>Представленная на этой странице методика калибровки магнитометра может использоваться также для квадрокоптеров и других устройств с магнитометрами</note> | ||
- | |||
- | Сырые данные с магнитометра можно получить выполнив код на языке C, представленный ниже. Во время сбора данных магнитометр необходимо хаотично вращать, | ||
- | стараясь повернуть его во все возможные стороны. | ||
- | |||
- | <code c> | ||
- | #include "libschsat.h" | ||
- | |||
- | void control(void) | ||
- | { | ||
- | int i; | ||
- | const int num = 1; /* magnetometer #1 */ | ||
- | printf("Enable magnetometer #%d\n", num); | ||
- | magnetometer_turn_on(num); | ||
- | printf("Get RAW data from magnetometer #%d\n", num); | ||
- | for (i = 0; i < 1000; i++) { | ||
- | int16_t x, y, z; | ||
- | if (LSS_OK == magnetometer_request_raw(num, &x, &y, &z)) { | ||
- | printf("%d: x=%d y=%d z=%d\n", i, x, y, z); | ||
- | } else { | ||
- | puts("Fail!"); | ||
- | } | ||
- | mSleep(100); | ||
- | } | ||
- | printf("Disable magnetometer #%d\n", num); | ||
- | magnetometer_turn_off(num); | ||
- | } | ||
- | </code> | ||
- | |||
- | Полученные данные измерений необходимо сохранить в файл с расширением .txt | ||
- | Для этого необходимо нажать <key>Ctrl</key>+<key>a</key>, и затем <key>Ctrl</key>+<key>c</key>, вставить в блокнот и сохранить. | ||
- | |||
- | {{ :ru:saving_magnetometer_data.gif?direct |}} | ||
- | |||
- | {{ :ru:mag_data_example_orbicraft.rar |Скачать }}пример файла с данными. | ||
- | |||
- | <note warning>Кроме данных в файле будут "лишние" строки с отладочной информацией. Эти лишние строки удалять не надо. Программа калибровки автоматически найдет строки с данными и вытащить необходимые значения.</note> | ||
- | |||
- | <WRAP center round download 80%> | ||
- | Скачать {{ ::magnetometer_calibration_v07.rar | программу калибровки}} | ||
- | </WRAP> | ||
- | |||
- | <WRAP center round download 80%> | ||
- | Скачать [[https://www.anaconda.com/download/#windows|Anaconda Navigator]] для запуска программы калибровки | ||
- | </WRAP> | ||
- | |||
- | <note important>Необходимо выбрать версию Python не ниже 3.6</note> | ||
- | {{ :ru:select_python_3_6_or_higher.png?nolink&400 |}} | ||
- | |||
- | Anaconda является мощной бесплатной интерактивной средой для Jupiter Notebook (iPython). | ||
- | В этом примере нам понадобится установить Anaconda для того чтобы запустить программу калибровки магнитометра. | ||
- | Перед скачиванием программы калибровки можно ознакомиться с ее [[http://nbviewer.jupyter.org/gist/Zaynulla/dbd9e1a8536090dd5262db9398f86035|содержимым]] | ||
- | |||
- | **Функция калибровки магнитометра на языке Python для использования в конструкторе** | ||
- | <code python> | ||
- | def mag_calibrated(magx,magy,magz): | ||
- | # эти 3 строки необходимо заменить на строки из программы калибровки | ||
- | magx_cal = 1.00*magx + 0.00*magy + 0.00*magz + 0.00 | ||
- | magy_cal = 0.00*magx + 1.00*magy + 0.00*magz + 0.00 | ||
- | magz_cal = 0.00*magx + 0.00*magy + 1.00*magz + 0.00 | ||
- | return magx_cal, magy_cal, magz_cal | ||
- | </code> | ||
- | <note important> | ||
- | Выше представлены калибровочные коэффициенты идеального магнитометра. Вместо них необходимо использовать коэффициенты, которые выдаст программа калибровки. | ||
- | </note> | ||
- | |||
- | {{ :ru:paste_calibration_parameters.gif?direct |}} | ||
- | |||
- | По проекциям магнитного поля можно вычислить угол между осью x магнитометра и проекцией магнитного поля на плоскость OXY магнитометра. | ||
- | |||
- | <code python> | ||
- | import math | ||
- | |||
- | # Для того чтобы положительное направление угла определялось против часовой стрелки, | ||
- | # необходимо перейти из левой системы координат, которая изображена на магнитометре, | ||
- | # в правую систему координат, для этого меняем знак измерения вдоль оси Y "magy_cal" на противоположный | ||
- | |||
- | magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw) | ||
- | magy_cal = - magy_cal | ||
- | mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180 | ||
- | </code> | ||
- | |||
- | Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal | ||
- | По умолчанию угол определяется в диапазоне от -180 до 180 градусов | ||
- | Функция angle_transformation изменяет диапазона измерения угла. Для решения задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до (alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра и проекцией вектора магнитного поля на горизонтальную плоскость. | ||
- | |||
- | <code python> | ||
- | def angle_transformation(alpha, alpha_goal): | ||
- | if alpha<(alpha_goal - 180): | ||
- | alpha = alpha + 360 | ||
- | elif alpha>(alpha_goal +180): | ||
- | alpha = alpha - 360 | ||
- | return alpha | ||
- | </code> | ||
- | |||
- | Полученный угол можно использовать для управления спутником по правилу PD-регулятора. | ||
- | Новая скорость маховика равна текущей скорости маховика + приращение скорости. | ||
- | Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. | ||
- | Коэффициенты kp и kd предлагается подобрать самостоятельно. | ||
- | |||
- | <code python> | ||
- | #mtr_speed - текущая скорость маховика | ||
- | #alpha, omega - текущие угол и угловая скорость спутника | ||
- | #alpha_goal, omega_goal - целевые угол и угловая скорость спутника | ||
- | |||
- | def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal): | ||
- | mtr_new_speed = int(mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal)) | ||
- | return mtr_new_speed | ||
- | </code> |