satellite_model/src/magnetic.py - main - fedorovaaa2/ucheba_python

satellite_model/src/magnetic.py
204 строки8 KB
Zaynulla Zhumaev
* sat_model: pytest, ruff fixed;
24 сен 2023, 12:45
24 сен 2023, 12:451c85779
import datetime

import igrf
import numpy as np
from pymap3d import ecef2eci, eci2geodetic, ned2ecef

from adcs import unit_vector
from mission_parameters import COILS_MAT, MAGNET_MOUNT, SWITCHING_COEF

# Компиляция библиотеки igrf (запускается один раз, нужен cmake)
# igrf.base.build()


def required_magnetic_moment(B_dot_body: np.ndarray, B_DOT_K: float) -> list:
    """Calculates the required_magnetic_moment by using the B_DOT algorithm

    Parameters
    ----------
    B_dot_body : 1x3 list
        the temporal derivative of the B field with respect to body frame.
    B_DOT_K : coefficient
        B-Dot algorithm coefficient

    Returns
    -------
    required_magnetic_moment : 1x3 list
        required magnetic moment
    """
    required_magnetic_moment = -B_dot_body * B_DOT_K

    return required_magnetic_moment


def B_dot_body(B_body: np.ndarray, sat_omega_body: np.ndarray) -> np.ndarray:
    """Функция для расчёта производной вектора внешнего магнитного поля в связанной СК
    спутника

    Args:
        B_body (np.ndarray): вектор магнитного поля sat_omega_body (np.ndarray): угловая
        скорость аппарата

    Returns:
        np.ndarray: производная вектора магнитного поля в связанной СК.

    В соответствии с формулой Бура, при равенстве нулю полной производной, локальная
    производная (то есть результат функции) равен по модулю и противоположен по знаку
    векторному произведению угловой скорости на вектор поля.
    """
    B_dot_body = np.cross(B_body, sat_omega_body)
    return B_dot_body


# %%
def magnetic_field_vector_eci(
    sat_position_eci: np.ndarray, time: datetime
) -> np.ndarray:
    """Данная функция предназначена для вычисления магнитного вектора Земли

    Parameters
    ----------
    sat_position_eci : 1x3 np.ndarray вектор положения КА в геоцентрической инерциальной
        системе координат
    time : datetime время

    Return
    ------
    1x3 np.ndarray : вектор магнитного поля Земли в геоцентрической инерциальной системе
    координат
    """
    geodetic_vector = eci2geodetic(
        sat_position_eci[0], sat_position_eci[1], sat_position_eci[2], time
    )
    lat = geodetic_vector[0]
    lon = geodetic_vector[1]
    alt = geodetic_vector[2]

    mag = igrf.igrf(
        str(time.date()), glat=float(lat), glon=float(lon), alt_km=float(alt)
    )

    buff_value = []
    for i in mag.variables.items():
        buff_value.append(float(i[1][0]))

    mag_north = buff_value[0]
    mag_east = buff_value[1]
    mag_down = buff_value[2]

    ecef_coord = ned2ecef(
        n=mag_north, e=mag_east, d=mag_down, lat0=lat, lon0=lon, h0=alt
    )
    eci_coord = ecef2eci(x=ecef_coord[0], y=ecef_coord[1], z=ecef_coord[2], time=time)
    mag_vector_eci = np.array(eci_coord) - np.array(sat_position_eci)

    return mag_vector_eci


# %%
def magnetic_torquers_mechanical_torque(
    m_body: np.ndarray, B_body: np.ndarray
) -> np.ndarray:
    """Данная функция предназначена для расчета механического момента КА (космический
    аппарат).

    Parameters
    ----------
    m_body : 1x3 list собственный магнитный момент B_body : 1x3 list магнитная индукция
    магнитного поля Земли

    Returns
    -------
    1x3 np.ndarray
        механический момент космического аппарата равный векторному произведению
        векторов m_body и B_body
    """

    T_body = np.cross(m_body, B_body)
    return T_body


# %%
def actual_magnetic_moment(
    m_required_body: np.ndarray, B_body: np.ndarray
) -> np.ndarray:
    """Возвращает фактический магнитный момент КА.

    Parameters
    ----------
        m_required_body: требуемый магнитный момент
        B_body: вектор магнитного поля Земли

    Returns
    -------
        m_actual_body: actual magnetic moment (in body coord. system)

    Фактический магнитный момент равен проекции требуемого на ось катушки. Если это
    значение превышает макс. возможный момент, выдаваемый катушкой, она выдаёт
    максимально возможное для себя значение.
    """
    m_required_body = np.array(m_required_body)
    B_body = np.array(B_body)

    # переменная для вывода - суммарный момент всех катушек
    actual_magnetic_moment = np.array([0, 0, 0])

    # единичный вектор вдоль магнитного поля (бесполезный момент)
    unit_useless = unit_vector(B_body)

    # единичный вектор вдоль требуемого момента (полезный момент)
    unit_useful = unit_vector(
        m_required_body - unit_useless * m_required_body.dot(unit_useless)
    )

    # единичный вектор вдоль 3-го направления (вредный момент)
    unit_bad = unit_vector(np.cross(unit_useless, unit_useful))

    # Проверим, имеет ли смысл включать каждую катушку
    # цикл проверки каждой катушки
    for coil in COILS_MAT:
        # составляющие магнитного момента катушки (проекции)
        coil_useful = coil.dot(unit_useful)
        coil_useless = coil.dot(unit_useless)
        coil_bad = coil.dot(unit_bad)

        # ось катушки (вектор)
        coil_axis = unit_vector(coil)

        # выходной момент катушки
        coil_out = np.array([0, 0, 0])

        # условие включения катушки (её полезный момент достаточно велик)
        coil_switcher = (
            abs(coil_useful) > (abs(coil_useless) + abs(coil_bad)) * SWITCHING_COEF
        )

        # если условие выполняется, будем включать катушку
        if coil_switcher:
            # выходной момент катушки:
            coil_out = coil_axis * m_required_body.dot(coil_axis)

            # при этом выходной момент не может быть больше макс. возможного
            if np.linalg.norm(coil_out) > np.linalg.norm(coil):
                coil_out = coil * np.sign(m_required_body.dot(coil_axis))

        actual_magnetic_moment = actual_magnetic_moment + coil_out

    return np.array(actual_magnetic_moment)


# %%
def magnetometer_measure(B_body: np.ndarray) -> np.ndarray:
    """Пересчет магнитного момента из с.к. спутника в с.к. датчика

    Parameters
    -----------
    B_body: 1х3 Магнитный момент в с.к. спутника
    MAGNET_MOUNT: 3х3 Установочная матрица датчика

    Returns
    ----------
    B_sensor: 1х3 Магнитный момент в с.к. датчика"""

    B_sensor = MAGNET_MOUNT.dot(B_body)
    return B_sensor