06 Урок. Калибровка магнитометра

Получение сырых данных для калибровки магнитометра

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

Код на Python:

def control(): #Основная программы, в которой вызываем остальные функции
    mgn_result = [0,0,0,0]      #Инициализируем mgn_result
    num = 1
    magnetometer_turn_on(num)   #Включаем магнитометр
    sleep(1)
    for i in range(500):        #Выполним 500 измерений
        mgn_result = magnetometer_request_raw(num)
        if not mgn_result[0]: #Если датчик не вернул сообщение об ошибке
            print mgn_result[1], mgn_result[2], mgn_result[3]
        sleep(0.05)     #Задержка пять сотых секунды
    magnetometer_turn_off(num)  #Выключение магнитометра

Код на С:

#include <stdio.h>
#include <stdint.h>     
 
void control(void){ //Основная программа, в которой вызываем остальные функции
/*Для вывода данных необходимость в объявлении массива отпадает*/
    int16_t mgn_result[] = {0, 0, 0, 0}; //Инициализируем mgn_result
    uint16_t num = 1;
    magnetometer_turn_on(num); //Включаем магнитометр 
    mSleep(1000);
    int i;
    for (i = 0; i < 500; i++) //Выполним 500 измерений   
    {
        mgn_result[0] = magnetometer_request_raw(num, &mgn_result[1],&mgn_result[2],&mgn_result[3]);
        if(!mgn_result[0]){ //Если датчик не вернул сообщение об ошибке
            printf("%d, %d, %d\n", mgn_result[1], mgn_result[2], mgn_result[3]);
        }
        mSleep(50); //Задержка пять сотых секунды
    }
    magnetometer_turn_off(num); 
}

Соедините БКУ с СЭП и магнитометром как на прошлом уроке. Не прикрепляйте магнитометр к пластине, чтобы его было удобно вращать. Загрузите программу в Орбикрафт и запустите ее. Хаотично вращайте магнитометр, чтобы собрать сырые данные со всех возможных направлений.

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

Анализ работы программы

В программе используются следующие функции работы с магнитометром:

magnetometer_turn_on(num)

– функция включения магнитометра, где num – это номер магнитометра.

magnetometer_request_raw(num)

– функция возвращающая сырые данные измеренные магнитометром с номером num, представляющие собой список из 4 числовых значений. Поэтому считанные данные мы помещаем в список mgn_result, состоящий из 4 значений.

mgn_result = [0,0,0,0]
mgn_result = magnetometer_request_raw(num)

Первое значение списка возвращает информацию об ошибке. Если возвращено значение 0, то ошибки нет, если 1, то датчик не соединен, если 2, то ошибка в программе.

В программе использован оператор цикла for i in range(1000) который будет выполнен 1000 раз, соответственно будет выведено 1000 значений.

Эти значения понадобятся на следующем уроке при калибровке магнитометра.

Анализ полученных данных в Excel

Откройте полученный файл с данными с Excel. Нажмите Ctrl-O и выберите папку с файлом. По умолчанию Excel не открывает файлы типа txt, однако если в поле ввода имени ввести * и нажать на Enter, то будут показаны все файлы. Выберите файл с данными и нажмите Открыть.

Запустится мастер импорта. Нажмите Далее. На второй странице укажите что символом-разделителем столбцов является пробел.

Убедитесь, что в окне с образцом разбора данных все столбцы с данными разделены вертикальными линиями и нажмите Далее.

Формат данных столбцов оставьте общим. Нажмите Готово и данные будут загружены в лист Excel.

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

Кликните Вставка и выберите точечную диаграмму.

Будет построена точечная диаграмма, на которой хорошо видны точки, которые выбиваются из общего ряда.

Наведите на точку курсор и увидите значение.

Найдите в таблице строку с указанным значением и удалите ее целиком. Аналогичным образом удалите ошибочные точки из второго и третьего столбцов.

Сохраните файл в формате txt. На вопрос о сохранении в текстовом формате ответьте Да.

Запомните где сохранен этот файл, он потребуется на следующем этапе калибровки магнитометра.

Получение калибровочных коэффициентов

Скачайте программу Magneto по ссылке.

Разархивируйте и запустите программу Magneto.

Введите в окно, отмеченное цифрой 1, значение 700, затем нажмите на кнопку Open и выберите файл с сырыми данными, очищенный от ошибок. Затем нажмите на кнопку Calibrate. Программа Magneto вычислит и отобразит поправочные коэффициенты. В дальнейшем нам потребуются не все коэффициенты, рассчитанные программой Magneto , а только три значения Combined bias (b) и девять значений Correction for combined scale factors.

Тест откалиброванного магнитометра

Загрузите в Орбикрафт следующую программу, которая произведет 60 измерений с шагом в 1 сек. Используйте распечатанный транспортир и магнитометр с линейкой. Коэффициенты для функции def mag_calibrated возьмите из программы Magneto.

Скопируйте округленные значения следующим образом.

осторожно

Обратите внимание! Три верхних нормировочных коэффициента, отвечающие за смещение ноля, вставляются в формулу с обратными знаками, в отличие от остальных коэффициентов, описывающих искажения эллипсоида.

Код на Python:

calibr.py
import math
 
time_step = 1       # Временной шаг измерений
mag_num = 1     # Номер магнитометра
 
# Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
def mag_calibrated(magx,magy,magz):
    magx_cal = 1.06*(magx + -7.49) + -0.01*(magy + -23.59) + 0.07*(magz + -108.24)
    magy_cal = -0.01*(magx + -7.49) + 1.11*(magy + -23.59) + 0.09*(magz + -108.24)
    magz_cal = 0.07*(magx + -7.49) + 0.09*(magy + -23.59) + 1.00*(magz + -108.24)
    return magx_cal, magy_cal, magz_cal
 
def initialize_all():
    print "Enable magnetometer", mag_num
    magnetometer_turn_on(mag_num)
    sleep(1)
 
def switch_off_all():
    print "Disable magnetometer", mag_num
    magnetometer_turn_off(mag_num)
 
def control(): # основная функция программы, в которой вызываются остальные функции
    initialize_all()
    mag_state = 0       # Инициализируем статус магнитометра
    alpha_goal = 0      # Целевой угол
    omega_goal = 0      # Целевая угловая скорость
 
    for i in range(60):
        # опрос датчиков и маховика
        mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)
 
        if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет
            magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw)
            mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180
            print "mag_alpha atan2= ", mag_alpha
 
        elif mag_state == 1:
            print "Fail because of access error, check the connection"
 
        elif mag_state == 2:
            print "Fail because of interface error, check your code"
 
        sleep(time_step)
 
    switch_off_all()

Код на С:

calibr.c
#include <stdio.h>
#include <stdint.h>
#include "libschsat.h"
#define LSS_OK 0 
#define LSS_ERROR 1 
#define LSS_BREAK 2
 
#include <math.h>
const int time_step = 1;
const uint16_t mag_num = 1;
 
int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){
//Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
 
    float magx_cal;
    magx_cal = 1.06*(*magx + -7.49) + -0.01*(*magy + -23.59) + 0.07*(*magz + -108.24);
    float magy_cal;
    magy_cal = -0.01*(*magx + -7.49) + 1.11*(*magy + -23.59) + 0.09*(*magz + -108.24);
    float magz_cal;
    magz_cal = 0.07*(*magx + -7.49) + 0.09*(*magy + -23.59) + 1.00*(*magz + -108.24);
    *magx = (int16_t) magx_cal;
    *magy = (int16_t) magy_cal;
    *magz = (int16_t) magz_cal;
 
    return 0;
}
 
void initialize_all(){
    printf("Enable magnetometer %d\n", mag_num);
    magnetometer_turn_on(mag_num);
    Sleep(1);
}
 
void switch_off_all(){
    printf("Disable magnetometer %d\n", mag_num);
    magnetometer_turn_off(mag_num);
}
 
int control(){
    initialize_all();
    int mag_state = 0;
    int16_t mgx_cal=0;
    int16_t mgy_cal=0;
    int16_t mgz_cal=0;
    int16_t *magx_cal = &mgx_cal;
    int16_t *magy_cal = &mgy_cal;
    int16_t *magz_cal = &mgz_cal;
    int i;
 
    for ( i= 0; i < 60; i++){
        mag_state = magnetometer_request_raw(mag_num, magx_cal,magy_cal,magz_cal);
        float mag_alpha;
        if (!mag_state){
            mag_calibrated(magx_cal,magy_cal,magz_cal);
            mag_alpha = atan2(mgy_cal, mgx_cal)/ M_PI *180;
            printf("mag_alpha atan2 = %f\n", mag_alpha);
        }
        else if (mag_state == 1){
            printf("Fail because of access error, check the connection");
        }
        else if (mag_state == 2){
            printf("Fail because of interface error, check your code");
        }
        Sleep(time_step);
    }
    switch_off_all();
    return 0;
}

Поворачивайте магнитометр с шагом в 30 градусов и записывайте выдаваемое значение угла в таблицу EXCEL. Должна получится примерно вот такая таблица с данными.

Анализ данных в Excel

Проанализируйте полученные данные как на прошлом уроке.

Средне значение разности между измеренными углами 30.

Стандартное отклонение вычисленных разностей от среднего значения 1.3.

Процентное соотношение среднего значения и стандартного отклонения 4%.

Измеренное на прошлом уроке процентное соотношение составляло 17%, следовательно, откалиброванный магнитометр измеряет примерно в 4 раза точнее.