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

Код на Python.

orientation.py

import math
 
kd = 200			# К-т дифференциальной обратной связи. Если угловая скорость спутника положительна, то спутник надо раскручивать по часовой стрелки, т.е. маховик надо разгонять по часовой стрелке.
kp = -50			# К-т пропорциональной обратной связи. Если текущий угол больше целевого, то спутник надо вращать против часовой стрелки, соответственно маховик надо разгонять против часовой стрелки.
time_step = 0.05		# Временной шаг работы
 
mtr_num = 1			# Номер маховика
hyr_num = 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
 
# Функция motor_new_speed_PD определяет новую скорость маховика
# Новая скорость маховика равна текущей скорости маховика + приращение скорости
# Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости
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
	# return 0
 
def initialize_all(): # Функция инициализации всех систем
	print "Enable motor №", mtr_num 
	motor_turn_on(mtr_num)
	sleep(1)
 
	print "Enable angular velocity sensor №", hyr_num 
	hyro_turn_on(hyr_num) # Включаем ДУС
	sleep(1)
 
	print "Enable magnetometer", mag_num
	magnetometer_turn_on(mag_num)
	sleep(1)
 
def switch_off_all(): # Функция выключения всех систем
	print "Finishing..."
	print "Disable angular velocity sensor №", hyr_num
	hyro_turn_off(hyr_num)   # Выключаем ДУС
	print "Disable magnetometer", mag_num
	magnetometer_turn_off(mag_num)
	motor_set_speed(mtr_num, 0)
	sleep (1)
	motor_turn_off(mtr_num)
	print "Finish program"
 
def control(): # основная функция программы
	initialize_all()
	mtr_state = 0		# Инициализируем статус маховика
	hyro_state = 0 		# Инициализируем статус ДУС
	mag_state = 0 		# Инициализируем статус магнитометра
	alpha_goal = 0		# Целевой угол
	omega_goal = 0 		# Целевая угловая скорость
	mag_alpha = 0
 
	for i in range(500):
		# опрос датчиков и маховика
		mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)
		hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) 
		mtr_state, mtr_speed = motor_request_speed(mtr_num)
 
		if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет
			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
			print "magx_cal =", magx_cal, "magy_cal =", magy_cal, "magz_cal =", magz_cal # Вывод откалиброванных значений магнитометра
			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"
 
		if not hyro_state: # если ДУС вернул код ошибки 0, т.е. ошибки нет
			gx_degs = gx_raw * 0.00875
			gy_degs = gy_raw * 0.00875
			gz_degs = gz_raw * 0.00875
			omega = gz_degs	# если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z
			print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Выводим данные
		elif hyro_state == 1: # если датчик вернул сообщение об ошибке 1
			print "Fail because of access error, check the connection"
		elif hyro_state == 2: # если датчик вернул сообщение об ошибке 2
			print "Fail because of interface error, check your code"
 
		if not mtr_state:	# если маховик вернул код ошибки 0, т.е. ошибки нет
			print "Motor_speed: ", mtr_speed
			mtr_new_speed = motor_new_speed_PD(mtr_speed,mag_alpha,alpha_goal,gz_degs,omega_goal)	# установка новой скорости маховика
			motor_set_speed(mtr_num, mtr_new_speed)
 
		sleep(time_step)
	switch_off_all()

Код на С.

orientation.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 kd = 200; /* К-т дифференциальной обратной связи. 
Если угловая скорость спутника положительна, то спутник надо раскручивать
 по часовой стрелки, т.е. маховик надо разгонять по часовой стрелке.*/
 
const int kp = -50; /* К-т пропорциональной обратной связи. 
Если текущий угол больше целевого, то спутник надо вращать против 
часовой стрелки, соответственно маховик надо разгонять против часовой стрелки.*/
const float time_step = 0.05; // Временной шаг работы
const uint16_t mtr_num = 1;			// Номер маховика
const uint16_t hyr_num = 1; 		        // Номер ДУС
const uint16_t mag_num = 1;			// Номер магнитометра
// Максимально допустимая скорость маховика, об/мин
const int16_t mtr_max_speed = 5000;
int16_t mtr_new_speed;
 
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;
}
 
int motor_new_speed_PD(int mtr_speed, int omega, int omega_goal){	
	/* Функция для определения новой скорости маховика.
 Новая скорость маховика складывается из
 текущей скорости маховика и приращения скорости.
 Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости.
 mtr_speed - текущая угловая скорость маховика, об/мин
 omega - текущая угловая скорость спутника, град/с
 omega_goal - целевая угловая скорость спутника, град/с
 mtr_new_speed - требуемая угловая скорость маховика, об/мин*/
 
	mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal));	
	if (mtr_new_speed > mtr_max_speed)
	{
		mtr_new_speed = mtr_max_speed;
	}
	else if (mtr_new_speed < -mtr_max_speed)
	{
		mtr_new_speed = -mtr_max_speed;
	}
	return mtr_new_speed;
}
 
void initialize_all(void){
/*	 Функция инициализации всех систем,
	включает все приборы,которые будут использоваться в основной программе.*/
	printf ("Enable motor №%d\n", mtr_num);
	motor_turn_on(mtr_num);
	Sleep(1);
	printf("Enable angular velocity sensor №%d\n", hyr_num); 
	hyro_turn_on(hyr_num); // Включаем ДУС
	Sleep(1);
	printf("Enable magnetometer %d\n", mag_num);
	magnetometer_turn_on(mag_num);
	Sleep(1);
}
 
int switch_off_all(){
/*Функция отключает все приборы,которые будут использоваться в основной программе.*/
	printf("Finishing...");
	int16_t new_speed = mtr_new_speed; 
	printf("\nDisable angular velocity sensor №%d\n", hyr_num);
	hyro_turn_off(hyr_num);
	printf("Disable magnetometer %d\n", mag_num);
	magnetometer_turn_off(mag_num);
	motor_set_speed(mtr_num, 0, &new_speed);
	Sleep (1);
	motor_turn_off(mtr_num);
	printf("Finish program\n");
	return 0;
}
 
int control(){
	float mtr_new_speed;
	//данные магнитометра
	int16_t mgx_cal=0;
	int16_t mgy_cal=0;
	int16_t mgz_cal=0;
	int16_t *magx_raw = &mgx_cal;
	int16_t *magy_raw = &mgy_cal;
	int16_t *magz_raw = &mgz_cal;
	//данные ДУС
	int16_t gx_raw;
	int16_t gy_raw;
	int16_t gz_raw;
	int16_t *hyrox_raw=&gx_raw;
	int16_t *hyroy_raw= &gy_raw;
	int16_t *hyroz_raw = &gz_raw;
	int16_t mtr_speed;
	int16_t omega;
	int16_t omega_goal=0;  // Целевая угловая скорость
 
	initialize_all();
 
	int mtr_state = 0;		// Инициализируем статус маховика
	int hyro_state = 0; 		// Инициализируем статус ДУС
	int mag_state = 0; 		// Инициализируем статус магнитометра
//	int alpha_goal = 0;		// Целевой угол		
	double mag_alpha = 0;
	int i;
	for (i = 0; i < 500; i++)	{
		mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw);
		hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); 
		gx_raw = *hyrox_raw; 
		gy_raw = *hyroy_raw; 
		gz_raw = *hyroz_raw; 
		mtr_state = motor_request_speed(mtr_num, &mtr_speed);
		if (!mag_state){
			mag_calibrated(magx_raw,magy_raw,magz_raw);
			*magy_raw = - *magy_raw;/* переходим из левой системы координат,
			которая изображена на магнитометре в правую, для того чтобы 
			положительное направление угла было против часовой стрелки*/
			mag_alpha = atan2(mgy_cal, mgx_cal)/M_PI*180;
			printf("magx_cal = %d", mgx_cal);
			printf(" magy_cal = %d", mgy_cal);
			printf(" magz_cal = %d", mgz_cal);
			printf(" mag_alpha atan2 = %f\n", mag_alpha);
		}
		else if (mag_state == 1){
			printf("Fail because of access error, check the connection\n");
		   }
		else if (mag_state == 2){
			printf("Fail because of interface error, check your code\n");
		  }
 
	if (!hyro_state){
			float gx_degs = gx_raw * 0.00875;
			float gy_degs = gy_raw * 0.00875;
			float gz_degs = gz_raw * 0.00875;
			/* если ДУС установлен осью z вверх, то угловая скорость
			// спутника совпадает с показаниями ДУС по оси z, иначе
			 необходимо изменить знак: omega = - gz_degs */
			omega = gz_degs;
			printf("\ngx_degs = %f", gx_degs);
			printf(" gy_degs = %f", gy_degs);
			printf(" gz_degs = %f\n", gz_degs);
		}
		else if (hyro_state == 1){
			printf("Fail because of access error, check the connection\n");
		}
		else if (hyro_state == 2){
			printf("Fail because of interface error, check your code\n");
		}
 
		//Обработка показаний маховика и установка требуемой угловой скорости.
		if (!mtr_state)	{
			// если код ошибки 0, т.е. ошибки нет
			int16_t mtr_speed=0;
			motor_request_speed(mtr_num, &mtr_speed);
			printf("\nMotor_speed: %d\n", mtr_speed);	
			// установка новой скорости маховика
			mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal);
			motor_set_speed(mtr_num, mtr_new_speed, &omega);
		}	
		Sleep(time_step);	
	}
	switch_off_all();
	return 0;
}

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

magnetometer_turn_on(num)

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

magnetometer_turn_off(num)

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

magnetometer_request_raw(num)

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

hyro_turn_on(hyr_num)

– функция включения ДУС, где hyr_num – это номер ДУС.

hyro_turn_off(hyr_num)

– функция выключения ДУС, где hyr_num – это номер ДУС.

hyro_request_raw(hyr_num)

– функция возвращающая сырые данные измеренные ДУС с номером hyr_num, представляющие собой список из 4 числовых значений.

motor_turn_on(num)

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

motor_turn_off(num)

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

motor_set_speed(mtr_num, speed)

– функция устанавливающая скорость вращения маховика с номером mtr_num, в значение speed.

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