What is Orbicraft for?
How to work with it
Orbicraft Subsystems
Arduino-Based payload
Lessons
Laboratory equipment
Feedback
News
What is Orbicraft for?
How to work with it
Orbicraft Subsystems
Arduino-Based payload
Lessons
Laboratory equipment
Feedback
News
This is an old revision of the document!
This page is not fully translated, yet. Please help completing the translation.
(remove this paragraph once the translation is finished)
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.
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()
Код на С.
#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.