les07 [ОрбиКрафт 3D]

Инструменты пользователя


Боковая панель

Назначение ОрбиКрафт 3D

Сборка ОрбиКрафт 3D

Интерфейсы конструктора ОрбиКрафт 3D

Работа с ОрбиКрафт 3D по УКВ

Работа с ОрбиКрафт 3D по Wi-Fi

Работа с ОрбиКрафт 3D через WEB-интерфейс

Полезная нагрузка на базе Arduino

ОрбиКрафт 3D (трехосный) на стенде полунатурного моделирования

Среда разработки

РЭ Houston Control Center

Лабораторная оснастка

Обратная связь

les07

07 Урок. Ориентация ОрбиКрафт 3D с помощью магнитометра

Программа ориентации по магнитометру

Создайте следующую программу

Код на языке С:

stab_mag.c
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "libschsat.h"
 
struct motor_torque
{
    float torque;
    uint16_t time;
}__attribute__((packed));
 
float alphaBeta(float omega, float dt, float phi);
float NormAngleDifference(float angleDifference);
float filtrt;
int completePhoto = 0;
float phi_0 = 100; 
 
void control(void){ 
    float dt = 0.1;
    float AVS[3];
    float mag_x = 0, mag_y = 0, mag_z = 0;
    uint16_t mag_num = 0;
    float motorSpeed = 0;
    struct motor_torque tq;
    float P = 3e-3;
    float D = 2e-2;
    float I = 0.0005;
    float I_sum = 0.004;
    float pid_border = 0.1;
    float PD_OUT = 0;
    float error = 0;
    float p_error = 0;
    float previous_error = 0;
    float intgral_error = 0;
    float DerrorDt = 0;
    float phi_current;
    float average;
    float absolute_angle;
    float privious_angle;
    int count = 0;
    int fixation = 0;
	magnetometer_gyro_set_telemetry_period(0, (uint16_t)(dt * 1000));
	motor_set_telemetry_period(0, (uint16_t)(dt * 1000));
	motor_set_speed(0, 0);
    magnetometer_request_raw(mag_num, &mag_x, &mag_y, &mag_z);
    phi_current = atan2(mag_x, mag_y) * 180 / 3.14;
    privious_angle = phi_current;
    Sleep(dt);
 
	for (int i = 0; i < 20000; i++) 
{
         gyro_request_raw(0, AVS, AVS + 1, AVS + 2);
        magnetometer_request_raw(mag_num, &mag_x, &mag_y, &mag_z);
        phi_current = atan2(mag_x, mag_y) * 180 / 3.14;
        float angle_delta  = NormAngleDifference(phi_current - privious_angle);
        privious_angle = phi_current;
        absolute_angle += angle_delta;
        average = alphaBeta(AVS[2], dt, absolute_angle);
        phi_current = atan2(sin(average * 3.1415 / 180), cos(average * 3.1415 / 180)) * 180 / 3.1415;
        error = NormAngleDifference(phi_0 - phi_current);
        DerrorDt = (error - previous_error) / dt;
        intgral_error += error / dt;
        if(intgral_error * I > I_sum)
        {
            intgral_error = I_sum / I;
        }
 
        if(intgral_error * I < -I_sum){
            intgral_error = -I_sum / I ;    
        }
 
        PD_OUT = error * P + DerrorDt * D + intgral_error * I;
 
        if(PD_OUT > pid_border){
            PD_OUT = pid_border;
        }
 
        if(PD_OUT < -pid_border){        
            PD_OUT = -pid_border;    
        }
        tq.torque = -PD_OUT;
        tq.time = (uint16_t)(dt * 1000.0);
        send_unican_message(10, 2576, &tq, sizeof(tq));
        motor_request_speed(0, &motorSpeed);
        previous_error = error;
        Sleep(dt);
 
        if (!fixation && (3 > fabs(error)) && (AVS[2] < 2))
        {
            if(count > 100){
                camera_take_photo(1);
                printf("Photo is taken");
                completePhoto = 1;
                phi_0 = -50;
                count = 0;
                fixation = 1;
            }else{
                count++;
            }
        }
        if (completePhoto && (3 > fabs(error)) && (AVS[2] < 2))
        {
            if (count > 300){
                motor_set_speed(0, 0);
                return;
            }
            else{
                count++;
            }
        }
    }
}
float alphaBeta(float omega, float dt, float phi){
 
    static uint8_t first = 1;
    const float alpha = 0.99;
    float beta = 1 - alpha;
    if(first)
    {
        filtrt = phi;
        first = 0;
        return phi;
    }
    filtrt = (filtrt + omega * dt) * alpha + phi * beta;
    return filtrt;
}
 
float NormAngleDifference(float angleDifference)
{
    if(angleDifference > 180)
        return angleDifference - 2 * 180;
    if(angleDifference < -180)
        return angleDifference + 2 * 180;
    return angleDifference;
}

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

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

magnetometer_gyro_set_telemetry_period

Функция устанавливает период отправки телеметрии заданными магнитометром и ДУС.

gyro_set_offset

Функция задаёт постоянное смещение показателей ДУС совмещая физический и логический ноль.

motor_set_speed

Функция задаёт угловую скорость вращения определённого управляющего двигателя-маховика.

motor_request_speed

Функция возвращает угловую скорость вращения определённого управляющего двигателя-маховика.

gyro_request_raw

Функция получает актуальные данные от заданного датчика угловых скоростей.

magnetometer_request_raw

Функция получает актуальные данные от заданного магнитометра.

Тест программы

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

Тест скорректированной программы

Протестируйте работу программы при следующих значениях целевого угла alpha_goal: 0, 90, -90, 180. Теперь при повороте на целевой угол в 180 градусов программа будет работать правильно.

les07.txt · Последние изменения: 2023/01/11 14:55 — ekaterina.manucharova

Инструменты страницы