User Tools

Translations of this page:

Site Tools


en:lesson10

This is an old revision of the document!


FIXME This page is not fully translated, yet. Please help completing the translation.
(remove this paragraph once the translation is finished)

OrbiCraft positioning with solar sensors

Program loading

Open the file SUN.py and enter to the array data_sun = [] all data from the data text file; delete the space and comma characters at the end of array.

Python code.

sun_orient.py
import time
import math
 
kd = 200			# Incremental feedback coefficient. If the satellite’s angular velocity is positive, than satellite must be spun clock-wise (i.e. the flywheel must rotated clock-wise.
kp = -100			# К-т пропорциональной обратной связи. Если текущий угол больше целевого, то спутник надо вращать против часовой стрелки, соответственно маховик надо разгонять против часовой стрелки.
time_step = 0.05		# Временной шаг работы системы ориентации и стабилизации
 
mtr_num = 1			# Номер маховика
hyr_num = 1			# Номер ДУС
mag_num = 1			# Номер магнитометра
# Максимально допустимая скорость маховика, об/мин
mtr_max_speed = 3000
sun_max = 25000		# Максимальное значение солнечного датчика больше которого считаем ошибкой
sun_min = 50		# Минимальное значение солнечного датчика меньше которого считаем ошибкой
 
# Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal
# По умолчанию угол определяется в диапазоне от -180 до 180 градусов
# Функция 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
 
# Определение новой скорости маховика
# Новая скорость маховика равна текущей скорости маховика + приращение скорости
# Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости
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))
	if mtr_new_speed > mtr_max_speed:
		mtr_new_speed = mtr_max_speed
	elif mtr_new_speed < -mtr_max_speed:
		mtr_new_speed = -mtr_max_speed
	return mtr_new_speed
 
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 Sun sensors 1-4"
	sun_sensor_turn_on(1)
	sun_sensor_turn_on(2)
	sun_sensor_turn_on(3)
	sun_sensor_turn_on(4)
	sleep(1)
 
def switch_off_all():
	print "Finishing..."
	hyro_turn_off(hyr_num)   # Выключаем ДУС
	sun_sensor_turn_off(1)
	sun_sensor_turn_off(2)
	sun_sensor_turn_off(3)
	sun_sensor_turn_off(4)
	motor_set_speed(mtr_num, 0)
	sleep (1)
	motor_turn_off(mtr_num)
	print "Finish program"
 
def check_sun(sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42):
	delta1 = 0
	delta2 = 0 
	delta3 = 0 
	delta4 = 0 
	delta5 = 0
	delta6 = 0 
	delta7 = 0 
	delta8 = 0	
	delta_sum = 36000
	delta_min = 35000	
	delta_i = 0 
	data_sun = []
	for i in range(0, len(data_sun), 9):  # len(data_sun) - длина массива
		delta1 = abs(sun11 - data_sun[i+0])
		delta2 = abs(sun12 - data_sun[i+1])
		delta3 = abs(sun21 - data_sun[i+2])
		delta4 = abs(sun22 - data_sun[i+3])
		delta5 = abs(sun31 - data_sun[i+4])
		delta6 = abs(sun32 - data_sun[i+5])
		delta7 = abs(sun41 - data_sun[i+6])
		delta8 = abs(sun42 - data_sun[i+7])
		delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8
		#print i, data_sun[i+7]
		#print "Sun_sens", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42
		#print "deltas= ", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum		
		if delta_min > delta_sum :
			delta_min = delta_sum
			delta_i = data_sun[i+8]
			#print "delta_min= ", delta_min, "delta_i= ", delta_i
	return delta_i
 
def sun_range(sun, sun_old):	
	if sun[1] > sun_max:
		sun[1] = sun_old[1]
	if sun[1] < sun_min:
		sun[1] = sun_old[1]
	if sun[2] > sun_max:
		sun[2] = sun_old[2]
	if sun[2] < sun_min:
		sun[2] = sun_old[2]		
	return sun
 
def control(): # основная функция программы
	initialize_all()
	# Инициализируем статус маховика
	mtr_state = 0
	# Инициализируем статус ДУС	
	hyro_state = 0
	sun_sensor_num = 0 	   # Инициализируем переменную для номера солнечного датчика
	sun_result_1 = [0,0,0] # Инициализируем sun_result_1
	sun_result_2 = [0,0,0] # Инициализируем sun_result_2
	sun_result_3 = [0,0,0] # Инициализируем sun_result_3
	sun_result_4 = [0,0,0] # Инициализируем sun_result_4
	sun_result_1_Old = [0,0,0] # Инициализируем sun_result_1
	sun_result_2_Old = [0,0,0] # Инициализируем sun_result_2
	sun_result_3_Old = [0,0,0] # Инициализируем sun_result_3
	sun_result_4_Old = [0,0,0] # Инициализируем sun_result_4	
 
	omega_goal = 0 		# Целевая угловая скорость
	alpha_goal = 30		# Целевой угол поворота
 
	for i in range(300):
		# Запоминаем старые значения
		sun_result_1_Old =	sun_result_1	
		sun_result_2_Old =	sun_result_2		
		sun_result_3_Old =	sun_result_3		
		sun_result_4_Old =	sun_result_4		
		# опрос датчиков и маховика
		sun_result_1 = sun_sensor_request_raw(1)
		sun_result_2 = sun_sensor_request_raw(2)			
		sun_result_3 = sun_sensor_request_raw(3)
		sun_result_4 = sun_sensor_request_raw(4)
		#print sun_result_1, sun_result_2, sun_result_3, sun_result_4
		# Проверяем чтобы новое измеренное значение не выходило за пределы диапазона
		sun_result_1 = sun_range(sun_result_1, sun_result_1_Old)
		sun_result_2 = sun_range(sun_result_2, sun_result_2_Old)	
		sun_result_3 = sun_range(sun_result_3, sun_result_3_Old)	
		sun_result_4 = sun_range(sun_result_4, sun_result_4_Old)
		alpha_sun = check_sun(sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2])
		alpha_sun = angle_transformation(alpha_sun, alpha_goal)
		print alpha_sun
		hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) 
		mtr_state, mtr_speed = motor_request_speed(mtr_num)
		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,alpha_sun,alpha_goal,gz_degs,omega_goal)	# установка новой скорости маховика
			motor_set_speed(mtr_num, mtr_new_speed)
 
		sleep(time_step)
	switch_off_all()

Код на С.

sun_orient.c
#include "libschsat.h"
#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 = -100; /*К-т пропорциональной обратной связи. 
Если текущий угол больше целевого, 
то спутник надо вращать против часовой стрелки, 
соответственно маховик надо разгонять против часовой стрелки.*/
const float time_step = 0.05; //Временной шаг работы системы ориентации и стабилизации
const int mtr_max_speed = 3000; 		// Максимально допустимая скорость маховика, об/мин
const uint16_t mtr_num = 1;			// Номер маховика
const uint16_t hyr_num = 1; 		        // Номер ДУС
const uint16_t mag_num = 1;			// Номер магнитометра
 
const int sun_max = 25000;//Максимальное значение солнечного датчика больше которого считаем ошибкой
const int sun_min = 50;//Минимальное значение солнечного датчика меньше которого считаем ошибкой
int16_t mtr_new_speed;
 
int angle_transformation(int alpha, int alpha_goal){
	/* Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal
// По умолчанию угол определяется в диапазоне от -180 до 180 градусов
// Функция angle_transformation изменяет диапазона измерения угла. Для решения 
задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до 
(alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра 
и проекцией вектора магнитного поля на горизонтальную плоскость.*/
	if (alpha <= (alpha_goal - 180)){
		alpha = alpha + 360;
	}
	else if (alpha>(alpha_goal +180)){
		alpha = alpha - 360;
	}
	return alpha;
}
 
int motor_new_speed_PD(int mtr_speed,int alpha,int alpha_goal, float omega, int omega_goal){
	/* Определение новой скорости маховика
// Новая скорость маховика равна текущей скорости маховика + приращение скорости
// Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости*/
	int mtr_new_speed = mtr_speed + kp*(alpha-alpha_goal) + 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 Sun sensors 1-4\n");
	sun_sensor_turn_on(1);
	sun_sensor_turn_on(2);
	sun_sensor_turn_on(3);
	sun_sensor_turn_on(4);
	Sleep(1);
}
 
void switch_off_all(void){/* Функция отключает все приборы,
 которые будут использоваться в основной программе.*/
	printf("Finishing...\n");
	int16_t new_speed = mtr_new_speed;
	hyro_turn_off(hyr_num); // Выключаем ДУС
	magnetometer_turn_off(mag_num);
	sun_sensor_turn_off(1);
	sun_sensor_turn_off(2);
	sun_sensor_turn_off(3);
	sun_sensor_turn_off(4);
	motor_set_speed(mtr_num, 0, &new_speed);
	Sleep(1);
	motor_turn_off(mtr_num);
	printf("Finish program\n");
}
 
int check_sun(int sun11, int sun12, int sun21, int sun22, int sun31, int sun32, int sun41, int sun42){ 
	int delta1 = 0;
	int delta2 = 0; 
	int delta3 = 0; 
	int delta4 = 0; 
	int delta5 = 0;
	int delta6 = 0; 
	int delta7 = 0; 
	int delta8 = 0;	
	int delta_sum = 36000;
	int delta_min = 35000;	
	int delta_i = 0; 
	float data_sun[] = {-47.329501295977, 239, 102, -48.1458278265892}; //вставить данные из текстового файла с данными
	int i;
	for (i = 0; i < 4; i++){ 				 
		delta1 = abs(sun11 - data_sun[i]);
		delta2 = abs(sun12 - data_sun[i]);
		delta3 = abs(sun21 - data_sun[i]);
		delta4 = abs(sun22 - data_sun[i]);
		delta5 = abs(sun31 - data_sun[i]);
		delta6 = abs(sun32 - data_sun[i]);
		delta7 = abs(sun41 - data_sun[i]);
		delta8 = abs(sun42 - data_sun[i]);
		delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8;
		printf("%d, %f\n", i, data_sun[i]);
		printf("Sun_sens: %d, %d, %d, %d, %d, %d, %d, %d\n", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42);
		printf("deltas= %d, %d, %d, %d, %d, %d, %d, %d, %d\n", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum);		
		if (delta_min > delta_sum) {
			delta_min = delta_sum;
			delta_i = data_sun[i];
			printf("delta_min= %d\n", delta_min);
			printf("delta_i= %d\n", delta_i);
		}
	}
	return delta_i;
}
 
int sun_range(uint16_t *sun_1, uint16_t *sun_2, uint16_t sun_old_1, uint16_t sun_old_2){ 	
 
	if (*sun_1 > sun_max){
		*sun_1 = sun_old_1;
	}
 
	if (*sun_1 < sun_min){
		*sun_1 = sun_old_1;
	}
	if (*sun_2> sun_max){
		*sun_2 = sun_old_2;
	}
	if (*sun_2 < sun_min){
		*sun_2 = sun_old_2;
	}		
	return 0;
}
 
int control(){ // основная функция программы
	initialize_all(); // Инициализируем статус маховика
	int mtr_state = 0;  // Инициализируем статус ДУС	
	int hyro_state = 0;
	//int mag_state = 0; //Инициализируем статус магнитометра 
	uint16_t sun_result_1[] = {0,0,0}; // Инициализируем sun_result_1
	uint16_t sun_result_2[] = {0,0,0}; // Инициализируем sun_result_2
	uint16_t sun_result_3[] = {0,0,0}; // Инициализируем sun_result_3
	uint16_t sun_result_4[] = {0,0,0}; // Инициализируем sun_result_4
	uint16_t sun_result_1_Old[] = {0,0,0}; // Инициализируем sun_result_1
	uint16_t sun_result_2_Old[] = {0,0,0}; // Инициализируем sun_result_2
	uint16_t sun_result_3_Old[] = {0,0,0}; // Инициализируем sun_result_3
	uint16_t sun_result_4_Old[] = {0,0,0}; // Инициализируем sun_result_4
	int16_t mtr_speed;
	//данные ДУС
	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 mgx;
	int16_t mgy;
	int16_t mgz;
	int16_t *magx_raw = &mgx;
	int16_t *magy_raw = &mgy; 
	int16_t *magz_raw = &mgz;*/
 
	int16_t omega;
	int omega_goal = 0;		// omega_goal - целевая угловая скорость спутника, град/с
	int alpha_goal = 30;		// Целевой угол поворота
	int i;
	int j;
	for (i = 0; i < 300; i++){
		// Запоминаем старые значения
		for(j = 0; j < 3; j++){
		sun_result_1_Old[j] = sun_result_1[j];	
		sun_result_2_Old[j] = sun_result_2[j];		
		sun_result_3_Old[j] = sun_result_3[j];		
		sun_result_4_Old[j] = sun_result_4[j];
		}		
		// опрос датчиков и маховика
		sun_result_1[0] = sun_sensor_request_raw(1,&sun_result_1[1],&sun_result_1[2]);
		sun_result_2[0] = sun_sensor_request_raw(2,&sun_result_2[1],&sun_result_2[2]);			
		sun_result_3[0] = sun_sensor_request_raw(3,&sun_result_3[1],&sun_result_3[2]);
		sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]);
		//print sun_result_1, sun_result_2, sun_result_3, sun_result_4
		// Проверяем чтобы новое измеренное значение не выходило за пределы диапазона
		sun_result_1[0] = sun_range( & sun_result_1[1], & sun_result_1[2], sun_result_1_Old[1], sun_result_1_Old[2]);
		sun_result_2[0] = sun_range( & sun_result_2[1], & sun_result_2[2], sun_result_2_Old[1], sun_result_2_Old[2]);
		sun_result_3[0] = sun_range( & sun_result_3[1], & sun_result_3[2], sun_result_3_Old[1], sun_result_3_Old[2]);
		sun_result_4[0] = sun_range( & sun_result_4[1], & sun_result_4[2], sun_result_4_Old[1], sun_result_4_Old[2]);
		int alpha_sun = check_sun(sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2]);
		alpha_sun = angle_transformation(alpha_sun, alpha_goal);
		printf("alpha_sun = %d\n",alpha_sun);
 
		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);
		//mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw);
		mtr_speed = motor_request_speed(mtr_num, &mtr_speed);
 
 
		if (!hyro_state){ // если ДУС вернул код ошибки 0, т.е. ошибки нет
			float gx_degs = gx_raw * 0.00875;
			float gy_degs = gy_raw * 0.00875;
			float gz_degs = gz_raw * 0.00875;
			omega = (int16_t)gz_degs;	// если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z
			printf("gx_degs = %f, gy_degs = %f, gz_degs = %f\n", gx_degs, gy_degs, gz_degs); // Выводим данные
		}
		else if (hyro_state == 1){ // если датчик вернул сообщение об ошибке 1
			printf("Fail because of access error, check the connection");
		}
		else if (hyro_state == 2){ // если датчик вернул сообщение об ошибке 2
			printf("Fail because of interface error, check your code");
		}
 
		if (!mtr_state)	{ // если код ошибки 0, т.е. ошибки нет
			int16_t mtr_speed = 0;
			motor_request_speed(mtr_num, &mtr_speed);
			printf("Motor_speed: %d\n", mtr_speed);	
			// установка новой скорости маховика
			mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun, alpha_goal,omega,omega_goal );
			motor_set_speed(mtr_num, mtr_new_speed, &omega);
		}	
 
		Sleep(time_step);
	}
	switch_off_all();
	return 0;
}

Принцип работы программы

В огромном массиве с данными хранятся значения, измеренные при калибровке солнечных датчиков и значения углов. Функция check_sun получает 8 значений, считанных с солнечных датчиков (по два с каждого), и вычисляет 8 значений отклонений delta1…delta8, как модуль разности между измеренными значениями и значениями из таблицы. Затем вычисляется суммарное отклонение delta_sum. Программа проходит по всему массиву значений и ищет минимальное отклонение delta_sum и соответствующее значение угла. В результате функция check_sun возвращает значение угла, на который повернут Орбикрафт. Функция sun_range отбраковывает ошибочные значения, полученные от солнечного датчика, и если измеренное значение больше 25000 или меньше 50 (больше sun_max или меньше sun_min), то она вернет старое значение из нормального диапазона. Применение этой функции позволяет избавиться от ошибок измерения, которые иногда возникают. Основная функция программы control 300 раз в цикле считывает значения с солнечных датчиков и проверяет чтобы значения не выходили за пределы диапазона с помощью функции sun_range. Корректные значения передаются в функцию check_sun, которая возвращает значение угла, на который повернут Орбикрафт. Затем с помощью функции angle_transformation происходит трансформация угла к диапазону -180…180, и с помощью функции motor_new_speed_PD вычисляется новая скорость с которой необходимо вращаться маховику. Затем работа программы приостанавливается на время time_step.

en/lesson10.1573124081.txt.gz · Last modified: 2020/03/25 16:29 (external edit)