Дополнительный урок. Калибровка магнитометра в среде Jupyter notebook

Определение угла по магнитометру

В представленном ниже видео спутник OrbiCraft разворачивается Web-камерой в сторону макета Земли. Информацию об ориентации спутник получает по датчику угловой скорости и магнитометру. Информация об угле нужна для того чтобы разворачивать спутник в необходимую сторону, а информация об угловой скорости необходима для того чтобы стабилизировать спутник, т.е. погасить угловое вращение. Управляющий момент создается с помощью двигателя-маховика.

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

подсказка

Представленная на этой странице методика калибровки магнитометра может использоваться также для квадрокоптеров и других устройств с магнитометрами

Сырые данные с магнитометра можно получить выполнив код на языке 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);
}

Полученные данные измерений необходимо сохранить в файл с расширением .txt Для этого необходимо нажать Ctrl+A, и затем Ctrl+C, вставить в блокнот и сохранить.

• Скачать пример файла с данными.

осторожно

Кроме данных в файле будут «лишние» строки с отладочной информацией. Эти лишние строки удалять не надо. Программа калибровки автоматически найдет строки с данными и вытащить необходимые значения.

• Скачать программу калибровки

• Скачать Anaconda Navigator для запуска программы калибровки

осторожно

Необходимо выбрать версию Python не ниже 3.6!

Anaconda является мощной бесплатной интерактивной средой для Jupiter Notebook (iPython). В этом примере нам понадобится установить Anaconda для того чтобы запустить программу калибровки магнитометра. Перед скачиванием программы калибровки можно ознакомиться с ее содержимым.

Функция калибровки магнитометра на языке 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

к сведению

Выше представлены калибровочные коэффициенты идеального магнитометра. Вместо них необходимо использовать коэффициенты, которые выдаст программа калибровки.

По проекциям магнитного поля можно вычислить угол между осью x магнитометра и проекцией магнитного поля на плоскость OXY магнитометра.

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

Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal По умолчанию угол определяется в диапазоне от -180 до 180 градусов Функция angle_transformation изменяет диапазона измерения угла. Для решения задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до (alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра и проекцией вектора магнитного поля на горизонтальную плоскость.

def angle_transformation(alpha, alpha_goal):
    if alpha<(alpha_goal - 180):
        alpha = alpha + 360
    elif alpha>(alpha_goal +180):
        alpha = alpha - 360
    return alpha

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

#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