Миссия дистанционного зондирования Земли
Запустите ПО ЦУП (рисунок 1)
Рисунок 1. ПО ЦУП
Нажмите на кнопку Подключить для подключения ПО ЦУП к глобусу по USB;
Нажмите на кнопку Исходная чтобы установить глобус в исходное положение;
Нажмите на кнопку Старт непосредственно перед запуском программы в Орбикрафт.
Определите положение ОрбиКрафт относительно станции приема после установки глобуса в исходное положение.
Это нужно для определения времени задержки, которое необходимо прописать в программе Орбикрафт.
Например, если точка фотографирования находится на меридиане 80, а орбикрафт подвешен напротив меридиана 40, то расстояние между ними равно 40 градусов. Скорость вращения глобуса - один оборот за 5 минут (360 градусов за 600 секунд, или 0,6 градусов в секунду). Следовательно на угол в 40 градусов глобус повернется за 24 секунды. Используйте эту величину в программе Орбикрафт.
Загрузите в Орбикрафт следующий код Код на языке Python для стабилизации Орбикрафт («спутника») и получения фотографий глобуса («Земли»):
import math
time_step = 0.1 # Временной шаг работы алгоритма, с
omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0.
mtr_max_speed = 3500 # Максимально допустимая скорость маховика, об/мин
mtr_num = 1 # Номер маховика
hyr_num = 1 # Номер ДУС
mag_num = 1 # Номер магнитометра
alpha_goal = 0 # Целевой угол поворота
errora = 0 # Ошибка по углу
Pa = 0 # Воздействие пропорционального звена
Da = 0 # Воздействие дифференциального звена
Ia = 0 # Воздействие интегрального звена
Kpa = 20 # Пропорциональный коэфициент ошибки по углу
Kda = 0.1 # Дифференциальный коэффициент
Kia = 1 # Интегральный коэффициент
lastErrora = 0 # Прошлая ошибка по углу
Integratora = 0 # Интеграл (сумма всех ошибок по углу)
PID_alpha = 0 # Величина управляющего воздействия
Integrator_maxa = 10 # Ограничение максимального значения интергатора
Integrator_mina = -10 # Ограничение минимального значения интергатора
error = 0 # Ошибка по угловой скорости
P = 0 # Воздействие пропорционального звена
D = 0 # Воздействие дифференциального звена
I = 0 # Воздействие интегрального звена
Kp = 20 # Пропорциональный коэффициент
Kd = 0.1 # Дифференциальный коэффициент
Ki = 1 # Интегральный коэффициент
lastError = 0 # Прошлая ошибка по угловой скорости
Integrator = 0 # Интеграл (сумма всех ошибок по угловой скорости)
PID_omega = 0 # Величина управляющего воздействия
Integrator_max = 10 # Ограничение максимального значения интергатора
Integrator_min = -10 # Ограничение минимального значения интергатора
# Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
def mag_calibrated(magx,magy,magz): # коэффициенты после калибровки магнитометра у вас будут другими
magx_cal = 1.668867*(magx + -38.326818) + -0.007649*(magy + 20.442007) + 0.113348*(magz + -69.659602)
magy_cal = -0.007649*(magx + -38.326818) + 1.749597*(magy + 20.442007) + 0.130999*(magz + -69.659602)
magz_cal = 0.113348*(magx + -38.326818) + 0.130999*(magy + 20.442007) + 1.577785*(magz + -69.659602)
return magx_cal, magy_cal, magz_cal
# Функции для определение новой скорости маховика.
# Новая скорость маховика складывается из
# текущей скорости маховика mtr_speed и двух приращений скорости PID_omega и PID_alpha
# PID_omega - приращение скорости для уменьшения отклонения по угловой скорости
# PID_alpha - приращение скорости для уменьшения отклонения по углу
# Приращения скорости получены путем сложения трех коэффициентов:
# P - пропорциональный коэффициент регулятора
# I - интегральный коэффициент регулятора
# D - дифференциальный коэффициент регулятора
# Integrator накапливает суммарную ошибку
# Сатуратор ограничивает максимальную величину суммарной ошибки
# mtr_speed - текущая угловая скорость маховика, об/мин
# omega - текущая угловая скорость спутника, град/с
# omega_goal - целевая угловая скорость спутника, град/с
# mtr_new_speed - требуемая угловая скорость маховика, об/мин
# все переменные регулятора отклонения по углу имеют букву "а" на конце имени
def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal):
global Integrator
global lastError
global Integratora
global lastErrora
error = omega - omega_goal # Вычисление ошибки
P = Kp * error # Вычисление воздействия пропорционального звена
D = Kd * (error - lastError) / time_step # Вычисление воздействия дифференциального звена
lastError = error # Запоминаем ошибку
Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку
if Integrator > Integrator_max: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
Integrator = Integrator_max
elif Integrator < Integrator_min:
Integrator = Integrator_min
I = Integrator * Ki # Вычисление воздействия интегрального звена
PID_omega = P + I + D # Вычисление суммарного управляющего воздействия
errora = alpha - alpha_goal # Вычисление ошибки
Pa = Kpa * errora # Вычисление воздействия пропорционального звена
Da = Kda * (errora - lastErrora) / time_step # Вычисление воздействия дифференциального звена
lastErrora = errora # Запоминаем ошибку
Integratora = Integratora + errora * time_step # Накапливаем суммарную ошибку
if Integratora > Integrator_maxa: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
Integratora = Integrator_maxa
elif Integratora < Integrator_mina:
Integratora = Integrator_mina
Ia = Integratora * Kia # Вычисление воздействия интегрального звена
PID_alpha = Pa + Ia + Da # Вычисление суммарного управляющего воздействия
mtr_new_speed = int(mtr_speed + PID_omega + PID_alpha)
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
print "P= ", P, "\t\tI= ", I, "\t\tD= ", D, "\t\tPID_omega= ", PID_omega, "\tmtr_new_speed= ", mtr_new_speed
print "Pa= ", Pa, "\tIa= ", Ia, "\t\tDa= ", Da, "\tPID_alpha=", PID_alpha
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 magnetometer", mag_num
magnetometer_turn_on(mag_num) # Включаем магнитометр
sleep(1)
camera_turn_on() #Включаем камеру и передатчик и ждем их загрузки
transmitter_turn_on(1)
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) # Выключаем мотор
camera_turn_off() #Включаем камеру и передатчик и ждем их загрузки
transmitter_turn_off(1)
sleep(1)
print "Finish program"
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 control():
initialize_all()
mtr_state = 0 # Инициализируем статус маховика
hyro_state = 0 # Инициализируем статус ДУС
mag_state = 0 # Инициализируем статус магнитометра
alpha_goal = -120 # Целевой угол
omega_goal = 0 # Целевая угловая скорость
mag_alpha = 0
frame = 0
err = 0
omega = 0
# глобус делает оборот в 360 градусов за 600 секунд. Угловая скорость 0.6 градуса в секунду
# по меридианам определяем угол от орбикрафт до точки приема (у меня 40 градусов)
# вычисляем задержку 40*0.6=24 сек. И вычитаем 5 сек на поворот спутника 24-5=19
sleep(22) # подбираем вручную время (или вычисляем по формуле в зависимости от угловой скорости глобуса)
for i in range(60):
# Опрос магнитометра, датчика угловой скорости и маховика
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)
mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180
mag_alpha = angle_transformation(mag_alpha, alpha_goal)
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"
# Обработка показаний датчика угловой скорости,
# вычисление угловой скорости спутника по показаниям ДУС.
# Если код ошибки ДУС равен 0, т.е. ошибки нет
if not hyro_state:
gz_degs = gz_raw * 0.00875
# если ДУС установлен осью z вверх, то угловая скорость
# спутника совпадает с показаниями ДУС по оси z, иначе
# необходимо изменить знак: omega = - gz_degs
omega = gz_degs #если ДУС установлен осью z вверх
elif hyro_state == 1:
print "Fail because of access error, check the connection"
elif hyro_state == 2:
print "Fail because of interface error, check your code"
#Обработка показаний маховика и установка требуемой угловой скорости.
if not mtr_state: # если код ошибки 0, т.е. ошибки нет
print "Motor_speed: ", mtr_speed, "\t\ti= ", i,
# установка новой скорости маховика
mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal)
motor_set_speed(mtr_num, mtr_new_speed)
if i == 50: # Делаем снимок
err_camera = camera_take_photo(frame)
if err == 1:
print 'Camera access error, check the connection', frame
elif err == 2:
print 'Camera interface errorr, check the code', frame
#Передаем снимок
err_transmitter = transmitter_transmit_photo(1, frame)
if err_transmitter == 1:
print 'Transmition failed'
sleep(time_step)
switch_off_all()
Код на языке Python для стабилизации Орбикрафт («спутника»), получения фотографий глобуса («Земли») и передачи телеметрии по УКВ:
import math
time_step = 0.1 # Временной шаг работы алгоритма, с
omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0.
mtr_max_speed = 3500 # Максимально допустимая скорость маховика, об/мин
mtr_num = 1 # Номер маховика
hyr_num = 1 # Номер ДУС
mag_num = 1 # Номер магнитометра
alpha_goal = 0 # Целевой угол поворота
errora = 0 # Ошибка по углу
Pa = 0 # Воздействие пропорционального звена
Da = 0 # Воздействие дифференциального звена
Ia = 0 # Воздействие интегрального звена
Kpa = 20 # Пропорциональный коэффициент ошибки по углу
Kda = 0.1 # Дифференциальный коэффициент
Kia = 1 # Интегральный коэффициент
lastErrora = 0 # Прошлая ошибка по углу
Integratora = 0 # Интеграл (сумма всех ошибок по углу)
PID_alpha = 0 # Величина управляющего воздействия
Integrator_maxa = 10 # Ограничение максимального значения интергатора
Integrator_mina = -10 # Ограничение минимального значения интергатора
error = 0 # Ошибка по угловой скорости
P = 0 # Воздействие пропорционального звена
D = 0 # Воздействие дифференциального звена
I = 0 # Воздействие интегрального звена
Kp = 20 # Пропорциональный коэффициент
Kd = 0.1 # Дифференциальный коэффициент
Ki = 1 # Интегральный коэффициент
lastError = 0 # Прошлая ошибка по угловой скорости
Integrator = 0 # Интеграл (сумма всех ошибок по угловой скорости)
PID_omega = 0 # Величина управляющего воздействия
Integrator_max = 10 # Ограничение максимального значения интергатора
Integrator_min = -10 # Ограничение минимального значения интергатора
# Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
def mag_calibrated(magx,magy,magz): # коэффициенты после калибровки магнитометра у вас будут другими
magx_cal = 1.668867*(magx + -38.326818) + -0.007649*(magy + 20.442007) + 0.113348*(magz + -69.659602)
magy_cal = -0.007649*(magx + -38.326818) + 1.749597*(magy + 20.442007) + 0.130999*(magz + -69.659602)
magz_cal = 0.113348*(magx + -38.326818) + 0.130999*(magy + 20.442007) + 1.577785*(magz + -69.659602)
return magx_cal, magy_cal, magz_cal
# Функции для определение новой скорости маховика.
# Новая скорость маховика складывается из
# текущей скорости маховика mtr_speed и двух приращений скорости PID_omega и PID_alpha
# PID_omega - приращение скорости для уменьшения отклонения по угловой скорости
# PID_alpha - приращение скорости для уменьшения отклонения по углу
# Приращения скорости получены путем сложения трех коэффициентов:
# P - пропорциональный коэффициент регулятора
# I - интегральный коэффициент регулятора
# D - дифференциальный коэффициент регулятора
# Integrator накапливает суммарную ошибку
# Сатуратор ограничивает максимальную величину суммарной ошибки
# mtr_speed - текущая угловая скорость маховика, об/мин
# omega - текущая угловая скорость спутника, град/с
# omega_goal - целевая угловая скорость спутника, град/с
# mtr_new_speed - требуемая угловая скорость маховика, об/мин
# все переменные регулятора отклонения по углу имеют букву "а" на конце имени
def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal):
global Integrator
global lastError
global Integratora
global lastErrora
error = omega - omega_goal # Вычисление ошибки
P = Kp * error # Вычисление воздействия пропорционального звена
D = Kd * (error - lastError) / time_step # Вычисление воздействия дифференциального звена
lastError = error # Запоминаем ошибку
Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку
if Integrator > Integrator_max: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
Integrator = Integrator_max
elif Integrator < Integrator_min:
Integrator = Integrator_min
I = Integrator * Ki # Вычисление воздействия интегрального звена
PID_omega = P + I + D # Вычисление суммарного управляющего воздействия
errora = alpha - alpha_goal # Вычисление ошибки
Pa = Kpa * errora # Вычисление воздействия пропорционального звена
Da = Kda * (errora - lastErrora) / time_step # Вычисление воздействия дифференциального звена
lastErrora = errora # Запоминаем ошибку
Integratora = Integratora + errora * time_step # Накапливаем суммарную ошибку
if Integratora > Integrator_maxa: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
Integratora = Integrator_maxa
elif Integratora < Integrator_mina:
Integratora = Integrator_mina
Ia = Integratora * Kia # Вычисление воздействия интегрального звена
PID_alpha = Pa + Ia + Da # Вычисление суммарного управляющего воздействия
mtr_new_speed = int(mtr_speed + PID_omega + PID_alpha)
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
print "P= ", P, "\t\tI= ", I, "\t\tD= ", D, "\t\tPID_omega= ", PID_omega, "\tmtr_new_speed= ", mtr_new_speed
print "Pa= ", Pa, "\tIa= ", Ia, "\t\tDa= ", Da, "\tPID_alpha=", PID_alpha
return mtr_new_speed
# Функция включает все приборы,
# которые будут использоваться в основной программе.
def initialize_all(): # Функция инициализации всех систем
print "Enable motor №", mtr_num
motor_turn_on(mtr_num) # Включаем мотор
sleep(1)
print "Enable angular velocity sensor"
hyro_turn_on(hyr_num) # Включаем ДУС
sleep(1)
print "Enable magnetometer", mag_num
magnetometer_turn_on(mag_num) # Включаем магнитометр
sleep(1)
camera_turn_on() # Включаем камеру
sleep(1)
transmitter_turn_on(1)
sleep(1)
transceiver_turn_on(2) # Бортовой УКВ имеет номер 2
sleep(1)
def switch_off_all(): # Функция выключения всех систем
print "Finishing..."
print "Disable angular velocity sensor"
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) # Выключаем мотор
sleep (1)
camera_turn_off() # Выключаем камеру
sleep(1)
transmitter_turn_off(1)
sleep(1)
transceiver_turn_off(2)
sleep(1)
print "Finish program"
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 uhf(msg):
error = transceiver_send(2, 1, msg)
if not error:
print "data has been transmitted"
elif error == 1: # если датчик вернул сообщение об ошибке 1
print "Fail because of access error, check the connection"
elif error == 2: # если датчик вернул сообщение об ошибке 2
print "Fail because of interface error, check your code"
# Основная функция программы, в которой вызываются остальные функции.
def control():
initialize_all()
mtr_state = 0 # Инициализируем статус маховика
hyro_state = 0 # Инициализируем статус ДУС
mag_state = 0 # Инициализируем статус магнитометра
alpha_goal = -120 # Целевой угол
omega_goal = 0 # Целевая угловая скорость
mag_alpha = 0
frame = 0
err = 0
omega = 0
bus_setup();
#глобус делает оборот в 360 градусов за 600 секунд. Угловая скорость 0.6 градуса в секунду
# по меридианам определяем угол от орбикрафт до точки приема (у меня 40 градусов)
# вычисляем задержку 40*0.6=24 сек. И вычитаем 5 сек на поворот спутника 24-5=19
sleep(22) # подбираем вручную время (или вычисляем по формуле в зависимости от угловой скорости глобуса)
message = "Start mission\n"
uhf(message)
for i in range(60):
# Опрос магнитометра, датчика угловой скорости и маховика
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)
mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180
mag_alpha = angle_transformation(mag_alpha, alpha_goal)
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"
# Обработка показаний датчика угловой скорости,
# вычисление угловой скорости спутника по показаниям ДУС.
# Если код ошибки ДУС равен 0, т.е. ошибки нет
if not hyro_state:
gz_degs = gz_raw * 0.00875
# если ДУС установлен осью z вверх, то угловая скорость
# спутника совпадает с показаниями ДУС по оси z, иначе
# необходимо изменить знак: omega = - gz_degs
omega = gz_degs #если ДУС установлен осью z вверх
elif hyro_state == 1:
print "Fail because of access error, check the connection"
elif hyro_state == 2:
print "Fail because of interface error, check your code"
#Обработка показаний маховика и установка требуемой угловой скорости.
if not mtr_state: # если код ошибки 0, т.е. ошибки нет
print "Motor_speed: ", mtr_speed, "\t\ti= ", i,
# установка новой скорости маховика
mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal)
motor_set_speed(mtr_num, mtr_new_speed)
if i == 50: # Делаем снимок
err_camera = camera_take_photo(frame)
if err == 1:
print 'Camera access error, check the connection', frame
elif err == 2:
print 'Camera interface errorr, check the code', frame
#Передаем снимок
err_transmitter = transmitter_transmit_photo(1, frame)
if err_transmitter == 1:
print 'Transmition failed'
message = "Photo\n"
uhf(message)
message = "Omega = " +str(omega) + '\n'
uhf(message)
message = "Finish mission\n"
uhf(message)
sleep(time_step)
switch_off_all()