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

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


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

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

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

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

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

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

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

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

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

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

РЭ Houston Control Center

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

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

les07

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

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

Создайте следующую программу. Коэффициенты для функции def mag_calibrated возьмите из программы Magneto.

Рисунок 1. Данные, полученные в программе Magneto

Код на С.

stab_mag.c
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "libschsat.h"
 
float dt = 500;
float P = 0;
float D = 10;
float I = 0.8;
 
unsigned char ScreenOneTime = 1;
 
float NormAngleDifference(float angleDifference)
{
  if(angleDifference > 180)
    return angleDifference - 2 * 180;
  if(angleDifference < -180)
    return angleDifference + 2 * 180;
  return angleDifference;
}
 
void control(void){ 
	magnetometer_gyro_set_telemetry_period(0, dt);
	float mag_x = 0, mag_y = 0, mag_z = 0;
	uint16_t mag_num = 0;
 
	float phi_0 = 10;
	float phi_current;
 
	float error = 0;
	float privious_error = 0;
	float intgral_error = 0;
	float PD_OUT = 0;
	float DerrorDt = 0;
 
	float AVS[3];
 
	gyro_set_offset(0, -1.854248, -1.525269, 0.792542);
 
	float motorSpeed = 0;
    motor_set_speed(0, 0);
	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;
      error = NormAngleDifference(phi_0 - phi_current);
	  intgral_error += error;
      if(intgral_error * I > 500)
      {
        intgral_error = 500 / I;
      }
 
      if(intgral_error * I < -500)      
      {
        intgral_error = -500 / I ;    
      }
 
      DerrorDt = error - privious_error/ dt;
      PD_OUT = error * P + DerrorDt * D + intgral_error * I;
 
      if(PD_OUT > 3000)
      {
        PD_OUT = 3000;
      }
 
      if(PD_OUT < -3000)      
      {        
        PD_OUT = -3000;    
      }
      motor_set_speed(0, PD_OUT);
	  motor_request_speed(0, &motorSpeed);
      privious_error = error;
	  printf("%f %f %f\r\n", phi_current, error, motorSpeed);
      mSleep(dt);
 
 
      if ((2 > fabs(error)) & ScreenOneTime & (AVS[2] < 0.2))
      {
		ScreenOneTime = 0;
        camera_take_photo(1);
        motor_set_speed(0, 0);
        printf("photo is taken\r\n");
      }
	}
	puts("Sent");
}

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

В программе используются следующие функции для работы с Орбикрафт 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 магнитометра и проекцией вектора магнитного поля на горизонтальную плоскость.

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

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

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

les07.txt · Последние изменения: 2022/03/11 17:51 — ekaterina.manucharova

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