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

Инструменты сайта


lesson8

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

Предыдущая версия справа и слева Предыдущая версия
Следующая версия
Предыдущая версия
lesson8 [2019/05/14 11:27]
golikov [Проверка работоспособности солнечных датчиков]
lesson8 [2020/03/25 16:28] (текущий)
Строка 110: Строка 110:
 Загрузите в БКУ следующую программу. Загрузите в БКУ следующую программу.
  
-<code python>+Код на Python. 
 +<file python ​sun_raw.py> 
 import time import time
 import math import math
Строка 289: Строка 291:
  print output_data_all[i-1],​ output_data_all[i],​ output_data_all[i+1],​ output_data_all[i+2],​ output_data_all[i+3],​ output_data_all[i+4],​ output_data_all[i+5],​ output_data_all[i+6],​ output_data_all[i+7],​ output_data_all[i+8]   print output_data_all[i-1],​ output_data_all[i],​ output_data_all[i+1],​ output_data_all[i+2],​ output_data_all[i+3],​ output_data_all[i+4],​ output_data_all[i+5],​ output_data_all[i+6],​ output_data_all[i+7],​ output_data_all[i+8]
  
-</​code>​+</file> 
 + 
 +Код на С. 
 +<file c sun_raw.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>​  
 +#include <​time.h>​ 
 + 
 +/​*Коэффициент дифференциальной обратной связи. 
 + ​Коэффициент положительный,​ если маховик расположен осью z вверх 
 + и ДУС расположен осью z также вверх. 
 + ​Коэффициент подбирается экспериментально в зависимости от формы 
 + и массы вашего спутника.*/​ 
 +const float kd = 200.0; 
 + 
 +// Временной шаг работы алгоритма,​ с 
 +const float time_step = 0.1; 
 + 
 +/* Целевая угловая скорость спутника,​ град/​с. Для режима стабилизации равна 0.0.*/ 
 +const float omega_goal = 0.0; 
 +// Максимально допустимая скорость маховика,​ об/​мин 
 +const int mtr_max_speed = 5000; 
 +const uint16_t mtr_num = 1; // Номер маховика 
 +const uint16_t hyr_num = 1;         // Номер ДУС 
 +const uint16_t mag_num = 1; // Номер магнитометра 
 +// Номер результата измерений 
 +int i = 1; 
 +// Угол поворота текущий 
 +const float alpha = 0.0; 
 + 
 + 
 +  
 +void initialize_all(void){/​* Функция включает все приборы,​  
 +которые будут использоваться в основной программе.*/​ 
 + 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); // Ждем включения 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);​ 
 + printf("​Enable motor №%d\n",​ mtr_num);  
 + motor_turn_on(mtr_num);​ 
 + Sleep(1);​ 
 +
 + 
 +void switch_off_all(void){/​* Функция отключает все приборы,​ 
 + ​которые будут использоваться в основной программе.*/​ 
 + printf("​Finishing..."​);​ 
 + int16_t new_speed = 0; 
 + 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("​\nFinish program\n"​);​ 
 +
 + 
 +int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ 
 + /​*Функция mag_calibrated вносит поправки  
 +в показания магнитометра с учетом калибровочных коэффициентов 
 + ​вместо этих 3-х строк кода с коэффициентами калибровки,​ должны быть строки с коэффициентами калибровки для Вашего магнитометра 
 + //magx_cal = 1.04*magx - 0.26*magy + 0.05*magz - 68.76 # это 1-я строчка,​ которую нужно заменить по результатам калибровки Вашего магнитометра 
 + //magy_cal = 0.24*magx + 1.04*magy + 0.29*magz + 256.92 # это 2-я строчка,​ которую нужно заменить по результатам калибровки Вашего магнитометра 
 + //magz_cal = -0.09*magx - 0.19*magy + 0.77*magz + 159.41 # это 3-я строчка,​ которую нужно заменить по результатам калибровки Вашего магнитометра*/​ 
 + float magx_cal; 
 + float magy_cal; 
 + float magz_cal; 
 + 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); 
 + *magx = magx_cal; 
 + *magy = magy_cal; 
 + *magz = magz_cal; 
 + return 0; 
 +
 +  
 +int motor_new_speed_PD(int mtr_speed, float omega, int16_t omega_goal){  
 + /* Функция для определения новой скорости маховика. 
 + ​Новая скорость маховика складывается из 
 + ​текущей скорости маховика и приращения скорости. 
 + ​Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. 
 + ​mtr_speed - текущая угловая скорость маховика,​ об/​мин 
 + omega - текущая угловая скорость спутника,​ град/​с 
 + ​omega_goal - целевая угловая скорость спутника,​ град/​с 
 + ​mtr_new_speed - требуемая угловая скорость маховика,​ об/​мин*/​ 
 + int16_t 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;​ 
 +
 + 
 +int control(){//​ Основная функция программы,​ в которой вызываются остальные функции. 
 + int16_t omega; 
 + int omega_goal = 0; // omega_goal - целевая угловая скорость спутника,​ град/​с 
 + initialize_all();​  
 + int mtr_state = 0; // Инициализируем статус маховика  
 + int hyro_state = 0; // Инициализируем статус ДУС 
 + int mag_state = 0; // Инициализируем статус магнитометра 
 + int16_t mtr_speed;​ 
 + int16_t mtr_new_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_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;​ 
 + 
 + float gx_degs; 
 + float gy_degs; 
 + float gz_degs; 
 + 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 
 + int mag_alpha = 0; 
 + const int sizeOD = 10; 
 +    int sizeODA = 0; 
 +    int tempSODA; 
 +// double* output_data = (double*)calloc(sizeOD,​ sizeof(double));​ 
 +    double* output_data_all = (double*)calloc(sizeODA,​ sizeof(double));​ 
 +  
 + // Номер результата измерений 
 + int i = 0; 
 + // Запоминаем время начала вращения 
 + long int time_start = time(NULL);​ 
 + // Запоминаем время начала секундного интервала 
 + long int time_interval = time(NULL);​ 
 + // Интервал вывода данных с солнечных датчиков в секундах 
 + int time_output = 0.1; 
 + int j; 
 + char a=1; 
 +  
 + while (a==1){ 
 + // Опрос датчика угловой скорости и маховика. 
 + hyro_state = hyro_request_raw(hyr_num,​hyrox_raw,​hyroy_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);​ 
 + 
 +  
 + if (!hyro_state){ 
 + /​*Обработка показаний датчика угловой скорости,​ 
 + вычисление угловой скорости спутника по показаниям ДУС. 
 + Если код ошибки ДУС равен 0, т.е. ошибки нет*/ 
 + gx_degs = gx_raw * 0.00875;  
 + gy_degs = gy_raw * 0.00875; 
 + gz_degs = gz_raw * 0.00875; 
 + /* если ДУС установлен осью z вверх, то угловая скорость 
 + спутника совпадает с показаниями ДУС по оси z, иначе 
 + необходимо изменить знак: omega = - gz_degs*/ 
 + omega = gz_degs; 
 +//​ printf("​gx_degs=%f,​ gy_degs=%f, gz_degs=%f\n",​ gx_degs, gy_degs, 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("​Motor_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);​ 
 + long int time_current = time(NULL) - time_start;​ 
 +  
 + 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;​ 
 +
 +  
 + if ((time(NULL) - time_interval) > time_output){ 
 + // Запоминаем время начала следующего секундного интервала 
 + time_interval = time(NULL);​ 
 + 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]);​ 
 +  
 + int output_data[] = {time_current,​ 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],​ mag_alpha};​ 
 + tempSODA = sizeODA; 
 + sizeODA += sizeOD; 
 + output_data_all = (double*)realloc(output_data_all,​ sizeODA*sizeof(double));​ 
 + for (j=tempSODA;​ j<​sizeODA;​ j++) { 
 + output_data_all[j] = output_data[j-sizeODA+1];​ 
 +
 + }  
 + if (i > 100){  // Начинаем вращение через 5с после запуска 
 + omega_goal = 6.0;  // omega_goal - целевая угловая скорость спутника,​ град/​с 
 +
 +  
 + if (time_current > 90){ 
 + break;​  
 +
 + i += 1;  
 + }  
 +  
 + switch_off_all();​ 
 +  
 + printf("​time_end = %ld" , time(NULL) - time_start);​ 
 +  
 + for (i = 0; i < 5000; i = i + 10){ 
 + printf("​%f,​ %f, %f, %f, %f, %f, %f, %f, %f, %f\n",​output_data_all[i],​ output_data_all[i+1],​ output_data_all[i+2],​ output_data_all[i+3],​ output_data_all[i+4],​ output_data_all[i+5],​ output_data_all[i+6],​ output_data_all[i+7],​ output_data_all[i+8],​ output_data_all[i+9]);​ 
 +
 + printf ("​Ok\n"​);​ 
 + return 0; 
 +
 +</file> 
  
 По результатам работы программы будут выведены 500 строк с данными следующего вида: По результатам работы программы будут выведены 500 строк с данными следующего вида:
lesson8.1557822432.txt.gz · Последние изменения: 2020/03/25 16:29 (внешнее изменение)

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