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

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

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

к сведению

Более подробно ознакомиться с работой магнитометра вы можете по ссылке: 02 Урок. Знакомство с датчиками.

Сырые данные с магнитометра можно получить, выполнив код на языке С или Python, представленные ниже.

Код на С:

#include <stdio.h>
#include <stdint.h>
#include "libschsat.h"
 
int count = 400;    
 float arrX[400];
float arrY[400];
int dt = 100;
float speed = 600;
 
void control(void){ 
 
    float mag_x = 0, mag_y = 0, mag_z = 0;
    uint16_t mag_num = 0;
 
    mSleep(1000);
    magnetometer_gyro_set_telemetry_period(mag_num, dt);
    magnetometer_set_offset(mag_num, 0, 0, 0);
    motor_set_speed(0, speed);
    for (int i = 0; i < count; i++){
        mSleep(dt);
        magnetometer_request_raw(mag_num, &mag_x, &mag_y, &mag_z);
        arrX[i] = mag_x;
        arrY[i] = mag_y;
        if (i == count / 2){
            motor_set_speed(0, -speed);
        }
    }
 
    float min, max;
    min=max=arrX[0];
    for(int j = 0; j < count; j++)
    {
        if(min>arrX[j])
            min=arrX[j];   
        if(max<arrX[j])
            max=arrX[j];       
    }
    float xOffset = (max + min) / 2;
 
    min=max=arrY[0];
    for(int k = 0; k < count; k++)
    {
        if(min>arrY[k])
            min=arrY[k];   
        if(max<arrY[k])
            max=arrY[k];       
    }
    float yOffset = (max + min) / 2;
 
    xOffset = xOffset / 14;
    yOffset = yOffset / 14;
    magnetometer_set_offset(mag_num, xOffset, yOffset, 0);
    printf("x: %f  y: %f", xOffset, yOffset);
    motor_set_speed(0, 0);
    uint8_t data[0];
    //saving
    send_unican_message(25, 2656, data, 0);
}

Код на Python

from libschsat import Libschsat
import time
 
lib=Libschsat()
 
count = 400
arrX=[]
arrY=[]
dt=0.1
speed=600
time.sleep(1)
lib.magGyroSetTelemetryPeriod (100, 2)
lib.magSetOffset (0, 0, 0)
lib.motorSetSpeed('0xA', speed)
for i in range (count):
    time.sleep(dt)
    mag_x, mag_y, mag_z = lib.magRequestRaw (2)
    arrX.append(mag_x)
    arrY.append(mag_y)
 
    if i == count /2:
        lib.motorSetSpeed('0xA', -speed)
min=max=arrX[0]
for j in range (count):
    if min>arrX[j]:
        min=arrX[j]
    if max<arrX[j]:
        max=arrX[j]
xOffset=(max+min)/2
min=max=arrY[0]
for k in range (count):
    if min>arrY[k]:
       min=arrY[k]
    if max<arrY[k]:
        max=arrY[k]
yOffset=(max+min)/2
xOffset=xOffset/14
yOffset=yOffset/14
lib.magSetOffset(int(xOffset), int(yOffset), 0)
print('x: ' + str(xOffset) + '; y:' + str(yOffset))
lib.motorSetSpeed('0xA',0)
lib.sendMsgAndWaitAnswer('0xA60','0xF','0x19',0,[],'0x118')

Подвесьте Orbix в токовую рамку. При выполнении программы маховик будет вращать конструктор космического зонда самостоятельно. После калибровки данные будут отправлены в ПО Houston Application.

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

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

magnetometer_gyro_set_telemetry_period(mag_num, dt)

• установка периода телеметрии магнитометра и ДУСа с номером магнитометра mag_num и периодом dt

magnetometer_set_offset(mag_num, mag_x, mag_y, mag_z)

• функция задает смещение нуля для магнитометра с номером mag_num в плоскостях x,y,z.

motor_set_speed(0, speed);

• функция устанавливает скорость вращения маховика с номером 0 и скоростью speed

подсказка

В одноосном Orbix номер маховика соответствует 2, в трехосном - от 0 до 2 в зависимости от выгравированной оси (ось X - 0, ось Y - 1, ось Z -2)

magnetometer_request_raw(mag_num, &mag_x, &mag_y, &mag_z)

• функция, возвращающая сырые данные, измеренные магнитометром с номером mag_num, представляющие собой список из 4 числовых значений. Считанные данные мы помещаем в переменные mag_x, mag_y и mag_z.

В программе использован оператор цикла:

for (int i = 0; i < count; i++)

который будет выполнен n раз, соответственно будет собрано n значений с датчика.

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

magGyroSetTelemetryPeriod (100, 2)

• установка периода телеметрии магнитометра и ДУСа с номером магнитометра с периодом телеметрии 100 мс

magSetOffset (0, 0, 0)

• функция задает смещение нуля для магнитометра в плоскостях x,y,z.

motorSetSpeed('0xA', speed)

• функция устанавливает скорость вращения маховика 0хА и скоростью speed

mag_x, mag_y, mag_z = lib.magRequestRaw (2)
arrX.append(mag_x)
arrY.append(mag_y)

• функция запрашивает регулярную телеметрию carrier и возвращает список значений магнетометра (x, y, z). Так как в одноосной конфигурации конструктора зонда вращается вокруг оси Z, то в mag_x и mag_y записываются данные по осям x и y.