diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 000000000..6313b56c5 --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +* text=auto eol=lf diff --git a/gs/backend/sun/ephemeris_parser.py b/gs/backend/sun/ephemeris_parser.py index 406e0fdaf..69703a418 100644 --- a/gs/backend/sun/ephemeris_parser.py +++ b/gs/backend/sun/ephemeris_parser.py @@ -1,87 +1,87 @@ -# Parser for tests and will be used as the basis for the parser on the OBC -# Test harness for test_ephemeris.py -from __future__ import annotations - -import dataclasses -import struct - -# Standard library imports -from typing import BinaryIO - -# Local application imports -from . import ephemeris -from .ephemeris import DataPoint - - -@dataclasses.dataclass -class Header: - """ - Data class to store the header information - """ - - start_time: float - step_size: float - num_data_points: int - - -def parse_header(file: str) -> Header: - """ - Tests the header file to ensure that the data was read correctly - - :param file: The file to read from - """ - with open(file, "rb") as f: - f.seek(0) - # Read 2 double values - start_time = get_single_data_point(f, False) - step_size = get_single_data_point(f, False) - - # Read 1 uint value - num_data_points = int(struct.unpack(ephemeris.DATA_UINT, f.read(ephemeris.SIZE_OF_INT))[0]) - return Header(start_time, step_size, num_data_points) - - -def get_single_data_point(file: BinaryIO, is_float: bool = True) -> float: - """ - Tests the output file to ensure that the data was written correctly - - :param is_float: If true, then will parse as float, otherwise will parse as double - :param file: The file to read from - """ - if is_float: - read_type = ephemeris.DATA_FLOAT - read_size = ephemeris.SIZE_OF_FLOAT - else: - read_type = ephemeris.DATA_DOUBLE - read_size = ephemeris.SIZE_OF_DOUBLE - - # Read and parse 1 float/double value - byte_str = file.read(read_size) - float_val = struct.unpack(read_type, byte_str)[0] - return float(float_val) - - -def parse_file(file: str) -> list[DataPoint]: - """ - Tests the output file to ensure that the data was written correctly - - :param file: The file to read from - """ - output = [] - header = parse_header(file) - - with open(file, "rb") as f: - f.seek(ephemeris.SIZE_OF_HEADER) - - # Read and calculate the data points - for i in range(header.num_data_points): - jd = header.start_time + (i * header.step_size) - data_point = DataPoint( - jd, - get_single_data_point(f), - get_single_data_point(f), - get_single_data_point(f), - ) - output.append(data_point) - - return output +# Parser for tests and will be used as the basis for the parser on the OBC +# Test harness for test_ephemeris.py +from __future__ import annotations + +import dataclasses +import struct + +# Standard library imports +from typing import BinaryIO + +# Local application imports +from . import ephemeris +from .ephemeris import DataPoint + + +@dataclasses.dataclass +class Header: + """ + Data class to store the header information + """ + + start_time: float + step_size: float + num_data_points: int + + +def parse_header(file: str) -> Header: + """ + Tests the header file to ensure that the data was read correctly + + :param file: The file to read from + """ + with open(file, "rb") as f: + f.seek(0) + # Read 2 double values + start_time = get_single_data_point(f, False) + step_size = get_single_data_point(f, False) + + # Read 1 uint value + num_data_points = int(struct.unpack(ephemeris.DATA_UINT, f.read(ephemeris.SIZE_OF_INT))[0]) + return Header(start_time, step_size, num_data_points) + + +def get_single_data_point(file: BinaryIO, is_float: bool = True) -> float: + """ + Tests the output file to ensure that the data was written correctly + + :param is_float: If true, then will parse as float, otherwise will parse as double + :param file: The file to read from + """ + if is_float: + read_type = ephemeris.DATA_FLOAT + read_size = ephemeris.SIZE_OF_FLOAT + else: + read_type = ephemeris.DATA_DOUBLE + read_size = ephemeris.SIZE_OF_DOUBLE + + # Read and parse 1 float/double value + byte_str = file.read(read_size) + float_val = struct.unpack(read_type, byte_str)[0] + return float(float_val) + + +def parse_file(file: str) -> list[DataPoint]: + """ + Tests the output file to ensure that the data was written correctly + + :param file: The file to read from + """ + output = [] + header = parse_header(file) + + with open(file, "rb") as f: + f.seek(ephemeris.SIZE_OF_HEADER) + + # Read and calculate the data points + for i in range(header.num_data_points): + jd = header.start_time + (i * header.step_size) + data_point = DataPoint( + jd, + get_single_data_point(f), + get_single_data_point(f), + get_single_data_point(f), + ) + output.append(data_point) + + return output diff --git a/libs/cserialport/bindings/tmp.ts b/libs/cserialport/bindings/tmp.ts deleted file mode 100644 index e69de29bb..000000000 diff --git a/libs/cserialport/include/CSerialPort/SerialPort.h b/libs/cserialport/include/CSerialPort/SerialPort.h index 4c34559c0..f12fd4733 100644 --- a/libs/cserialport/include/CSerialPort/SerialPort.h +++ b/libs/cserialport/include/CSerialPort/SerialPort.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPort.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortBase.h b/libs/cserialport/include/CSerialPort/SerialPortBase.h index 172e94d17..31cc5758d 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortBase.h +++ b/libs/cserialport/include/CSerialPort/SerialPortBase.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortBase.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortInfo.h b/libs/cserialport/include/CSerialPort/SerialPortInfo.h index 04ece9b9d..73606d3dd 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortInfo.h +++ b/libs/cserialport/include/CSerialPort/SerialPortInfo.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortInfo.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortInfoBase.h b/libs/cserialport/include/CSerialPort/SerialPortInfoBase.h index a20f7cdaf..4ff86fb0c 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortInfoBase.h +++ b/libs/cserialport/include/CSerialPort/SerialPortInfoBase.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortInfoBase.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortInfoUnixBase.h b/libs/cserialport/include/CSerialPort/SerialPortInfoUnixBase.h index d08a96381..e7a108ca4 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortInfoUnixBase.h +++ b/libs/cserialport/include/CSerialPort/SerialPortInfoUnixBase.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortInfoUnixBase.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortInfoWinBase.h b/libs/cserialport/include/CSerialPort/SerialPortInfoWinBase.h index ec13dcd56..bd37671b7 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortInfoWinBase.h +++ b/libs/cserialport/include/CSerialPort/SerialPortInfoWinBase.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortInfoWinBase.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortListener.h b/libs/cserialport/include/CSerialPort/SerialPortListener.h index 298ef2557..b00f0f66c 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortListener.h +++ b/libs/cserialport/include/CSerialPort/SerialPortListener.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortListener.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortUnixBase.h b/libs/cserialport/include/CSerialPort/SerialPortUnixBase.h index 6b1cefc1b..5fe4e8fe9 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortUnixBase.h +++ b/libs/cserialport/include/CSerialPort/SerialPortUnixBase.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortUnixBase.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPortWinBase.h b/libs/cserialport/include/CSerialPort/SerialPortWinBase.h index 0b718c6a8..c4d009440 100644 --- a/libs/cserialport/include/CSerialPort/SerialPortWinBase.h +++ b/libs/cserialport/include/CSerialPort/SerialPortWinBase.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPortWinBase.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPort_global.h b/libs/cserialport/include/CSerialPort/SerialPort_global.h index b3bded196..752346338 100644 --- a/libs/cserialport/include/CSerialPort/SerialPort_global.h +++ b/libs/cserialport/include/CSerialPort/SerialPort_global.h @@ -1,4 +1,4 @@ -/** +/** * @file SerialPort_global.h * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/SerialPort_version.h b/libs/cserialport/include/CSerialPort/SerialPort_version.h index c18ba8f97..0994e4044 100644 --- a/libs/cserialport/include/CSerialPort/SerialPort_version.h +++ b/libs/cserialport/include/CSerialPort/SerialPort_version.h @@ -1,4 +1,4 @@ -/** +/** * \file SerialPort_global.h * \author itas109 \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/ibuffer.hpp b/libs/cserialport/include/CSerialPort/ibuffer.hpp index 6b23626c4..fbd39c9c0 100644 --- a/libs/cserialport/include/CSerialPort/ibuffer.hpp +++ b/libs/cserialport/include/CSerialPort/ibuffer.hpp @@ -1,4 +1,4 @@ -/** +/** * @file ibuffer.hpp * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/ithread.hpp b/libs/cserialport/include/CSerialPort/ithread.hpp index a26784de8..0654fd55a 100644 --- a/libs/cserialport/include/CSerialPort/ithread.hpp +++ b/libs/cserialport/include/CSerialPort/ithread.hpp @@ -1,4 +1,4 @@ -/** +/** * @file ithread.hpp * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/itimer.hpp b/libs/cserialport/include/CSerialPort/itimer.hpp index a1a481b3c..eb67fa4ea 100644 --- a/libs/cserialport/include/CSerialPort/itimer.hpp +++ b/libs/cserialport/include/CSerialPort/itimer.hpp @@ -1,4 +1,4 @@ -/** +/** * @file itimer.hpp * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/include/CSerialPort/iutils.hpp b/libs/cserialport/include/CSerialPort/iutils.hpp index d3712d399..0af025d8f 100644 --- a/libs/cserialport/include/CSerialPort/iutils.hpp +++ b/libs/cserialport/include/CSerialPort/iutils.hpp @@ -1,4 +1,4 @@ -/** +/** * @file iutils.hpp * @author itas109 (itas109@qq.com) \n\n * Blog : https://blog.csdn.net/itas109 \n diff --git a/libs/cserialport/source/SerialPort.cpp b/libs/cserialport/source/SerialPort.cpp index 5069d96db..bbfea02b7 100644 --- a/libs/cserialport/source/SerialPort.cpp +++ b/libs/cserialport/source/SerialPort.cpp @@ -1,4 +1,4 @@ -#include +#include #include "CSerialPort/SerialPort.h" #include "CSerialPort/SerialPort_version.h" diff --git a/libs/cserialport/source/SerialPortBase.cpp b/libs/cserialport/source/SerialPortBase.cpp index 8999b02d6..135697fc4 100644 --- a/libs/cserialport/source/SerialPortBase.cpp +++ b/libs/cserialport/source/SerialPortBase.cpp @@ -1,4 +1,4 @@ -#include "CSerialPort/SerialPortBase.h" +#include "CSerialPort/SerialPortBase.h" #include "CSerialPort/ithread.hpp" #include "CSerialPort/itimer.hpp" diff --git a/libs/cserialport/source/SerialPortInfo.cpp b/libs/cserialport/source/SerialPortInfo.cpp index 08e9e7d34..a2b7c12f6 100644 --- a/libs/cserialport/source/SerialPortInfo.cpp +++ b/libs/cserialport/source/SerialPortInfo.cpp @@ -1,4 +1,4 @@ -#include "CSerialPort/SerialPortInfo.h" +#include "CSerialPort/SerialPortInfo.h" #include "CSerialPort/SerialPort_global.h" #ifdef I_OS_WIN diff --git a/libs/cserialport/source/SerialPortInfoBase.cpp b/libs/cserialport/source/SerialPortInfoBase.cpp index 53febbd51..39836b9f3 100644 --- a/libs/cserialport/source/SerialPortInfoBase.cpp +++ b/libs/cserialport/source/SerialPortInfoBase.cpp @@ -1,4 +1,4 @@ -#include "CSerialPort/SerialPortInfoBase.h" +#include "CSerialPort/SerialPortInfoBase.h" CSerialPortInfoBase::CSerialPortInfoBase() {} diff --git a/libs/cserialport/source/SerialPortInfoUnixBase.cpp b/libs/cserialport/source/SerialPortInfoUnixBase.cpp index 79e12c323..4f0617dd9 100644 --- a/libs/cserialport/source/SerialPortInfoUnixBase.cpp +++ b/libs/cserialport/source/SerialPortInfoUnixBase.cpp @@ -1,4 +1,4 @@ -#include "CSerialPort/SerialPortInfoUnixBase.h" +#include "CSerialPort/SerialPortInfoUnixBase.h" #include "CSerialPort/SerialPort_global.h" #include "CSerialPort/iutils.hpp" diff --git a/libs/cserialport/source/SerialPortInfoWinBase.cpp b/libs/cserialport/source/SerialPortInfoWinBase.cpp index fbdc02037..b8f7b5d99 100644 --- a/libs/cserialport/source/SerialPortInfoWinBase.cpp +++ b/libs/cserialport/source/SerialPortInfoWinBase.cpp @@ -1,4 +1,4 @@ -#include +#include #include // GUID #include // SetupDiGetClassDevs Setup* #include // _T diff --git a/libs/cserialport/source/SerialPortUnixBase.cpp b/libs/cserialport/source/SerialPortUnixBase.cpp index 21f9d4857..7503f4af9 100644 --- a/libs/cserialport/source/SerialPortUnixBase.cpp +++ b/libs/cserialport/source/SerialPortUnixBase.cpp @@ -1,4 +1,4 @@ -#include // usleep +#include // usleep #include "CSerialPort/SerialPortUnixBase.h" #include "CSerialPort/SerialPortListener.h" diff --git a/libs/cserialport/source/SerialPortWinBase.cpp b/libs/cserialport/source/SerialPortWinBase.cpp index 49efe01e0..de7a95d84 100644 --- a/libs/cserialport/source/SerialPortWinBase.cpp +++ b/libs/cserialport/source/SerialPortWinBase.cpp @@ -1,4 +1,4 @@ -#include "CSerialPort/SerialPortWinBase.h" +#include "CSerialPort/SerialPortWinBase.h" #include "CSerialPort/SerialPortListener.h" #include "CSerialPort/iutils.hpp" #include "CSerialPort/ithread.hpp" diff --git a/libs/gnc_code/attitude_control_ert_rtw/include/attitude_control.h b/libs/gnc_code/attitude_control_ert_rtw/include/attitude_control.h index a643b2a8e..afd440bbc 100644 --- a/libs/gnc_code/attitude_control_ert_rtw/include/attitude_control.h +++ b/libs/gnc_code/attitude_control_ert_rtw/include/attitude_control.h @@ -1,115 +1,115 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: attitude_control.h - * - * Code generated for Simulink model 'attitude_control'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:47:01 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#ifndef RTW_HEADER_attitude_control_h_ -#define RTW_HEADER_attitude_control_h_ -#ifndef attitude_control_COMMON_INCLUDES_ -#define attitude_control_COMMON_INCLUDES_ -#include "rtwtypes.h" -#include "rtw_continuous.h" -#include "rtw_solver.h" -#endif /* attitude_control_COMMON_INCLUDES_ */ - -/* Macros for accessing real-time model data structure */ -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -/* Forward declaration for rtModel */ -typedef struct tag_RTM_attitude_control RT_MODEL_attitude_control; - -/* External inputs (root inport signals with default storage) */ -typedef struct { - real_T est_curr_ang_vel_body[3]; /* '/ang_vel_body' */ - real_T est_curr_quat_body[4]; /* '/curr_quat_body' */ - real_T com_quat_body[4]; /* '/com_quat_body' */ - real_T mag_field_body[3]; /* '/mag_field_body' */ -} attitude_control_model_ext_inputs_t; - -/* External outputs (root outports fed by signals with default storage) */ -typedef struct { - real_T comm_wheel_torque_body[3]; /* '/comm_wheel_torque_body' */ - real_T comm_mag_dipole_body[3]; /* '/comm_mag_dipole_body' */ -} attitude_control_model_ext_outputs_t; - -/* Real-time Model Data Structure */ -struct tag_RTM_attitude_control { - const char_T * volatile errorStatus; -}; - -/* External inputs (root inport signals with default storage) */ -extern attitude_control_model_ext_inputs_t attitude_control_model_ext_inputs; - -/* External outputs (root outports fed by signals with default storage) */ -extern attitude_control_model_ext_outputs_t attitude_control_model_ext_outputs; - -/* Model entry point functions */ -extern void attitude_control_initialize(void); -extern void attitude_control_step(void); - -/* Real-time Model object */ -extern RT_MODEL_attitude_control *const attitude_control_model_rt_object; - -/*- - * These blocks were eliminated from the model due to optimizations: - * - * Block '/reaction_wheel_torque' : Unused code path elimination - */ - -/*- - * The generated code includes comments that allow you to trace directly - * back to the appropriate location in the model. The basic format - * is /block_name, where system is the system number (uniquely - * assigned by Simulink) and block_name is the name of the block. - * - * Note that this particular code originates from a subsystem build, - * and has its own system numbers different from the parent model. - * Refer to the system hierarchy for this subsystem below, and use the - * MATLAB hilite_system command to trace the generated code back - * to the parent model. For example, - * - * hilite_system('cubesat_alg_dev_env_v2_main/attitude_control') - opens subsystem cubesat_alg_dev_env_v2_main/attitude_control - * hilite_system('cubesat_alg_dev_env_v2_main/attitude_control/Kp') - opens and selects block Kp - * - * Here is the system hierarchy for this model - * - * '' : 'cubesat_alg_dev_env_v2_main' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Inverse1' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Inverse1/Quaternion Conjugate' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Inverse1/Quaternion Norm' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q0' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q1' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q2' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q3' - */ -#endif /* RTW_HEADER_attitude_control_h_ */ - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: attitude_control.h + * + * Code generated for Simulink model 'attitude_control'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:47:01 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTW_HEADER_attitude_control_h_ +#define RTW_HEADER_attitude_control_h_ +#ifndef attitude_control_COMMON_INCLUDES_ +#define attitude_control_COMMON_INCLUDES_ +#include "rtwtypes.h" +#include "rtw_continuous.h" +#include "rtw_solver.h" +#endif /* attitude_control_COMMON_INCLUDES_ */ + +/* Macros for accessing real-time model data structure */ +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +/* Forward declaration for rtModel */ +typedef struct tag_RTM_attitude_control RT_MODEL_attitude_control; + +/* External inputs (root inport signals with default storage) */ +typedef struct { + real_T est_curr_ang_vel_body[3]; /* '/ang_vel_body' */ + real_T est_curr_quat_body[4]; /* '/curr_quat_body' */ + real_T com_quat_body[4]; /* '/com_quat_body' */ + real_T mag_field_body[3]; /* '/mag_field_body' */ +} attitude_control_model_ext_inputs_t; + +/* External outputs (root outports fed by signals with default storage) */ +typedef struct { + real_T comm_wheel_torque_body[3]; /* '/comm_wheel_torque_body' */ + real_T comm_mag_dipole_body[3]; /* '/comm_mag_dipole_body' */ +} attitude_control_model_ext_outputs_t; + +/* Real-time Model Data Structure */ +struct tag_RTM_attitude_control { + const char_T * volatile errorStatus; +}; + +/* External inputs (root inport signals with default storage) */ +extern attitude_control_model_ext_inputs_t attitude_control_model_ext_inputs; + +/* External outputs (root outports fed by signals with default storage) */ +extern attitude_control_model_ext_outputs_t attitude_control_model_ext_outputs; + +/* Model entry point functions */ +extern void attitude_control_initialize(void); +extern void attitude_control_step(void); + +/* Real-time Model object */ +extern RT_MODEL_attitude_control *const attitude_control_model_rt_object; + +/*- + * These blocks were eliminated from the model due to optimizations: + * + * Block '/reaction_wheel_torque' : Unused code path elimination + */ + +/*- + * The generated code includes comments that allow you to trace directly + * back to the appropriate location in the model. The basic format + * is /block_name, where system is the system number (uniquely + * assigned by Simulink) and block_name is the name of the block. + * + * Note that this particular code originates from a subsystem build, + * and has its own system numbers different from the parent model. + * Refer to the system hierarchy for this subsystem below, and use the + * MATLAB hilite_system command to trace the generated code back + * to the parent model. For example, + * + * hilite_system('cubesat_alg_dev_env_v2_main/attitude_control') - opens subsystem cubesat_alg_dev_env_v2_main/attitude_control + * hilite_system('cubesat_alg_dev_env_v2_main/attitude_control/Kp') - opens and selects block Kp + * + * Here is the system hierarchy for this model + * + * '' : 'cubesat_alg_dev_env_v2_main' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Inverse1' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Inverse1/Quaternion Conjugate' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Inverse1/Quaternion Norm' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q0' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q1' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q2' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_control/Quaternion Multiplication/q3' + */ +#endif /* RTW_HEADER_attitude_control_h_ */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/attitude_control_ert_rtw/include/rtwtypes.h b/libs/gnc_code/attitude_control_ert_rtw/include/rtwtypes.h index 525054864..29a3f2291 100644 --- a/libs/gnc_code/attitude_control_ert_rtw/include/rtwtypes.h +++ b/libs/gnc_code/attitude_control_ert_rtw/include/rtwtypes.h @@ -1,106 +1,106 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: rtwtypes.h - * - * Code generated for Simulink model 'attitude_control'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:47:01 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#ifndef RTWTYPES_H -#define RTWTYPES_H - -/* Logical type definitions */ -#if (!defined(__cplusplus)) -#ifndef false -#define false (0U) -#endif - -#ifndef true -#define true (1U) -#endif -#endif - -/*=======================================================================* - * Target hardware information - * Device type: ARM Compatible->ARM Cortex-R - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 long long: 64 - * native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef long long int64_T; -typedef unsigned long long uint64_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * - * real_T, time_T, ulong_T, ulonglong_T. * - *===========================================================================*/ -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef unsigned long long ulonglong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MAX_int64_T ((int64_T)(9223372036854775807LL)) -#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) -#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) - -/* Block D-Work pointer type */ -typedef void * pointer_T; - -#endif /* RTWTYPES_H */ - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: rtwtypes.h + * + * Code generated for Simulink model 'attitude_control'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:47:01 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/* Logical type definitions */ +#if (!defined(__cplusplus)) +#ifndef false +#define false (0U) +#endif + +#ifndef true +#define true (1U) +#endif +#endif + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex-R + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 long long: 64 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef long long int64_T; +typedef unsigned long long uint64_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * + * real_T, time_T, ulong_T, ulonglong_T. * + *===========================================================================*/ +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef unsigned long long ulonglong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MAX_int64_T ((int64_T)(9223372036854775807LL)) +#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) + +/* Block D-Work pointer type */ +typedef void * pointer_T; + +#endif /* RTWTYPES_H */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/attitude_control_ert_rtw/source/attitude_control.c b/libs/gnc_code/attitude_control_ert_rtw/source/attitude_control.c index 21e93ec29..107e71a6b 100644 --- a/libs/gnc_code/attitude_control_ert_rtw/source/attitude_control.c +++ b/libs/gnc_code/attitude_control_ert_rtw/source/attitude_control.c @@ -1,365 +1,365 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: attitude_control.c - * - * Code generated for Simulink model 'attitude_control'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:47:01 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#include "attitude_control.h" -#include "rtwtypes.h" -#include -#define NumBitsPerChar 8U - -/* External inputs (root inport signals with default storage) */ -attitude_control_model_ext_inputs_t attitude_control_model_ext_inputs; - -/* External outputs (root outports fed by signals with default storage) */ -attitude_control_model_ext_outputs_t attitude_control_model_ext_outputs; - -/* Real-time model */ -static RT_MODEL_attitude_control rtM_; -RT_MODEL_attitude_control *const attitude_control_model_rt_object= &rtM_; -static real_T rtGetNaN(void); -static real32_T rtGetNaNF(void); - -#define NOT_USING_NONFINITE_LITERALS 1 - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -static void rt_InitInfAndNaN(size_t realSize); -// static boolean_T rtIsInf(real_T value); -// static boolean_T rtIsInfF(real32_T value); -static boolean_T rtIsNaN(real_T value); -static boolean_T rtIsNaNF(real32_T value); -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; -static real_T rtGetInf(void); -static real32_T rtGetInfF(void); -static real_T rtGetMinusInf(void); -static real32_T rtGetMinusInfF(void); - -/* - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -static real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - } - - return nan; -} - -/* - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -static real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0.0F } }; - - nanF.wordL.wordLuint = 0xFFC00000U; - return nanF.wordL.wordLreal; -} - -/* - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -static void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -// /* Test if value is infinite */ -// static boolean_T rtIsInf(real_T value) -// { -// return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); -// } - -// /* Test if single-precision value is infinite */ -// static boolean_T rtIsInfF(real32_T value) -// { -// return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -// } - -/* Test if value is not a number */ -static boolean_T rtIsNaN(real_T value) -{ - boolean_T result = (boolean_T) 0; - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - if (bitsPerReal == 32U) { - result = rtIsNaNF((real32_T)value); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.fltVal = value; - result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == 0x7FF00000 && - ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || - (tmpVal.bitVal.words.wordL != 0) )); - } - - return result; -} - -/* Test if single-precision value is not a number */ -static boolean_T rtIsNaNF(real32_T value) -{ - IEEESingle tmp; - tmp.wordL.wordLreal = value; - return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && - (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); -} - -/* - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -static real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - } - - return inf; -} - -/* - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -static real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -static real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - } - - return minf; -} - -/* - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -static real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* Model step function */ -void attitude_control_step(void) -{ - real_T rtb_Product1; - real_T rtb_Product2; - real_T rtb_Product2_l; - real_T rtb_Sign; - real_T rtb_Sum_b; - - /* Sum: '/Sum' incorporates: - * Inport: '/com_quat_body' - * Product: '/Product' - * Product: '/Product1' - * Product: '/Product2' - * Product: '/Product3' - */ - rtb_Product2_l = ((attitude_control_model_ext_inputs.com_quat_body[0] * attitude_control_model_ext_inputs.com_quat_body[0] + - attitude_control_model_ext_inputs.com_quat_body[1] * attitude_control_model_ext_inputs.com_quat_body[1]) + - attitude_control_model_ext_inputs.com_quat_body[2] * attitude_control_model_ext_inputs.com_quat_body[2]) + - attitude_control_model_ext_inputs.com_quat_body[3] * attitude_control_model_ext_inputs.com_quat_body[3]; - - /* Product: '/Divide' incorporates: - * Inport: '/com_quat_body' - */ - rtb_Sum_b = attitude_control_model_ext_inputs.com_quat_body[0] / rtb_Product2_l; - - /* Product: '/Divide1' incorporates: - * Inport: '/com_quat_body' - * UnaryMinus: '/Unary Minus' - */ - rtb_Product1 = -attitude_control_model_ext_inputs.com_quat_body[1] / rtb_Product2_l; - - /* Product: '/Divide2' incorporates: - * Inport: '/com_quat_body' - * UnaryMinus: '/Unary Minus1' - */ - rtb_Product2 = -attitude_control_model_ext_inputs.com_quat_body[2] / rtb_Product2_l; - - /* Product: '/Divide3' incorporates: - * Inport: '/com_quat_body' - * UnaryMinus: '/Unary Minus2' - */ - rtb_Product2_l = -attitude_control_model_ext_inputs.com_quat_body[3] / rtb_Product2_l; - - /* Sum: '/Sum' incorporates: - * Inport: '/curr_quat_body' - * Product: '/Product' - * Product: '/Product1' - * Product: '/Product2' - * Product: '/Product3' - */ - rtb_Sign = ((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[0] - rtb_Product1 * - attitude_control_model_ext_inputs.est_curr_quat_body[1]) - rtb_Product2 * - attitude_control_model_ext_inputs.est_curr_quat_body[2]) - rtb_Product2_l * - attitude_control_model_ext_inputs.est_curr_quat_body[3]; - - /* Signum: '/Sign' */ - if (rtIsNaN(rtb_Sign)) { - rtb_Sign = (rtNaN); - } else if (rtb_Sign < 0.0) { - rtb_Sign = -1.0; - } else { - rtb_Sign = (rtb_Sign > 0.0); - } - - /* End of Signum: '/Sign' */ - - /* Outport: '/comm_wheel_torque_body' incorporates: - * Gain: '/k_d' - * Gain: '/k_p' - * Inport: '/ang_vel_body' - * Inport: '/curr_quat_body' - * Product: '/Product' - * Product: '/Product' - * Product: '/Product1' - * Product: '/Product2' - * Product: '/Product3' - * Product: '/Product' - * Product: '/Product1' - * Product: '/Product2' - * Product: '/Product3' - * Product: '/Product' - * Product: '/Product1' - * Product: '/Product2' - * Product: '/Product3' - * Sum: '/Sum6' - * Sum: '/Sum' - * Sum: '/Sum' - * Sum: '/Sum' - */ - attitude_control_model_ext_outputs.comm_wheel_torque_body[0] = (((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[1] + - rtb_Product1 * attitude_control_model_ext_inputs.est_curr_quat_body[0]) + rtb_Product2 * - attitude_control_model_ext_inputs.est_curr_quat_body[3]) - rtb_Product2_l * attitude_control_model_ext_inputs.est_curr_quat_body[2]) * - 0.2 * rtb_Sign + 0.1 * attitude_control_model_ext_inputs.est_curr_ang_vel_body[0]; - attitude_control_model_ext_outputs.comm_wheel_torque_body[1] = (((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[2] - - rtb_Product1 * attitude_control_model_ext_inputs.est_curr_quat_body[3]) + rtb_Product2 * - attitude_control_model_ext_inputs.est_curr_quat_body[0]) + rtb_Product2_l * attitude_control_model_ext_inputs.est_curr_quat_body[1]) * - 0.2 * rtb_Sign + 0.1 * attitude_control_model_ext_inputs.est_curr_ang_vel_body[1]; - attitude_control_model_ext_outputs.comm_wheel_torque_body[2] = (((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[3] + - rtb_Product1 * attitude_control_model_ext_inputs.est_curr_quat_body[2]) - rtb_Product2 * - attitude_control_model_ext_inputs.est_curr_quat_body[1]) + rtb_Product2_l * attitude_control_model_ext_inputs.est_curr_quat_body[0]) * - 0.2 * rtb_Sign + 0.1 * attitude_control_model_ext_inputs.est_curr_ang_vel_body[2]; -} - -/* Model initialize function */ -void attitude_control_initialize(void) -{ - /* Registration code */ - - /* initialize non-finites */ - rt_InitInfAndNaN(sizeof(real_T)); -} - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: attitude_control.c + * + * Code generated for Simulink model 'attitude_control'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:47:01 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#include "attitude_control.h" +#include "rtwtypes.h" +#include +#define NumBitsPerChar 8U + +/* External inputs (root inport signals with default storage) */ +attitude_control_model_ext_inputs_t attitude_control_model_ext_inputs; + +/* External outputs (root outports fed by signals with default storage) */ +attitude_control_model_ext_outputs_t attitude_control_model_ext_outputs; + +/* Real-time model */ +static RT_MODEL_attitude_control rtM_; +RT_MODEL_attitude_control *const attitude_control_model_rt_object= &rtM_; +static real_T rtGetNaN(void); +static real32_T rtGetNaNF(void); + +#define NOT_USING_NONFINITE_LITERALS 1 + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +static void rt_InitInfAndNaN(size_t realSize); +// static boolean_T rtIsInf(real_T value); +// static boolean_T rtIsInfF(real32_T value); +static boolean_T rtIsNaN(real_T value); +static boolean_T rtIsNaNF(real32_T value); +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; +static real_T rtGetInf(void); +static real32_T rtGetInfF(void); +static real_T rtGetMinusInf(void); +static real32_T rtGetMinusInfF(void); + +/* + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +static real_T rtGetNaN(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + } + + return nan; +} + +/* + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +static real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0.0F } }; + + nanF.wordL.wordLuint = 0xFFC00000U; + return nanF.wordL.wordLreal; +} + +/* + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +static void rt_InitInfAndNaN(size_t realSize) +{ + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +// /* Test if value is infinite */ +// static boolean_T rtIsInf(real_T value) +// { +// return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); +// } + +// /* Test if single-precision value is infinite */ +// static boolean_T rtIsInfF(real32_T value) +// { +// return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +// } + +/* Test if value is not a number */ +static boolean_T rtIsNaN(real_T value) +{ + boolean_T result = (boolean_T) 0; + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + if (bitsPerReal == 32U) { + result = rtIsNaNF((real32_T)value); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.fltVal = value; + result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == 0x7FF00000 && + ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || + (tmpVal.bitVal.words.wordL != 0) )); + } + + return result; +} + +/* Test if single-precision value is not a number */ +static boolean_T rtIsNaNF(real32_T value) +{ + IEEESingle tmp; + tmp.wordL.wordLreal = value; + return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && + (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); +} + +/* + * Initialize rtInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +static real_T rtGetInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + } + + return inf; +} + +/* + * Initialize rtInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +static real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* + * Initialize rtMinusInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +static real_T rtGetMinusInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + } + + return minf; +} + +/* + * Initialize rtMinusInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +static real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* Model step function */ +void attitude_control_step(void) +{ + real_T rtb_Product1; + real_T rtb_Product2; + real_T rtb_Product2_l; + real_T rtb_Sign; + real_T rtb_Sum_b; + + /* Sum: '/Sum' incorporates: + * Inport: '/com_quat_body' + * Product: '/Product' + * Product: '/Product1' + * Product: '/Product2' + * Product: '/Product3' + */ + rtb_Product2_l = ((attitude_control_model_ext_inputs.com_quat_body[0] * attitude_control_model_ext_inputs.com_quat_body[0] + + attitude_control_model_ext_inputs.com_quat_body[1] * attitude_control_model_ext_inputs.com_quat_body[1]) + + attitude_control_model_ext_inputs.com_quat_body[2] * attitude_control_model_ext_inputs.com_quat_body[2]) + + attitude_control_model_ext_inputs.com_quat_body[3] * attitude_control_model_ext_inputs.com_quat_body[3]; + + /* Product: '/Divide' incorporates: + * Inport: '/com_quat_body' + */ + rtb_Sum_b = attitude_control_model_ext_inputs.com_quat_body[0] / rtb_Product2_l; + + /* Product: '/Divide1' incorporates: + * Inport: '/com_quat_body' + * UnaryMinus: '/Unary Minus' + */ + rtb_Product1 = -attitude_control_model_ext_inputs.com_quat_body[1] / rtb_Product2_l; + + /* Product: '/Divide2' incorporates: + * Inport: '/com_quat_body' + * UnaryMinus: '/Unary Minus1' + */ + rtb_Product2 = -attitude_control_model_ext_inputs.com_quat_body[2] / rtb_Product2_l; + + /* Product: '/Divide3' incorporates: + * Inport: '/com_quat_body' + * UnaryMinus: '/Unary Minus2' + */ + rtb_Product2_l = -attitude_control_model_ext_inputs.com_quat_body[3] / rtb_Product2_l; + + /* Sum: '/Sum' incorporates: + * Inport: '/curr_quat_body' + * Product: '/Product' + * Product: '/Product1' + * Product: '/Product2' + * Product: '/Product3' + */ + rtb_Sign = ((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[0] - rtb_Product1 * + attitude_control_model_ext_inputs.est_curr_quat_body[1]) - rtb_Product2 * + attitude_control_model_ext_inputs.est_curr_quat_body[2]) - rtb_Product2_l * + attitude_control_model_ext_inputs.est_curr_quat_body[3]; + + /* Signum: '/Sign' */ + if (rtIsNaN(rtb_Sign)) { + rtb_Sign = (rtNaN); + } else if (rtb_Sign < 0.0) { + rtb_Sign = -1.0; + } else { + rtb_Sign = (rtb_Sign > 0.0); + } + + /* End of Signum: '/Sign' */ + + /* Outport: '/comm_wheel_torque_body' incorporates: + * Gain: '/k_d' + * Gain: '/k_p' + * Inport: '/ang_vel_body' + * Inport: '/curr_quat_body' + * Product: '/Product' + * Product: '/Product' + * Product: '/Product1' + * Product: '/Product2' + * Product: '/Product3' + * Product: '/Product' + * Product: '/Product1' + * Product: '/Product2' + * Product: '/Product3' + * Product: '/Product' + * Product: '/Product1' + * Product: '/Product2' + * Product: '/Product3' + * Sum: '/Sum6' + * Sum: '/Sum' + * Sum: '/Sum' + * Sum: '/Sum' + */ + attitude_control_model_ext_outputs.comm_wheel_torque_body[0] = (((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[1] + + rtb_Product1 * attitude_control_model_ext_inputs.est_curr_quat_body[0]) + rtb_Product2 * + attitude_control_model_ext_inputs.est_curr_quat_body[3]) - rtb_Product2_l * attitude_control_model_ext_inputs.est_curr_quat_body[2]) * + 0.2 * rtb_Sign + 0.1 * attitude_control_model_ext_inputs.est_curr_ang_vel_body[0]; + attitude_control_model_ext_outputs.comm_wheel_torque_body[1] = (((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[2] - + rtb_Product1 * attitude_control_model_ext_inputs.est_curr_quat_body[3]) + rtb_Product2 * + attitude_control_model_ext_inputs.est_curr_quat_body[0]) + rtb_Product2_l * attitude_control_model_ext_inputs.est_curr_quat_body[1]) * + 0.2 * rtb_Sign + 0.1 * attitude_control_model_ext_inputs.est_curr_ang_vel_body[1]; + attitude_control_model_ext_outputs.comm_wheel_torque_body[2] = (((rtb_Sum_b * attitude_control_model_ext_inputs.est_curr_quat_body[3] + + rtb_Product1 * attitude_control_model_ext_inputs.est_curr_quat_body[2]) - rtb_Product2 * + attitude_control_model_ext_inputs.est_curr_quat_body[1]) + rtb_Product2_l * attitude_control_model_ext_inputs.est_curr_quat_body[0]) * + 0.2 * rtb_Sign + 0.1 * attitude_control_model_ext_inputs.est_curr_ang_vel_body[2]; +} + +/* Model initialize function */ +void attitude_control_initialize(void) +{ + /* Registration code */ + + /* initialize non-finites */ + rt_InitInfAndNaN(sizeof(real_T)); +} + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/attitude_determination_and_vehi.h b/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/attitude_determination_and_vehi.h index 817d3d6f1..14432f7c9 100644 --- a/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/attitude_determination_and_vehi.h +++ b/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/attitude_determination_and_vehi.h @@ -1,139 +1,139 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: attitude_determination_and_vehi.h - * - * Code generated for Simulink model 'attitude_determination_and_vehi'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:50:14 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#ifndef RTW_HEADER_attitude_determination_and_vehi_h_ -#define RTW_HEADER_attitude_determination_and_vehi_h_ -#ifndef attitude_determination_and_vehi_COMMON_INCLUDES_ -#define attitude_determination_and_vehi_COMMON_INCLUDES_ -#include "rtwtypes.h" -#include "rtw_continuous.h" -#include "rtw_solver.h" -#endif /* attitude_determination_and_vehi_COMMON_INCLUDES_ */ - -/* Macros for accessing real-time model data structure */ -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -/* Forward declaration for rtModel */ -typedef struct tag_RTM_attitude_determination RT_MODEL_attitude_determination; - -/* Block signals and states (default storage) for system '' */ -typedef struct { - real_T P_o[36]; /* '/Data Store Memory' */ - real_T q_n2m[4]; /* '/Data Store Memory1' */ - real_T beta[3]; /* '/Data Store Memory2' */ -} DW; - -/* Constant parameters (default storage) */ -typedef struct { - /* Expression: diag([1*ones(1,3), 1*ones(1,3)]) - * Referenced by: '/Data Store Memory' - */ - real_T DataStoreMemory_InitialValue[36]; -} ConstP; - -/* External inputs (root inport signals with default storage) */ -typedef struct { - real_T earth_mag_field_ref[3]; /* '/earth_mag_field_ref' */ - real_T sat_to_sun_unit_ref[3]; /* '/sat_to_sun_unit_ref' */ - real_T omega[3]; /* '/omega' */ - real_T mes_ss[3]; /* '/mes_ss' */ - real_T r_sat_com_ax1[3]; /* '/r_sat_com' */ - real_T mes_mag[3]; /* '/mes_mag' */ - real_T ref_aam[3]; /* '/ref_aam' */ - real_T mes_aam[3]; /* '/mes_aam' */ - real_T steve_mes[3]; /* '/steve_mes' */ -} attitude_determination_model_ext_inputs_t; - -/* External outputs (root outports fed by signals with default storage) */ -typedef struct { - real_T meas_ang_vel_body[3]; /* '/meas_ang_vel_body' */ - real_T meas_quat_body[4]; /* '/meas_quat_body' */ -} attitude_determination_model_ext_outputs_t; - -/* Real-time Model Data Structure */ -struct tag_RTM_attitude_determination { - const char_T * volatile errorStatus; -}; - -/* Block signals and states (default storage) */ -extern DW rtDW; - -/* External inputs (root inport signals with default storage) */ -extern attitude_determination_model_ext_inputs_t attitude_determination_model_ext_inputs; - -/* External outputs (root outports fed by signals with default storage) */ -extern attitude_determination_model_ext_outputs_t attitude_determination_model_ext_outputs; - -/* Constant parameters (default storage) */ -extern const ConstP rtConstP; - -/* Model entry point functions */ -extern void attitude_determination_and_vehi_initialize(void); -extern void attitude_determination_and_vehi_step(void); - -/* Real-time Model object */ -extern RT_MODEL_attitude_determination *const attitude_determinataion_model_rt_object; - -/*- - * These blocks were eliminated from the model due to optimizations: - * - * Block '/Estimated Angular Velocity' : Unused code path elimination - * Block '/Estimated Attitude' : Unused code path elimination - * Block '/MEKF Covariance Matrix' : Unused code path elimination - * Block '/Reshape' : Reshape block reduction - * Block '/Reshape1' : Reshape block reduction - * Block '/Reshape2' : Reshape block reduction - */ - -/*- - * The generated code includes comments that allow you to trace directly - * back to the appropriate location in the model. The basic format - * is /block_name, where system is the system number (uniquely - * assigned by Simulink) and block_name is the name of the block. - * - * Note that this particular code originates from a subsystem build, - * and has its own system numbers different from the parent model. - * Refer to the system hierarchy for this subsystem below, and use the - * MATLAB hilite_system command to trace the generated code back - * to the parent model. For example, - * - * hilite_system('cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est') - opens subsystem cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est - * hilite_system('cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est/Kp') - opens and selects block Kp - * - * Here is the system hierarchy for this model - * - * '' : 'cubesat_alg_dev_env_v2_main' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est' - * '' : 'cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est/MEKF' - */ -#endif /* RTW_HEADER_attitude_determination_and_vehi_h_ */ - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: attitude_determination_and_vehi.h + * + * Code generated for Simulink model 'attitude_determination_and_vehi'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:50:14 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTW_HEADER_attitude_determination_and_vehi_h_ +#define RTW_HEADER_attitude_determination_and_vehi_h_ +#ifndef attitude_determination_and_vehi_COMMON_INCLUDES_ +#define attitude_determination_and_vehi_COMMON_INCLUDES_ +#include "rtwtypes.h" +#include "rtw_continuous.h" +#include "rtw_solver.h" +#endif /* attitude_determination_and_vehi_COMMON_INCLUDES_ */ + +/* Macros for accessing real-time model data structure */ +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +/* Forward declaration for rtModel */ +typedef struct tag_RTM_attitude_determination RT_MODEL_attitude_determination; + +/* Block signals and states (default storage) for system '' */ +typedef struct { + real_T P_o[36]; /* '/Data Store Memory' */ + real_T q_n2m[4]; /* '/Data Store Memory1' */ + real_T beta[3]; /* '/Data Store Memory2' */ +} DW; + +/* Constant parameters (default storage) */ +typedef struct { + /* Expression: diag([1*ones(1,3), 1*ones(1,3)]) + * Referenced by: '/Data Store Memory' + */ + real_T DataStoreMemory_InitialValue[36]; +} ConstP; + +/* External inputs (root inport signals with default storage) */ +typedef struct { + real_T earth_mag_field_ref[3]; /* '/earth_mag_field_ref' */ + real_T sat_to_sun_unit_ref[3]; /* '/sat_to_sun_unit_ref' */ + real_T omega[3]; /* '/omega' */ + real_T mes_ss[3]; /* '/mes_ss' */ + real_T r_sat_com_ax1[3]; /* '/r_sat_com' */ + real_T mes_mag[3]; /* '/mes_mag' */ + real_T ref_aam[3]; /* '/ref_aam' */ + real_T mes_aam[3]; /* '/mes_aam' */ + real_T steve_mes[3]; /* '/steve_mes' */ +} attitude_determination_model_ext_inputs_t; + +/* External outputs (root outports fed by signals with default storage) */ +typedef struct { + real_T meas_ang_vel_body[3]; /* '/meas_ang_vel_body' */ + real_T meas_quat_body[4]; /* '/meas_quat_body' */ +} attitude_determination_model_ext_outputs_t; + +/* Real-time Model Data Structure */ +struct tag_RTM_attitude_determination { + const char_T * volatile errorStatus; +}; + +/* Block signals and states (default storage) */ +extern DW rtDW; + +/* External inputs (root inport signals with default storage) */ +extern attitude_determination_model_ext_inputs_t attitude_determination_model_ext_inputs; + +/* External outputs (root outports fed by signals with default storage) */ +extern attitude_determination_model_ext_outputs_t attitude_determination_model_ext_outputs; + +/* Constant parameters (default storage) */ +extern const ConstP rtConstP; + +/* Model entry point functions */ +extern void attitude_determination_and_vehi_initialize(void); +extern void attitude_determination_and_vehi_step(void); + +/* Real-time Model object */ +extern RT_MODEL_attitude_determination *const attitude_determinataion_model_rt_object; + +/*- + * These blocks were eliminated from the model due to optimizations: + * + * Block '/Estimated Angular Velocity' : Unused code path elimination + * Block '/Estimated Attitude' : Unused code path elimination + * Block '/MEKF Covariance Matrix' : Unused code path elimination + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape1' : Reshape block reduction + * Block '/Reshape2' : Reshape block reduction + */ + +/*- + * The generated code includes comments that allow you to trace directly + * back to the appropriate location in the model. The basic format + * is /block_name, where system is the system number (uniquely + * assigned by Simulink) and block_name is the name of the block. + * + * Note that this particular code originates from a subsystem build, + * and has its own system numbers different from the parent model. + * Refer to the system hierarchy for this subsystem below, and use the + * MATLAB hilite_system command to trace the generated code back + * to the parent model. For example, + * + * hilite_system('cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est') - opens subsystem cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est + * hilite_system('cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est/Kp') - opens and selects block Kp + * + * Here is the system hierarchy for this model + * + * '' : 'cubesat_alg_dev_env_v2_main' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est' + * '' : 'cubesat_alg_dev_env_v2_main/attitude_determination_and_vehicle_est/MEKF' + */ +#endif /* RTW_HEADER_attitude_determination_and_vehi_h_ */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/rtwtypes.h b/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/rtwtypes.h index be6ef008d..0c9004b5f 100644 --- a/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/rtwtypes.h +++ b/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/include/rtwtypes.h @@ -1,106 +1,106 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: rtwtypes.h - * - * Code generated for Simulink model 'attitude_determination_and_vehi'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:50:14 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#ifndef RTWTYPES_H -#define RTWTYPES_H - -/* Logical type definitions */ -#if (!defined(__cplusplus)) -#ifndef false -#define false (0U) -#endif - -#ifndef true -#define true (1U) -#endif -#endif - -/*=======================================================================* - * Target hardware information - * Device type: ARM Compatible->ARM Cortex-R - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 long long: 64 - * native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef long long int64_T; -typedef unsigned long long uint64_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * - * real_T, time_T, ulong_T, ulonglong_T. * - *===========================================================================*/ -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef unsigned long long ulonglong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MAX_int64_T ((int64_T)(9223372036854775807LL)) -#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) -#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) - -/* Block D-Work pointer type */ -typedef void * pointer_T; - -#endif /* RTWTYPES_H */ - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: rtwtypes.h + * + * Code generated for Simulink model 'attitude_determination_and_vehi'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:50:14 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/* Logical type definitions */ +#if (!defined(__cplusplus)) +#ifndef false +#define false (0U) +#endif + +#ifndef true +#define true (1U) +#endif +#endif + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex-R + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 long long: 64 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef long long int64_T; +typedef unsigned long long uint64_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * + * real_T, time_T, ulong_T, ulonglong_T. * + *===========================================================================*/ +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef unsigned long long ulonglong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MAX_int64_T ((int64_T)(9223372036854775807LL)) +#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) + +/* Block D-Work pointer type */ +typedef void * pointer_T; + +#endif /* RTWTYPES_H */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/source/attitude_determination_and_vehi.c b/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/source/attitude_determination_and_vehi.c index d256d7629..63cc76d11 100644 --- a/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/source/attitude_determination_and_vehi.c +++ b/libs/gnc_code/attitude_determination_and_vehi_ert_rtw/source/attitude_determination_and_vehi.c @@ -1,627 +1,627 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: attitude_determination_and_vehi.c - * - * Code generated for Simulink model 'attitude_determination_and_vehi'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:50:14 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#include "attitude_determination_and_vehi.h" -#include "rtwtypes.h" -#include -#include - -/* Block signals and states (default storage) */ -DW rtDW; - -/* Constant parameters (default storage) */ -const ConstP rtConstP; - -/* External inputs (root inport signals with default storage) */ -attitude_determination_model_ext_inputs_t attitude_determination_model_ext_inputs; - -/* External outputs (root outports fed by signals with default storage) */ -attitude_determination_model_ext_outputs_t attitude_determination_model_ext_outputs; - -/* Real-time model */ -static RT_MODEL_attitude_determination rtM_; -RT_MODEL_attitude_determination *const attitude_determinataion_model_rt_object = &rtM_; - -/* Forward declaration for local functions */ -static void quatrotate(const real_T q[4], real_T v[3]); -static void mrdiv(const real_T A[18], const real_T B_0[9], real_T Y[18]); -static void quatmultiply(const real_T q[4], const real_T r[4], real_T qout[4]); -static real_T norm(const real_T x[4]); - -/* Function for MATLAB Function: '/MEKF' */ -static void quatrotate(const real_T q[4], real_T v[3]) -{ - real_T y[9]; - real_T y_0[3]; - real_T y_tmp; - real_T y_tmp_0; - real_T y_tmp_1; - real_T y_tmp_2; - real_T y_tmp_3; - real_T y_tmp_4; - int32_T i; - y_tmp_1 = q[3] * q[3] * 2.0; - y_tmp_4 = (1.0 - q[2] * q[2] * 2.0) - y_tmp_1; - y[0] = y_tmp_4; - y_tmp = q[1] * q[2]; - y_tmp_0 = q[0] * q[3]; - y[3] = (y_tmp + y_tmp_0) * 2.0; - y_tmp_2 = q[1] * q[3]; - y_tmp_3 = q[0] * q[2]; - y[6] = (y_tmp_2 - y_tmp_3) * 2.0; - y[1] = (y_tmp - y_tmp_0) * 2.0; - y[4] = (1.0 - q[1] * q[1] * 2.0) - y_tmp_1; - y_tmp_1 = q[2] * q[3]; - y_tmp = q[0] * q[1]; - y[7] = (y_tmp_1 + y_tmp) * 2.0; - y[2] = (y_tmp_2 + y_tmp_3) * 2.0; - y[5] = (y_tmp_1 - y_tmp) * 2.0; - y[8] = y_tmp_4; - y_tmp_1 = v[1]; - y_tmp_4 = v[0]; - y_tmp = v[2]; - for (i = 0; i < 3; i++) { - y_0[i] = (y[i + 3] * y_tmp_1 + y[i] * y_tmp_4) + y[i + 6] * y_tmp; - } - - v[0] = y_0[0]; - v[1] = y_0[1]; - v[2] = y_0[2]; -} - -/* Function for MATLAB Function: '/MEKF' */ -static void mrdiv(const real_T A[18], const real_T B_0[9], real_T Y[18]) -{ - real_T b_A[9]; - real_T a21; - real_T maxval; - int32_T r1; - int32_T r2; - int32_T r3; - int32_T rtemp; - memcpy(&b_A[0], &B_0[0], 9U * sizeof(real_T)); - r1 = 0; - r2 = 1; - r3 = 2; - maxval = fabs(B_0[0]); - a21 = fabs(B_0[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if (fabs(B_0[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - b_A[r2] = B_0[r2] / B_0[r1]; - b_A[r3] /= b_A[r1]; - b_A[r2 + 3] -= b_A[r1 + 3] * b_A[r2]; - b_A[r3 + 3] -= b_A[r1 + 3] * b_A[r3]; - b_A[r2 + 6] -= b_A[r1 + 6] * b_A[r2]; - b_A[r3 + 6] -= b_A[r1 + 6] * b_A[r3]; - if (fabs(b_A[r3 + 3]) > fabs(b_A[r2 + 3])) { - rtemp = r2; - r2 = r3; - r3 = rtemp; - } - - b_A[r3 + 3] /= b_A[r2 + 3]; - b_A[r3 + 6] -= b_A[r3 + 3] * b_A[r2 + 6]; - for (rtemp = 0; rtemp < 6; rtemp++) { - int32_T Y_tmp; - int32_T Y_tmp_0; - int32_T Y_tmp_1; - Y_tmp = 6 * r1 + rtemp; - Y[Y_tmp] = A[rtemp] / b_A[r1]; - Y_tmp_0 = 6 * r2 + rtemp; - Y[Y_tmp_0] = A[rtemp + 6] - b_A[r1 + 3] * Y[Y_tmp]; - Y_tmp_1 = 6 * r3 + rtemp; - Y[Y_tmp_1] = A[rtemp + 12] - b_A[r1 + 6] * Y[Y_tmp]; - Y[Y_tmp_0] /= b_A[r2 + 3]; - Y[Y_tmp_1] -= b_A[r2 + 6] * Y[Y_tmp_0]; - Y[Y_tmp_1] /= b_A[r3 + 6]; - Y[Y_tmp_0] -= b_A[r3 + 3] * Y[Y_tmp_1]; - Y[Y_tmp] -= Y[Y_tmp_1] * b_A[r3]; - Y[Y_tmp] -= Y[Y_tmp_0] * b_A[r2]; - } -} - -/* Function for MATLAB Function: '/MEKF' */ -static void quatmultiply(const real_T q[4], const real_T r[4], real_T qout[4]) -{ - qout[0] = ((q[0] * r[0] - q[1] * r[1]) - q[2] * r[2]) - q[3] * r[3]; - qout[1] = (q[0] * r[1] + r[0] * q[1]) + (q[2] * r[3] - r[2] * q[3]); - qout[2] = (q[0] * r[2] + r[0] * q[2]) + (r[1] * q[3] - q[1] * r[3]); - qout[3] = (q[0] * r[3] + r[0] * q[3]) + (q[1] * r[2] - r[1] * q[2]); -} - -/* Function for MATLAB Function: '/MEKF' */ -static real_T norm(const real_T x[4]) -{ - real_T absxk; - real_T scale; - real_T t; - real_T y; - scale = 3.3121686421112381E-170; - absxk = fabs(x[0]); - if (absxk > 3.3121686421112381E-170) { - y = 1.0; - scale = absxk; - } else { - t = absxk / 3.3121686421112381E-170; - y = t * t; - } - - absxk = fabs(x[1]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - absxk = fabs(x[2]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - absxk = fabs(x[3]); - if (absxk > scale) { - t = scale / absxk; - y = y * t * t + 1.0; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - - return scale * sqrt(y); -} - -/* Model step function */ -void attitude_determination_and_vehi_step(void) -{ - real_T F[36]; - real_T F_0[36]; - real_T F_1[36]; - real_T P_0[36]; - real_T P_o_tmp_0[36]; - real_T a[36]; - real_T H[18]; - real_T H_0[18]; - real_T K[18]; - real_T P_1[18]; - real_T H_1[9]; - real_T delta_x[6]; - real_T q_n2m[4]; - real_T tmp[4]; - real_T tmp_0[4]; - real_T rtb_hat_omega[3]; - real_T tmp_1[3]; - real_T F_2; - real_T P_2; - real_T rtb_q_n2m_idx_0; - real_T rtb_q_n2m_idx_1; - real_T rtb_q_n2m_idx_2; - real_T rtb_q_n2m_idx_3; - int32_T F_tmp; - int32_T H_tmp; - int32_T a_tmp; - int32_T k; - int8_T P_o_tmp[36]; - int8_T K_tmp[9]; - static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - static const int8_T e_a[36] = { -1, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, -1, - 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; - - /* MATLAB Function: '/MEKF' incorporates: - * Constant: '/Kalman Filter Timestep' - * DataStoreRead: '/Data Store Read1' - * DataStoreRead: '/Data Store Read2' - * DataStoreWrite: '/Data Store Write' - * Inport: '/earth_mag_field_ref' - * Inport: '/mes_mag' - * Inport: '/mes_ss' - * Inport: '/omega' - * Inport: '/sat_to_sun_unit_ref' - * Math: '/Transpose1' - */ - rtb_hat_omega[0] = attitude_determination_model_ext_inputs.sat_to_sun_unit_ref[0]; - rtb_hat_omega[1] = attitude_determination_model_ext_inputs.sat_to_sun_unit_ref[1]; - rtb_hat_omega[2] = attitude_determination_model_ext_inputs.sat_to_sun_unit_ref[2]; - quatrotate(rtDW.q_n2m, rtb_hat_omega); - H[0] = 0.0; - H[3] = -rtb_hat_omega[2]; - H[6] = rtb_hat_omega[1]; - H[9] = 0.0; - H[12] = 0.0; - H[15] = 0.0; - H[1] = rtb_hat_omega[2]; - H[4] = 0.0; - H[7] = -rtb_hat_omega[0]; - H[10] = 0.0; - H[13] = 0.0; - H[16] = 0.0; - H[2] = -rtb_hat_omega[1]; - H[5] = rtb_hat_omega[0]; - H[8] = 0.0; - H[11] = 0.0; - H[14] = 0.0; - H[17] = 0.0; - for (k = 0; k < 3; k++) { - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - K[a_tmp + 6 * k] = H[3 * a_tmp + k]; - } - } - - for (k = 0; k < 9; k++) { - K_tmp[k] = b[k]; - } - - for (k = 0; k < 6; k++) { - for (a_tmp = 0; a_tmp < 3; a_tmp++) { - rtb_q_n2m_idx_0 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - rtb_q_n2m_idx_0 += H[3 * H_tmp + a_tmp] * rtDW.P_o[6 * k + H_tmp]; - } - - H_0[a_tmp + 3 * k] = rtb_q_n2m_idx_0; - } - } - - for (k = 0; k < 3; k++) { - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += rtDW.P_o[6 * H_tmp + a_tmp] * K[6 * k + H_tmp]; - } - - P_1[a_tmp + 6 * k] = P_2; - } - - for (a_tmp = 0; a_tmp < 3; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += H_0[3 * H_tmp + k] * K[6 * a_tmp + H_tmp]; - } - - H_tmp = 3 * a_tmp + k; - H_1[H_tmp] = (real_T)K_tmp[H_tmp] * 0.0625 + P_2; - } - } - - mrdiv(P_1, H_1, K); - for (k = 0; k < 3; k++) { - P_2 = 0.0; - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 += H[3 * a_tmp + k]; - } - - tmp_1[k] = (attitude_determination_model_ext_inputs.mes_ss[k] - rtb_hat_omega[k]) - P_2; - } - - P_2 = tmp_1[1]; - rtb_q_n2m_idx_0 = tmp_1[0]; - rtb_q_n2m_idx_1 = tmp_1[2]; - for (k = 0; k < 6; k++) { - delta_x[k] = (K[k + 6] * P_2 + K[k] * rtb_q_n2m_idx_0) + K[k + 12] * - rtb_q_n2m_idx_1; - } - - memset(&F[0], 0, 36U * sizeof(real_T)); - for (k = 0; k < 6; k++) { - F[k + 6 * k] = 1.0; - } - - for (k = 0; k < 6; k++) { - rtb_q_n2m_idx_0 = K[k + 6]; - rtb_q_n2m_idx_1 = K[k]; - rtb_q_n2m_idx_2 = K[k + 12]; - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - F_tmp = 6 * a_tmp + k; - F_0[F_tmp] = F[F_tmp] - ((H[3 * a_tmp + 1] * rtb_q_n2m_idx_0 + H[3 * a_tmp] - * rtb_q_n2m_idx_1) + H[3 * a_tmp + 2] * rtb_q_n2m_idx_2); - } - - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += F_0[6 * H_tmp + k] * rtDW.P_o[6 * a_tmp + H_tmp]; - } - - P_0[k + 6 * a_tmp] = P_2; - } - } - - rtb_hat_omega[0] = attitude_determination_model_ext_inputs.earth_mag_field_ref[0]; - rtb_hat_omega[1] = attitude_determination_model_ext_inputs.earth_mag_field_ref[1]; - rtb_hat_omega[2] = attitude_determination_model_ext_inputs.earth_mag_field_ref[2]; - quatrotate(rtDW.q_n2m, rtb_hat_omega); - H[0] = 0.0; - H[3] = -rtb_hat_omega[2]; - H[6] = rtb_hat_omega[1]; - H[9] = 0.0; - H[12] = 0.0; - H[15] = 0.0; - H[1] = rtb_hat_omega[2]; - H[4] = 0.0; - H[7] = -rtb_hat_omega[0]; - H[10] = 0.0; - H[13] = 0.0; - H[16] = 0.0; - H[2] = -rtb_hat_omega[1]; - H[5] = rtb_hat_omega[0]; - H[8] = 0.0; - H[11] = 0.0; - H[14] = 0.0; - H[17] = 0.0; - for (k = 0; k < 3; k++) { - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - F_tmp = 3 * a_tmp + k; - K[a_tmp + 6 * k] = H[F_tmp]; - rtb_q_n2m_idx_0 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - rtb_q_n2m_idx_0 += H[3 * H_tmp + k] * P_0[6 * a_tmp + H_tmp]; - } - - H_0[F_tmp] = rtb_q_n2m_idx_0; - } - } - - for (k = 0; k < 3; k++) { - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += P_0[6 * H_tmp + a_tmp] * K[6 * k + H_tmp]; - } - - P_1[a_tmp + 6 * k] = P_2; - } - - for (a_tmp = 0; a_tmp < 3; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += H_0[3 * H_tmp + k] * K[6 * a_tmp + H_tmp]; - } - - H_tmp = 3 * a_tmp + k; - H_1[H_tmp] = (real_T)K_tmp[H_tmp] * 0.0625 + P_2; - } - } - - mrdiv(P_1, H_1, K); - for (k = 0; k < 3; k++) { - P_2 = 0.0; - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 += H[3 * a_tmp + k] * delta_x[a_tmp]; - } - - tmp_1[k] = (attitude_determination_model_ext_inputs.mes_mag[k] - rtb_hat_omega[k]) - P_2; - } - - P_2 = tmp_1[1]; - rtb_q_n2m_idx_0 = tmp_1[0]; - rtb_q_n2m_idx_1 = tmp_1[2]; - for (k = 0; k < 6; k++) { - delta_x[k] += (K[k + 6] * P_2 + K[k] * rtb_q_n2m_idx_0) + K[k + 12] * - rtb_q_n2m_idx_1; - } - - memset(&F[0], 0, 36U * sizeof(real_T)); - for (k = 0; k < 6; k++) { - F[k + 6 * k] = 1.0; - } - - for (k = 0; k < 6; k++) { - rtb_q_n2m_idx_0 = K[k + 6]; - rtb_q_n2m_idx_1 = K[k]; - rtb_q_n2m_idx_2 = K[k + 12]; - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - F_tmp = 6 * a_tmp + k; - F_0[F_tmp] = F[F_tmp] - ((H[3 * a_tmp + 1] * rtb_q_n2m_idx_0 + H[3 * a_tmp] - * rtb_q_n2m_idx_1) + H[3 * a_tmp + 2] * rtb_q_n2m_idx_2); - } - - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - F_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - F_2 += F_0[6 * H_tmp + k] * P_0[6 * a_tmp + H_tmp]; - } - - F_1[k + 6 * a_tmp] = F_2; - } - } - - memcpy(&P_0[0], &F_1[0], 36U * sizeof(real_T)); - tmp[0] = 0.0; - tmp[1] = delta_x[0]; - tmp[2] = delta_x[1]; - tmp[3] = delta_x[2]; - quatmultiply(rtDW.q_n2m, tmp, tmp_0); - q_n2m[0] = 0.5 * tmp_0[0] + rtDW.q_n2m[0]; - q_n2m[1] = 0.5 * tmp_0[1] + rtDW.q_n2m[1]; - q_n2m[2] = 0.5 * tmp_0[2] + rtDW.q_n2m[2]; - q_n2m[3] = 0.5 * tmp_0[3] + rtDW.q_n2m[3]; - P_2 = norm(q_n2m); - q_n2m[0] /= P_2; - q_n2m[1] /= P_2; - q_n2m[2] /= P_2; - q_n2m[3] /= P_2; - rtb_hat_omega[0] = attitude_determination_model_ext_inputs.omega[0] - rtDW.beta[0]; - rtb_hat_omega[1] = attitude_determination_model_ext_inputs.omega[1] - rtDW.beta[1]; - rtb_hat_omega[2] = attitude_determination_model_ext_inputs.omega[2] - rtDW.beta[2]; - tmp[0] = 0.0; - tmp[1] = rtb_hat_omega[0]; - tmp[2] = rtb_hat_omega[1]; - tmp[3] = rtb_hat_omega[2]; - quatmultiply(q_n2m, tmp, tmp_0); - q_n2m[0] += 0.5 * tmp_0[0] * 0.1; - q_n2m[1] += 0.5 * tmp_0[1] * 0.1; - q_n2m[2] += 0.5 * tmp_0[2] * 0.1; - q_n2m[3] += 0.5 * tmp_0[3] * 0.1; - P_2 = norm(q_n2m); - rtb_q_n2m_idx_0 = q_n2m[0] / P_2; - rtb_q_n2m_idx_1 = q_n2m[1] / P_2; - rtb_q_n2m_idx_2 = q_n2m[2] / P_2; - rtb_q_n2m_idx_3 = q_n2m[3] / P_2; - for (k = 0; k < 9; k++) { - K_tmp[k] = b[k]; - } - - F[0] = -0.0; - F[6] = rtb_hat_omega[2]; - F[12] = -rtb_hat_omega[1]; - F[1] = -rtb_hat_omega[2]; - F[7] = -0.0; - F[13] = rtb_hat_omega[0]; - F[2] = rtb_hat_omega[1]; - F[8] = -rtb_hat_omega[0]; - F[14] = -0.0; - for (k = 0; k < 3; k++) { - F_tmp = (k + 3) * 6; - F[F_tmp] = K_tmp[3 * k]; - F[F_tmp + 1] = K_tmp[3 * k + 1]; - F[F_tmp + 2] = K_tmp[3 * k + 2]; - } - - for (k = 0; k < 6; k++) { - F[6 * k + 3] = 0.0; - F[6 * k + 4] = 0.0; - F[6 * k + 5] = 0.0; - } - - for (k = 0; k < 36; k++) { - P_o_tmp[k] = e_a[k]; - } - - for (k = 0; k < 6; k++) { - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - F_2 = 0.0; - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - F_tmp = 6 * H_tmp + k; - F_2 += P_0[6 * a_tmp + H_tmp] * F[F_tmp]; - P_2 += F[6 * H_tmp + a_tmp] * P_0[F_tmp]; - } - - H_tmp = 6 * a_tmp + k; - F_1[H_tmp] = P_2; - F_0[H_tmp] = F_2; - } - } - - for (k = 0; k < 3; k++) { - P_2 = (real_T)K_tmp[3 * k] * 0.0625; - a[6 * k] = P_2; - a_tmp = (k + 3) * 6; - a[a_tmp] = 0.0; - a[6 * k + 3] = 0.0; - a[a_tmp + 3] = P_2; - P_2 = (real_T)K_tmp[3 * k + 1] * 0.0625; - a[6 * k + 1] = P_2; - a[a_tmp + 1] = 0.0; - a[6 * k + 4] = 0.0; - a[a_tmp + 4] = P_2; - P_2 = (real_T)K_tmp[3 * k + 2] * 0.0625; - a[6 * k + 2] = P_2; - a[a_tmp + 2] = 0.0; - a[6 * k + 5] = 0.0; - a[a_tmp + 5] = P_2; - } - - for (k = 0; k < 6; k++) { - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += (real_T)P_o_tmp[6 * H_tmp + k] * a[6 * a_tmp + H_tmp]; - } - - P_o_tmp_0[k + 6 * a_tmp] = P_2; - } - - for (a_tmp = 0; a_tmp < 6; a_tmp++) { - P_2 = 0.0; - for (H_tmp = 0; H_tmp < 6; H_tmp++) { - P_2 += P_o_tmp_0[6 * H_tmp + k] * (real_T)P_o_tmp[6 * a_tmp + H_tmp]; - } - - F_tmp = 6 * a_tmp + k; - F[F_tmp] = (F_0[F_tmp] + F_1[F_tmp]) + P_2; - } - } - - for (k = 0; k < 36; k++) { - rtDW.P_o[k] = F[k] * 0.1 + P_0[k]; - } - - /* End of MATLAB Function: '/MEKF' */ - - /* DataStoreWrite: '/Data Store Write1' */ - rtDW.q_n2m[0] = rtb_q_n2m_idx_0; - rtDW.q_n2m[1] = rtb_q_n2m_idx_1; - rtDW.q_n2m[2] = rtb_q_n2m_idx_2; - rtDW.q_n2m[3] = rtb_q_n2m_idx_3; - - /* Outport: '/meas_ang_vel_body' incorporates: - * Math: '/Transpose2' - */ - attitude_determination_model_ext_outputs.meas_ang_vel_body[0] = rtb_hat_omega[0]; - attitude_determination_model_ext_outputs.meas_ang_vel_body[1] = rtb_hat_omega[1]; - attitude_determination_model_ext_outputs.meas_ang_vel_body[2] = rtb_hat_omega[2]; - - /* Outport: '/meas_quat_body' */ - attitude_determination_model_ext_outputs.meas_quat_body[0] = rtb_q_n2m_idx_0; - attitude_determination_model_ext_outputs.meas_quat_body[1] = rtb_q_n2m_idx_1; - attitude_determination_model_ext_outputs.meas_quat_body[2] = rtb_q_n2m_idx_2; - attitude_determination_model_ext_outputs.meas_quat_body[3] = rtb_q_n2m_idx_3; -} - -/* Model initialize function */ -void attitude_determination_and_vehi_initialize(void) -{ - /* Start for DataStoreMemory: '/Data Store Memory' */ - memcpy(&rtDW.P_o[0], &rtConstP.DataStoreMemory_InitialValue[0], 36U * sizeof - (real_T)); - - /* Start for DataStoreMemory: '/Data Store Memory1' */ - rtDW.q_n2m[0] = 1.0; - rtDW.q_n2m[1] = 0.0; - rtDW.q_n2m[2] = 0.0; - rtDW.q_n2m[3] = 0.0; -} - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: attitude_determination_and_vehi.c + * + * Code generated for Simulink model 'attitude_determination_and_vehi'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:50:14 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#include "attitude_determination_and_vehi.h" +#include "rtwtypes.h" +#include +#include + +/* Block signals and states (default storage) */ +DW rtDW; + +/* Constant parameters (default storage) */ +const ConstP rtConstP; + +/* External inputs (root inport signals with default storage) */ +attitude_determination_model_ext_inputs_t attitude_determination_model_ext_inputs; + +/* External outputs (root outports fed by signals with default storage) */ +attitude_determination_model_ext_outputs_t attitude_determination_model_ext_outputs; + +/* Real-time model */ +static RT_MODEL_attitude_determination rtM_; +RT_MODEL_attitude_determination *const attitude_determinataion_model_rt_object = &rtM_; + +/* Forward declaration for local functions */ +static void quatrotate(const real_T q[4], real_T v[3]); +static void mrdiv(const real_T A[18], const real_T B_0[9], real_T Y[18]); +static void quatmultiply(const real_T q[4], const real_T r[4], real_T qout[4]); +static real_T norm(const real_T x[4]); + +/* Function for MATLAB Function: '/MEKF' */ +static void quatrotate(const real_T q[4], real_T v[3]) +{ + real_T y[9]; + real_T y_0[3]; + real_T y_tmp; + real_T y_tmp_0; + real_T y_tmp_1; + real_T y_tmp_2; + real_T y_tmp_3; + real_T y_tmp_4; + int32_T i; + y_tmp_1 = q[3] * q[3] * 2.0; + y_tmp_4 = (1.0 - q[2] * q[2] * 2.0) - y_tmp_1; + y[0] = y_tmp_4; + y_tmp = q[1] * q[2]; + y_tmp_0 = q[0] * q[3]; + y[3] = (y_tmp + y_tmp_0) * 2.0; + y_tmp_2 = q[1] * q[3]; + y_tmp_3 = q[0] * q[2]; + y[6] = (y_tmp_2 - y_tmp_3) * 2.0; + y[1] = (y_tmp - y_tmp_0) * 2.0; + y[4] = (1.0 - q[1] * q[1] * 2.0) - y_tmp_1; + y_tmp_1 = q[2] * q[3]; + y_tmp = q[0] * q[1]; + y[7] = (y_tmp_1 + y_tmp) * 2.0; + y[2] = (y_tmp_2 + y_tmp_3) * 2.0; + y[5] = (y_tmp_1 - y_tmp) * 2.0; + y[8] = y_tmp_4; + y_tmp_1 = v[1]; + y_tmp_4 = v[0]; + y_tmp = v[2]; + for (i = 0; i < 3; i++) { + y_0[i] = (y[i + 3] * y_tmp_1 + y[i] * y_tmp_4) + y[i + 6] * y_tmp; + } + + v[0] = y_0[0]; + v[1] = y_0[1]; + v[2] = y_0[2]; +} + +/* Function for MATLAB Function: '/MEKF' */ +static void mrdiv(const real_T A[18], const real_T B_0[9], real_T Y[18]) +{ + real_T b_A[9]; + real_T a21; + real_T maxval; + int32_T r1; + int32_T r2; + int32_T r3; + int32_T rtemp; + memcpy(&b_A[0], &B_0[0], 9U * sizeof(real_T)); + r1 = 0; + r2 = 1; + r3 = 2; + maxval = fabs(B_0[0]); + a21 = fabs(B_0[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if (fabs(B_0[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + b_A[r2] = B_0[r2] / B_0[r1]; + b_A[r3] /= b_A[r1]; + b_A[r2 + 3] -= b_A[r1 + 3] * b_A[r2]; + b_A[r3 + 3] -= b_A[r1 + 3] * b_A[r3]; + b_A[r2 + 6] -= b_A[r1 + 6] * b_A[r2]; + b_A[r3 + 6] -= b_A[r1 + 6] * b_A[r3]; + if (fabs(b_A[r3 + 3]) > fabs(b_A[r2 + 3])) { + rtemp = r2; + r2 = r3; + r3 = rtemp; + } + + b_A[r3 + 3] /= b_A[r2 + 3]; + b_A[r3 + 6] -= b_A[r3 + 3] * b_A[r2 + 6]; + for (rtemp = 0; rtemp < 6; rtemp++) { + int32_T Y_tmp; + int32_T Y_tmp_0; + int32_T Y_tmp_1; + Y_tmp = 6 * r1 + rtemp; + Y[Y_tmp] = A[rtemp] / b_A[r1]; + Y_tmp_0 = 6 * r2 + rtemp; + Y[Y_tmp_0] = A[rtemp + 6] - b_A[r1 + 3] * Y[Y_tmp]; + Y_tmp_1 = 6 * r3 + rtemp; + Y[Y_tmp_1] = A[rtemp + 12] - b_A[r1 + 6] * Y[Y_tmp]; + Y[Y_tmp_0] /= b_A[r2 + 3]; + Y[Y_tmp_1] -= b_A[r2 + 6] * Y[Y_tmp_0]; + Y[Y_tmp_1] /= b_A[r3 + 6]; + Y[Y_tmp_0] -= b_A[r3 + 3] * Y[Y_tmp_1]; + Y[Y_tmp] -= Y[Y_tmp_1] * b_A[r3]; + Y[Y_tmp] -= Y[Y_tmp_0] * b_A[r2]; + } +} + +/* Function for MATLAB Function: '/MEKF' */ +static void quatmultiply(const real_T q[4], const real_T r[4], real_T qout[4]) +{ + qout[0] = ((q[0] * r[0] - q[1] * r[1]) - q[2] * r[2]) - q[3] * r[3]; + qout[1] = (q[0] * r[1] + r[0] * q[1]) + (q[2] * r[3] - r[2] * q[3]); + qout[2] = (q[0] * r[2] + r[0] * q[2]) + (r[1] * q[3] - q[1] * r[3]); + qout[3] = (q[0] * r[3] + r[0] * q[3]) + (q[1] * r[2] - r[1] * q[2]); +} + +/* Function for MATLAB Function: '/MEKF' */ +static real_T norm(const real_T x[4]) +{ + real_T absxk; + real_T scale; + real_T t; + real_T y; + scale = 3.3121686421112381E-170; + absxk = fabs(x[0]); + if (absxk > 3.3121686421112381E-170) { + y = 1.0; + scale = absxk; + } else { + t = absxk / 3.3121686421112381E-170; + y = t * t; + } + + absxk = fabs(x[1]); + if (absxk > scale) { + t = scale / absxk; + y = y * t * t + 1.0; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + + absxk = fabs(x[2]); + if (absxk > scale) { + t = scale / absxk; + y = y * t * t + 1.0; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + + absxk = fabs(x[3]); + if (absxk > scale) { + t = scale / absxk; + y = y * t * t + 1.0; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + + return scale * sqrt(y); +} + +/* Model step function */ +void attitude_determination_and_vehi_step(void) +{ + real_T F[36]; + real_T F_0[36]; + real_T F_1[36]; + real_T P_0[36]; + real_T P_o_tmp_0[36]; + real_T a[36]; + real_T H[18]; + real_T H_0[18]; + real_T K[18]; + real_T P_1[18]; + real_T H_1[9]; + real_T delta_x[6]; + real_T q_n2m[4]; + real_T tmp[4]; + real_T tmp_0[4]; + real_T rtb_hat_omega[3]; + real_T tmp_1[3]; + real_T F_2; + real_T P_2; + real_T rtb_q_n2m_idx_0; + real_T rtb_q_n2m_idx_1; + real_T rtb_q_n2m_idx_2; + real_T rtb_q_n2m_idx_3; + int32_T F_tmp; + int32_T H_tmp; + int32_T a_tmp; + int32_T k; + int8_T P_o_tmp[36]; + int8_T K_tmp[9]; + static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + static const int8_T e_a[36] = { -1, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, -1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; + + /* MATLAB Function: '/MEKF' incorporates: + * Constant: '/Kalman Filter Timestep' + * DataStoreRead: '/Data Store Read1' + * DataStoreRead: '/Data Store Read2' + * DataStoreWrite: '/Data Store Write' + * Inport: '/earth_mag_field_ref' + * Inport: '/mes_mag' + * Inport: '/mes_ss' + * Inport: '/omega' + * Inport: '/sat_to_sun_unit_ref' + * Math: '/Transpose1' + */ + rtb_hat_omega[0] = attitude_determination_model_ext_inputs.sat_to_sun_unit_ref[0]; + rtb_hat_omega[1] = attitude_determination_model_ext_inputs.sat_to_sun_unit_ref[1]; + rtb_hat_omega[2] = attitude_determination_model_ext_inputs.sat_to_sun_unit_ref[2]; + quatrotate(rtDW.q_n2m, rtb_hat_omega); + H[0] = 0.0; + H[3] = -rtb_hat_omega[2]; + H[6] = rtb_hat_omega[1]; + H[9] = 0.0; + H[12] = 0.0; + H[15] = 0.0; + H[1] = rtb_hat_omega[2]; + H[4] = 0.0; + H[7] = -rtb_hat_omega[0]; + H[10] = 0.0; + H[13] = 0.0; + H[16] = 0.0; + H[2] = -rtb_hat_omega[1]; + H[5] = rtb_hat_omega[0]; + H[8] = 0.0; + H[11] = 0.0; + H[14] = 0.0; + H[17] = 0.0; + for (k = 0; k < 3; k++) { + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + K[a_tmp + 6 * k] = H[3 * a_tmp + k]; + } + } + + for (k = 0; k < 9; k++) { + K_tmp[k] = b[k]; + } + + for (k = 0; k < 6; k++) { + for (a_tmp = 0; a_tmp < 3; a_tmp++) { + rtb_q_n2m_idx_0 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + rtb_q_n2m_idx_0 += H[3 * H_tmp + a_tmp] * rtDW.P_o[6 * k + H_tmp]; + } + + H_0[a_tmp + 3 * k] = rtb_q_n2m_idx_0; + } + } + + for (k = 0; k < 3; k++) { + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += rtDW.P_o[6 * H_tmp + a_tmp] * K[6 * k + H_tmp]; + } + + P_1[a_tmp + 6 * k] = P_2; + } + + for (a_tmp = 0; a_tmp < 3; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += H_0[3 * H_tmp + k] * K[6 * a_tmp + H_tmp]; + } + + H_tmp = 3 * a_tmp + k; + H_1[H_tmp] = (real_T)K_tmp[H_tmp] * 0.0625 + P_2; + } + } + + mrdiv(P_1, H_1, K); + for (k = 0; k < 3; k++) { + P_2 = 0.0; + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 += H[3 * a_tmp + k]; + } + + tmp_1[k] = (attitude_determination_model_ext_inputs.mes_ss[k] - rtb_hat_omega[k]) - P_2; + } + + P_2 = tmp_1[1]; + rtb_q_n2m_idx_0 = tmp_1[0]; + rtb_q_n2m_idx_1 = tmp_1[2]; + for (k = 0; k < 6; k++) { + delta_x[k] = (K[k + 6] * P_2 + K[k] * rtb_q_n2m_idx_0) + K[k + 12] * + rtb_q_n2m_idx_1; + } + + memset(&F[0], 0, 36U * sizeof(real_T)); + for (k = 0; k < 6; k++) { + F[k + 6 * k] = 1.0; + } + + for (k = 0; k < 6; k++) { + rtb_q_n2m_idx_0 = K[k + 6]; + rtb_q_n2m_idx_1 = K[k]; + rtb_q_n2m_idx_2 = K[k + 12]; + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + F_tmp = 6 * a_tmp + k; + F_0[F_tmp] = F[F_tmp] - ((H[3 * a_tmp + 1] * rtb_q_n2m_idx_0 + H[3 * a_tmp] + * rtb_q_n2m_idx_1) + H[3 * a_tmp + 2] * rtb_q_n2m_idx_2); + } + + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += F_0[6 * H_tmp + k] * rtDW.P_o[6 * a_tmp + H_tmp]; + } + + P_0[k + 6 * a_tmp] = P_2; + } + } + + rtb_hat_omega[0] = attitude_determination_model_ext_inputs.earth_mag_field_ref[0]; + rtb_hat_omega[1] = attitude_determination_model_ext_inputs.earth_mag_field_ref[1]; + rtb_hat_omega[2] = attitude_determination_model_ext_inputs.earth_mag_field_ref[2]; + quatrotate(rtDW.q_n2m, rtb_hat_omega); + H[0] = 0.0; + H[3] = -rtb_hat_omega[2]; + H[6] = rtb_hat_omega[1]; + H[9] = 0.0; + H[12] = 0.0; + H[15] = 0.0; + H[1] = rtb_hat_omega[2]; + H[4] = 0.0; + H[7] = -rtb_hat_omega[0]; + H[10] = 0.0; + H[13] = 0.0; + H[16] = 0.0; + H[2] = -rtb_hat_omega[1]; + H[5] = rtb_hat_omega[0]; + H[8] = 0.0; + H[11] = 0.0; + H[14] = 0.0; + H[17] = 0.0; + for (k = 0; k < 3; k++) { + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + F_tmp = 3 * a_tmp + k; + K[a_tmp + 6 * k] = H[F_tmp]; + rtb_q_n2m_idx_0 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + rtb_q_n2m_idx_0 += H[3 * H_tmp + k] * P_0[6 * a_tmp + H_tmp]; + } + + H_0[F_tmp] = rtb_q_n2m_idx_0; + } + } + + for (k = 0; k < 3; k++) { + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += P_0[6 * H_tmp + a_tmp] * K[6 * k + H_tmp]; + } + + P_1[a_tmp + 6 * k] = P_2; + } + + for (a_tmp = 0; a_tmp < 3; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += H_0[3 * H_tmp + k] * K[6 * a_tmp + H_tmp]; + } + + H_tmp = 3 * a_tmp + k; + H_1[H_tmp] = (real_T)K_tmp[H_tmp] * 0.0625 + P_2; + } + } + + mrdiv(P_1, H_1, K); + for (k = 0; k < 3; k++) { + P_2 = 0.0; + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 += H[3 * a_tmp + k] * delta_x[a_tmp]; + } + + tmp_1[k] = (attitude_determination_model_ext_inputs.mes_mag[k] - rtb_hat_omega[k]) - P_2; + } + + P_2 = tmp_1[1]; + rtb_q_n2m_idx_0 = tmp_1[0]; + rtb_q_n2m_idx_1 = tmp_1[2]; + for (k = 0; k < 6; k++) { + delta_x[k] += (K[k + 6] * P_2 + K[k] * rtb_q_n2m_idx_0) + K[k + 12] * + rtb_q_n2m_idx_1; + } + + memset(&F[0], 0, 36U * sizeof(real_T)); + for (k = 0; k < 6; k++) { + F[k + 6 * k] = 1.0; + } + + for (k = 0; k < 6; k++) { + rtb_q_n2m_idx_0 = K[k + 6]; + rtb_q_n2m_idx_1 = K[k]; + rtb_q_n2m_idx_2 = K[k + 12]; + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + F_tmp = 6 * a_tmp + k; + F_0[F_tmp] = F[F_tmp] - ((H[3 * a_tmp + 1] * rtb_q_n2m_idx_0 + H[3 * a_tmp] + * rtb_q_n2m_idx_1) + H[3 * a_tmp + 2] * rtb_q_n2m_idx_2); + } + + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + F_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + F_2 += F_0[6 * H_tmp + k] * P_0[6 * a_tmp + H_tmp]; + } + + F_1[k + 6 * a_tmp] = F_2; + } + } + + memcpy(&P_0[0], &F_1[0], 36U * sizeof(real_T)); + tmp[0] = 0.0; + tmp[1] = delta_x[0]; + tmp[2] = delta_x[1]; + tmp[3] = delta_x[2]; + quatmultiply(rtDW.q_n2m, tmp, tmp_0); + q_n2m[0] = 0.5 * tmp_0[0] + rtDW.q_n2m[0]; + q_n2m[1] = 0.5 * tmp_0[1] + rtDW.q_n2m[1]; + q_n2m[2] = 0.5 * tmp_0[2] + rtDW.q_n2m[2]; + q_n2m[3] = 0.5 * tmp_0[3] + rtDW.q_n2m[3]; + P_2 = norm(q_n2m); + q_n2m[0] /= P_2; + q_n2m[1] /= P_2; + q_n2m[2] /= P_2; + q_n2m[3] /= P_2; + rtb_hat_omega[0] = attitude_determination_model_ext_inputs.omega[0] - rtDW.beta[0]; + rtb_hat_omega[1] = attitude_determination_model_ext_inputs.omega[1] - rtDW.beta[1]; + rtb_hat_omega[2] = attitude_determination_model_ext_inputs.omega[2] - rtDW.beta[2]; + tmp[0] = 0.0; + tmp[1] = rtb_hat_omega[0]; + tmp[2] = rtb_hat_omega[1]; + tmp[3] = rtb_hat_omega[2]; + quatmultiply(q_n2m, tmp, tmp_0); + q_n2m[0] += 0.5 * tmp_0[0] * 0.1; + q_n2m[1] += 0.5 * tmp_0[1] * 0.1; + q_n2m[2] += 0.5 * tmp_0[2] * 0.1; + q_n2m[3] += 0.5 * tmp_0[3] * 0.1; + P_2 = norm(q_n2m); + rtb_q_n2m_idx_0 = q_n2m[0] / P_2; + rtb_q_n2m_idx_1 = q_n2m[1] / P_2; + rtb_q_n2m_idx_2 = q_n2m[2] / P_2; + rtb_q_n2m_idx_3 = q_n2m[3] / P_2; + for (k = 0; k < 9; k++) { + K_tmp[k] = b[k]; + } + + F[0] = -0.0; + F[6] = rtb_hat_omega[2]; + F[12] = -rtb_hat_omega[1]; + F[1] = -rtb_hat_omega[2]; + F[7] = -0.0; + F[13] = rtb_hat_omega[0]; + F[2] = rtb_hat_omega[1]; + F[8] = -rtb_hat_omega[0]; + F[14] = -0.0; + for (k = 0; k < 3; k++) { + F_tmp = (k + 3) * 6; + F[F_tmp] = K_tmp[3 * k]; + F[F_tmp + 1] = K_tmp[3 * k + 1]; + F[F_tmp + 2] = K_tmp[3 * k + 2]; + } + + for (k = 0; k < 6; k++) { + F[6 * k + 3] = 0.0; + F[6 * k + 4] = 0.0; + F[6 * k + 5] = 0.0; + } + + for (k = 0; k < 36; k++) { + P_o_tmp[k] = e_a[k]; + } + + for (k = 0; k < 6; k++) { + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + F_2 = 0.0; + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + F_tmp = 6 * H_tmp + k; + F_2 += P_0[6 * a_tmp + H_tmp] * F[F_tmp]; + P_2 += F[6 * H_tmp + a_tmp] * P_0[F_tmp]; + } + + H_tmp = 6 * a_tmp + k; + F_1[H_tmp] = P_2; + F_0[H_tmp] = F_2; + } + } + + for (k = 0; k < 3; k++) { + P_2 = (real_T)K_tmp[3 * k] * 0.0625; + a[6 * k] = P_2; + a_tmp = (k + 3) * 6; + a[a_tmp] = 0.0; + a[6 * k + 3] = 0.0; + a[a_tmp + 3] = P_2; + P_2 = (real_T)K_tmp[3 * k + 1] * 0.0625; + a[6 * k + 1] = P_2; + a[a_tmp + 1] = 0.0; + a[6 * k + 4] = 0.0; + a[a_tmp + 4] = P_2; + P_2 = (real_T)K_tmp[3 * k + 2] * 0.0625; + a[6 * k + 2] = P_2; + a[a_tmp + 2] = 0.0; + a[6 * k + 5] = 0.0; + a[a_tmp + 5] = P_2; + } + + for (k = 0; k < 6; k++) { + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += (real_T)P_o_tmp[6 * H_tmp + k] * a[6 * a_tmp + H_tmp]; + } + + P_o_tmp_0[k + 6 * a_tmp] = P_2; + } + + for (a_tmp = 0; a_tmp < 6; a_tmp++) { + P_2 = 0.0; + for (H_tmp = 0; H_tmp < 6; H_tmp++) { + P_2 += P_o_tmp_0[6 * H_tmp + k] * (real_T)P_o_tmp[6 * a_tmp + H_tmp]; + } + + F_tmp = 6 * a_tmp + k; + F[F_tmp] = (F_0[F_tmp] + F_1[F_tmp]) + P_2; + } + } + + for (k = 0; k < 36; k++) { + rtDW.P_o[k] = F[k] * 0.1 + P_0[k]; + } + + /* End of MATLAB Function: '/MEKF' */ + + /* DataStoreWrite: '/Data Store Write1' */ + rtDW.q_n2m[0] = rtb_q_n2m_idx_0; + rtDW.q_n2m[1] = rtb_q_n2m_idx_1; + rtDW.q_n2m[2] = rtb_q_n2m_idx_2; + rtDW.q_n2m[3] = rtb_q_n2m_idx_3; + + /* Outport: '/meas_ang_vel_body' incorporates: + * Math: '/Transpose2' + */ + attitude_determination_model_ext_outputs.meas_ang_vel_body[0] = rtb_hat_omega[0]; + attitude_determination_model_ext_outputs.meas_ang_vel_body[1] = rtb_hat_omega[1]; + attitude_determination_model_ext_outputs.meas_ang_vel_body[2] = rtb_hat_omega[2]; + + /* Outport: '/meas_quat_body' */ + attitude_determination_model_ext_outputs.meas_quat_body[0] = rtb_q_n2m_idx_0; + attitude_determination_model_ext_outputs.meas_quat_body[1] = rtb_q_n2m_idx_1; + attitude_determination_model_ext_outputs.meas_quat_body[2] = rtb_q_n2m_idx_2; + attitude_determination_model_ext_outputs.meas_quat_body[3] = rtb_q_n2m_idx_3; +} + +/* Model initialize function */ +void attitude_determination_and_vehi_initialize(void) +{ + /* Start for DataStoreMemory: '/Data Store Memory' */ + memcpy(&rtDW.P_o[0], &rtConstP.DataStoreMemory_InitialValue[0], 36U * sizeof + (real_T)); + + /* Start for DataStoreMemory: '/Data Store Memory1' */ + rtDW.q_n2m[0] = 1.0; + rtDW.q_n2m[1] = 0.0; + rtDW.q_n2m[2] = 0.0; + rtDW.q_n2m[3] = 0.0; +} + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/onboard_env_modelling_ert_rtw/include/onboard_env_modelling.h b/libs/gnc_code/onboard_env_modelling_ert_rtw/include/onboard_env_modelling.h index b62e55bf7..b29d1cee6 100644 --- a/libs/gnc_code/onboard_env_modelling_ert_rtw/include/onboard_env_modelling.h +++ b/libs/gnc_code/onboard_env_modelling_ert_rtw/include/onboard_env_modelling.h @@ -1,105 +1,105 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: onboard_env_modelling.h - * - * Code generated for Simulink model 'onboard_env_modelling'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:51:32 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#ifndef RTW_HEADER_onboard_env_modelling_h_ -#define RTW_HEADER_onboard_env_modelling_h_ -#ifndef onboard_env_modelling_COMMON_INCLUDES_ -#define onboard_env_modelling_COMMON_INCLUDES_ -#include "rtwtypes.h" -#include "rtw_continuous.h" -#include "rtw_solver.h" -#endif /* onboard_env_modelling_COMMON_INCLUDES_ */ - -/* Macros for accessing real-time model data structure */ -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -/* Forward declaration for rtModel */ -typedef struct tag_RTM_onboard_model RT_MODEL_onboard_model; - -/* External inputs (root inport signals with default storage) */ -typedef struct { - real_T commanded_mag_dipole_body[3]; /* '/magnetorquer comm' */ - real_T r_sat_com[3]; /* '/r_sat_com' */ - real_T r_sat_com_ax1[3]; /* '/r_sat_com_ax1' */ - real_T steve_values[2]; /* '/steve_values' */ -} onboard_env_model_ext_intputs_t; - -/* External outputs (root outports fed by signals with default storage) */ -typedef struct { - real_T estimated_expect_ang_acc_body[3]; - /* '/estimated_expect_ang_acc_body' */ - real_T r_ref_com_est[3]; /* '/r_ref_com_est' */ -} onboard_env_model_ext_outputs_t; - -/* Real-time Model Data Structure */ -struct tag_RTM_onboard_model { - const char_T * volatile errorStatus; -}; - -/* External inputs (root inport signals with default storage) */ -extern onboard_env_model_ext_intputs_t onboard_env_model_ext_intputs; - -/* External outputs (root outports fed by signals with default storage) */ -extern onboard_env_model_ext_outputs_t onboard_env_model_ext_outputs; - -/* Model entry point functions */ -extern void onboard_env_modelling_initialize(void); -extern void onboard_env_modelling_step(void); - -/* Real-time Model object */ -extern RT_MODEL_onboard_model *const onboard_env_model_rt_object; - -/*- - * The generated code includes comments that allow you to trace directly - * back to the appropriate location in the model. The basic format - * is /block_name, where system is the system number (uniquely - * assigned by Simulink) and block_name is the name of the block. - * - * Note that this particular code originates from a subsystem build, - * and has its own system numbers different from the parent model. - * Refer to the system hierarchy for this subsystem below, and use the - * MATLAB hilite_system command to trace the generated code back - * to the parent model. For example, - * - * hilite_system('cubesat_alg_dev_env_v2_main/onboard_env_modelling') - opens subsystem cubesat_alg_dev_env_v2_main/onboard_env_modelling - * hilite_system('cubesat_alg_dev_env_v2_main/onboard_env_modelling/Kp') - opens and selects block Kp - * - * Here is the system hierarchy for this model - * - * '' : 'cubesat_alg_dev_env_v2_main' - * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling' - * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling/Cross Product' - * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling/MATLAB Function6' - * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling/Regenerates from UV Values' - */ -#endif /* RTW_HEADER_onboard_env_modelling_h_ */ - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: onboard_env_modelling.h + * + * Code generated for Simulink model 'onboard_env_modelling'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:51:32 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTW_HEADER_onboard_env_modelling_h_ +#define RTW_HEADER_onboard_env_modelling_h_ +#ifndef onboard_env_modelling_COMMON_INCLUDES_ +#define onboard_env_modelling_COMMON_INCLUDES_ +#include "rtwtypes.h" +#include "rtw_continuous.h" +#include "rtw_solver.h" +#endif /* onboard_env_modelling_COMMON_INCLUDES_ */ + +/* Macros for accessing real-time model data structure */ +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +/* Forward declaration for rtModel */ +typedef struct tag_RTM_onboard_model RT_MODEL_onboard_model; + +/* External inputs (root inport signals with default storage) */ +typedef struct { + real_T commanded_mag_dipole_body[3]; /* '/magnetorquer comm' */ + real_T r_sat_com[3]; /* '/r_sat_com' */ + real_T r_sat_com_ax1[3]; /* '/r_sat_com_ax1' */ + real_T steve_values[2]; /* '/steve_values' */ +} onboard_env_model_ext_intputs_t; + +/* External outputs (root outports fed by signals with default storage) */ +typedef struct { + real_T estimated_expect_ang_acc_body[3]; + /* '/estimated_expect_ang_acc_body' */ + real_T r_ref_com_est[3]; /* '/r_ref_com_est' */ +} onboard_env_model_ext_outputs_t; + +/* Real-time Model Data Structure */ +struct tag_RTM_onboard_model { + const char_T * volatile errorStatus; +}; + +/* External inputs (root inport signals with default storage) */ +extern onboard_env_model_ext_intputs_t onboard_env_model_ext_intputs; + +/* External outputs (root outports fed by signals with default storage) */ +extern onboard_env_model_ext_outputs_t onboard_env_model_ext_outputs; + +/* Model entry point functions */ +extern void onboard_env_modelling_initialize(void); +extern void onboard_env_modelling_step(void); + +/* Real-time Model object */ +extern RT_MODEL_onboard_model *const onboard_env_model_rt_object; + +/*- + * The generated code includes comments that allow you to trace directly + * back to the appropriate location in the model. The basic format + * is /block_name, where system is the system number (uniquely + * assigned by Simulink) and block_name is the name of the block. + * + * Note that this particular code originates from a subsystem build, + * and has its own system numbers different from the parent model. + * Refer to the system hierarchy for this subsystem below, and use the + * MATLAB hilite_system command to trace the generated code back + * to the parent model. For example, + * + * hilite_system('cubesat_alg_dev_env_v2_main/onboard_env_modelling') - opens subsystem cubesat_alg_dev_env_v2_main/onboard_env_modelling + * hilite_system('cubesat_alg_dev_env_v2_main/onboard_env_modelling/Kp') - opens and selects block Kp + * + * Here is the system hierarchy for this model + * + * '' : 'cubesat_alg_dev_env_v2_main' + * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling' + * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling/Cross Product' + * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling/MATLAB Function6' + * '' : 'cubesat_alg_dev_env_v2_main/onboard_env_modelling/Regenerates from UV Values' + */ +#endif /* RTW_HEADER_onboard_env_modelling_h_ */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/onboard_env_modelling_ert_rtw/include/rtwtypes.h b/libs/gnc_code/onboard_env_modelling_ert_rtw/include/rtwtypes.h index 5028acd1d..9f715b11d 100644 --- a/libs/gnc_code/onboard_env_modelling_ert_rtw/include/rtwtypes.h +++ b/libs/gnc_code/onboard_env_modelling_ert_rtw/include/rtwtypes.h @@ -1,106 +1,106 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: rtwtypes.h - * - * Code generated for Simulink model 'onboard_env_modelling'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:51:32 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#ifndef RTWTYPES_H -#define RTWTYPES_H - -/* Logical type definitions */ -#if (!defined(__cplusplus)) -#ifndef false -#define false (0U) -#endif - -#ifndef true -#define true (1U) -#endif -#endif - -/*=======================================================================* - * Target hardware information - * Device type: ARM Compatible->ARM Cortex-R - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 long long: 64 - * native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef long long int64_T; -typedef unsigned long long uint64_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * - * real_T, time_T, ulong_T, ulonglong_T. * - *===========================================================================*/ -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef unsigned long long ulonglong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MAX_int64_T ((int64_T)(9223372036854775807LL)) -#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) -#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) - -/* Block D-Work pointer type */ -typedef void * pointer_T; - -#endif /* RTWTYPES_H */ - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: rtwtypes.h + * + * Code generated for Simulink model 'onboard_env_modelling'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:51:32 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/* Logical type definitions */ +#if (!defined(__cplusplus)) +#ifndef false +#define false (0U) +#endif + +#ifndef true +#define true (1U) +#endif +#endif + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex-R + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 long long: 64 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef long long int64_T; +typedef unsigned long long uint64_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * + * real_T, time_T, ulong_T, ulonglong_T. * + *===========================================================================*/ +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef unsigned long long ulonglong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MAX_int64_T ((int64_T)(9223372036854775807LL)) +#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) + +/* Block D-Work pointer type */ +typedef void * pointer_T; + +#endif /* RTWTYPES_H */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/libs/gnc_code/onboard_env_modelling_ert_rtw/source/onboard_env_modelling.c b/libs/gnc_code/onboard_env_modelling_ert_rtw/source/onboard_env_modelling.c index 82d804d24..ddca7e8de 100644 --- a/libs/gnc_code/onboard_env_modelling_ert_rtw/source/onboard_env_modelling.c +++ b/libs/gnc_code/onboard_env_modelling_ert_rtw/source/onboard_env_modelling.c @@ -1,112 +1,112 @@ -/* - * Academic License - for use in teaching, academic research, and meeting - * course requirements at degree granting institutions only. Not for - * government, commercial, or other organizational use. - * - * File: onboard_env_modelling.c - * - * Code generated for Simulink model 'onboard_env_modelling'. - * - * Model version : 3.78 - * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 - * C/C++ source code generated on : Mon Jan 1 12:51:32 2024 - * - * Target selection: ert.tlc - * Embedded hardware selection: ARM Compatible->ARM Cortex-R - * Code generation objectives: - * 1. Execution efficiency - * 2. RAM efficiency - * Validation result: Not run - */ - -#include "onboard_env_modelling.h" -#include -#include "rtwtypes.h" - -/* External inputs (root inport signals with default storage) */ -onboard_env_model_ext_intputs_t onboard_env_model_ext_intputs; - -/* External outputs (root outports fed by signals with default storage) */ -onboard_env_model_ext_outputs_t onboard_env_model_ext_outputs; - -/* Real-time model */ -static RT_MODEL_onboard_model rtM_; -RT_MODEL_onboard_model *const onboard_env_model_rt_object = &rtM_; - -/* Model step function */ -void onboard_env_modelling_step(void) -{ - real_T b; - - /* MATLAB Function: '/Regenerates from UV Values' incorporates: - * Inport: '/steve_values' - */ - b = sqrt((1.0 - onboard_env_model_ext_intputs.steve_values[0] * onboard_env_model_ext_intputs.steve_values[0]) - onboard_env_model_ext_intputs.steve_values - [1] * onboard_env_model_ext_intputs.steve_values[1]); - - /* Outport: '/r_ref_com_est' incorporates: - * Inport: '/r_sat_com' - * Inport: '/r_sat_com_ax1' - * Inport: '/steve_values' - * MATLAB Function: '/Regenerates from UV Values' - * Product: '/Element Product' - * Sum: '/Sum' - */ - onboard_env_model_ext_outputs.r_ref_com_est[0] = ((onboard_env_model_ext_intputs.r_sat_com_ax1[1] * onboard_env_model_ext_intputs.r_sat_com[2] - - onboard_env_model_ext_intputs.r_sat_com[1] * onboard_env_model_ext_intputs.r_sat_com_ax1[2]) * onboard_env_model_ext_intputs.steve_values[1] + - onboard_env_model_ext_intputs.r_sat_com[0] * onboard_env_model_ext_intputs.steve_values[0]) + onboard_env_model_ext_intputs.r_sat_com_ax1[0] * b; - - /* Outport: '/estimated_expect_ang_acc_body' incorporates: - * Inport: '/magnetorquer comm' - * MATLAB Function: '/MATLAB Function6' - */ - onboard_env_model_ext_outputs.estimated_expect_ang_acc_body[0] = onboard_env_model_ext_intputs.commanded_mag_dipole_body[0]; - - /* Outport: '/r_ref_com_est' incorporates: - * Inport: '/r_sat_com' - * Inport: '/r_sat_com_ax1' - * Inport: '/steve_values' - * MATLAB Function: '/Regenerates from UV Values' - * Product: '/Element Product' - * Sum: '/Sum' - */ - onboard_env_model_ext_outputs.r_ref_com_est[1] = ((onboard_env_model_ext_intputs.r_sat_com[0] * onboard_env_model_ext_intputs.r_sat_com_ax1[2] - - onboard_env_model_ext_intputs.r_sat_com_ax1[0] * onboard_env_model_ext_intputs.r_sat_com[2]) * onboard_env_model_ext_intputs.steve_values[1] + - onboard_env_model_ext_intputs.steve_values[0] * onboard_env_model_ext_intputs.r_sat_com[1]) + onboard_env_model_ext_intputs.r_sat_com_ax1[1] * b; - - /* Outport: '/estimated_expect_ang_acc_body' incorporates: - * Inport: '/magnetorquer comm' - * MATLAB Function: '/MATLAB Function6' - */ - onboard_env_model_ext_outputs.estimated_expect_ang_acc_body[1] = onboard_env_model_ext_intputs.commanded_mag_dipole_body[1]; - - /* Outport: '/r_ref_com_est' incorporates: - * Inport: '/r_sat_com' - * Inport: '/r_sat_com_ax1' - * Inport: '/steve_values' - * MATLAB Function: '/Regenerates from UV Values' - * Product: '/Element Product' - * Sum: '/Sum' - */ - onboard_env_model_ext_outputs.r_ref_com_est[2] = ((onboard_env_model_ext_intputs.r_sat_com_ax1[0] * onboard_env_model_ext_intputs.r_sat_com[1] - - onboard_env_model_ext_intputs.r_sat_com[0] * onboard_env_model_ext_intputs.r_sat_com_ax1[1]) * onboard_env_model_ext_intputs.steve_values[1] + - onboard_env_model_ext_intputs.steve_values[0] * onboard_env_model_ext_intputs.r_sat_com[2]) + onboard_env_model_ext_intputs.r_sat_com_ax1[2] * b; - - /* Outport: '/estimated_expect_ang_acc_body' incorporates: - * Inport: '/magnetorquer comm' - * MATLAB Function: '/MATLAB Function6' - */ - onboard_env_model_ext_outputs.estimated_expect_ang_acc_body[2] = onboard_env_model_ext_intputs.commanded_mag_dipole_body[2]; -} - -/* Model initialize function */ -void onboard_env_modelling_initialize(void) -{ - /* (no initialization code required) */ -} - -/* - * File trailer for generated code. - * - * [EOF] - */ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * + * File: onboard_env_modelling.c + * + * Code generated for Simulink model 'onboard_env_modelling'. + * + * Model version : 3.78 + * Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 + * C/C++ source code generated on : Mon Jan 1 12:51:32 2024 + * + * Target selection: ert.tlc + * Embedded hardware selection: ARM Compatible->ARM Cortex-R + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#include "onboard_env_modelling.h" +#include +#include "rtwtypes.h" + +/* External inputs (root inport signals with default storage) */ +onboard_env_model_ext_intputs_t onboard_env_model_ext_intputs; + +/* External outputs (root outports fed by signals with default storage) */ +onboard_env_model_ext_outputs_t onboard_env_model_ext_outputs; + +/* Real-time model */ +static RT_MODEL_onboard_model rtM_; +RT_MODEL_onboard_model *const onboard_env_model_rt_object = &rtM_; + +/* Model step function */ +void onboard_env_modelling_step(void) +{ + real_T b; + + /* MATLAB Function: '/Regenerates from UV Values' incorporates: + * Inport: '/steve_values' + */ + b = sqrt((1.0 - onboard_env_model_ext_intputs.steve_values[0] * onboard_env_model_ext_intputs.steve_values[0]) - onboard_env_model_ext_intputs.steve_values + [1] * onboard_env_model_ext_intputs.steve_values[1]); + + /* Outport: '/r_ref_com_est' incorporates: + * Inport: '/r_sat_com' + * Inport: '/r_sat_com_ax1' + * Inport: '/steve_values' + * MATLAB Function: '/Regenerates from UV Values' + * Product: '/Element Product' + * Sum: '/Sum' + */ + onboard_env_model_ext_outputs.r_ref_com_est[0] = ((onboard_env_model_ext_intputs.r_sat_com_ax1[1] * onboard_env_model_ext_intputs.r_sat_com[2] - + onboard_env_model_ext_intputs.r_sat_com[1] * onboard_env_model_ext_intputs.r_sat_com_ax1[2]) * onboard_env_model_ext_intputs.steve_values[1] + + onboard_env_model_ext_intputs.r_sat_com[0] * onboard_env_model_ext_intputs.steve_values[0]) + onboard_env_model_ext_intputs.r_sat_com_ax1[0] * b; + + /* Outport: '/estimated_expect_ang_acc_body' incorporates: + * Inport: '/magnetorquer comm' + * MATLAB Function: '/MATLAB Function6' + */ + onboard_env_model_ext_outputs.estimated_expect_ang_acc_body[0] = onboard_env_model_ext_intputs.commanded_mag_dipole_body[0]; + + /* Outport: '/r_ref_com_est' incorporates: + * Inport: '/r_sat_com' + * Inport: '/r_sat_com_ax1' + * Inport: '/steve_values' + * MATLAB Function: '/Regenerates from UV Values' + * Product: '/Element Product' + * Sum: '/Sum' + */ + onboard_env_model_ext_outputs.r_ref_com_est[1] = ((onboard_env_model_ext_intputs.r_sat_com[0] * onboard_env_model_ext_intputs.r_sat_com_ax1[2] - + onboard_env_model_ext_intputs.r_sat_com_ax1[0] * onboard_env_model_ext_intputs.r_sat_com[2]) * onboard_env_model_ext_intputs.steve_values[1] + + onboard_env_model_ext_intputs.steve_values[0] * onboard_env_model_ext_intputs.r_sat_com[1]) + onboard_env_model_ext_intputs.r_sat_com_ax1[1] * b; + + /* Outport: '/estimated_expect_ang_acc_body' incorporates: + * Inport: '/magnetorquer comm' + * MATLAB Function: '/MATLAB Function6' + */ + onboard_env_model_ext_outputs.estimated_expect_ang_acc_body[1] = onboard_env_model_ext_intputs.commanded_mag_dipole_body[1]; + + /* Outport: '/r_ref_com_est' incorporates: + * Inport: '/r_sat_com' + * Inport: '/r_sat_com_ax1' + * Inport: '/steve_values' + * MATLAB Function: '/Regenerates from UV Values' + * Product: '/Element Product' + * Sum: '/Sum' + */ + onboard_env_model_ext_outputs.r_ref_com_est[2] = ((onboard_env_model_ext_intputs.r_sat_com_ax1[0] * onboard_env_model_ext_intputs.r_sat_com[1] - + onboard_env_model_ext_intputs.r_sat_com[0] * onboard_env_model_ext_intputs.r_sat_com_ax1[1]) * onboard_env_model_ext_intputs.steve_values[1] + + onboard_env_model_ext_intputs.steve_values[0] * onboard_env_model_ext_intputs.r_sat_com[2]) + onboard_env_model_ext_intputs.r_sat_com_ax1[2] * b; + + /* Outport: '/estimated_expect_ang_acc_body' incorporates: + * Inport: '/magnetorquer comm' + * MATLAB Function: '/MATLAB Function6' + */ + onboard_env_model_ext_outputs.estimated_expect_ang_acc_body[2] = onboard_env_model_ext_intputs.commanded_mag_dipole_body[2]; +} + +/* Model initialize function */ +void onboard_env_modelling_initialize(void) +{ + /* (no initialization code required) */ +} + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/obc/app/drivers/rm46/obc_adc.c b/obc/app/drivers/rm46/obc_adc.c index 46912fe71..fc2a45682 100644 --- a/obc/app/drivers/rm46/obc_adc.c +++ b/obc/app/drivers/rm46/obc_adc.c @@ -1,143 +1,143 @@ -#include "obc_logging.h" -#include "obc_errors.h" -#include "adc.h" -#include "reg_adc.h" -#include "obc_adc.h" - -#include -#include -#include -#include - -static SemaphoreHandle_t adcConversionMutex = NULL; -static StaticSemaphore_t adcConversionMutexBuffer; - -// Should be configured to max FIFO size for all groups, so that an adcData_t array of a proper size can be made -#define MAX_GROUP_SIZE 16U - -// Assumes all groups have same resolution for simplicity. Could make this more flexible/an enum. -#define RESOLUTION 12U - -// VCCAD is 3.3V -#define REF_VOLTAGE_HIGH 3.3f -#define REF_VOLTAGE_LOW 0.0f - -// Arbitrary -#define ADC_MAX_WAIT_CYCLES 1000 - -#define ADC_NUM_GROUPS 3 - -typedef enum { - ADC1_GROUP0_NUM_CHANNELS = 16U, - ADC1_GROUP1_NUM_CHANNELS = 16U, - ADC1_GROUP2_NUM_CHANNELS = 16U, - ADC2_GROUP0_NUM_CHANNELS = 16U, - ADC2_GROUP1_NUM_CHANNELS = 16U, - ADC2_GROUP2_NUM_CHANNELS = 16U -} adc_group_channels_t; - -#define ADC_INDEX(module, group) ((module) * ADC_NUM_GROUPS + (group)) - -const uint32_t adcGroupLengths[6] = {ADC1_GROUP0_NUM_CHANNELS, ADC1_GROUP1_NUM_CHANNELS, ADC1_GROUP2_NUM_CHANNELS, - ADC2_GROUP0_NUM_CHANNELS, ADC2_GROUP1_NUM_CHANNELS, ADC2_GROUP2_NUM_CHANNELS}; - -void initADC(void) { - adcInit(); - if (adcConversionMutex == NULL) { - adcConversionMutex = xSemaphoreCreateMutexStatic(&adcConversionMutexBuffer); - } - ASSERT(adcConversionMutex != NULL); -} - -static obc_error_code_t adcGetGroupReadings(adc_module_t adc, adc_group_t group, adcData_t *data, - TickType_t blockTime) { - if (data == NULL) { - return OBC_ERR_CODE_INVALID_ARG; - } - - if (xSemaphoreTake(adcConversionMutex, blockTime) != pdTRUE) { - return OBC_ERR_CODE_MUTEX_TIMEOUT; - } - - adcBASE_t *adcReg = (adc == ADC_MODULE_1) ? adcREG1 : adcREG2; - - adcStartConversion(adcReg, group); - - uint16_t totalAttempts = 0; - while (!adcIsConversionComplete(adcReg, group) && totalAttempts < ADC_MAX_WAIT_CYCLES) { - totalAttempts++; - } - - if (totalAttempts >= ADC_MAX_WAIT_CYCLES) { - xSemaphoreGive(adcConversionMutex); - return OBC_ERR_CODE_ADC_FAILURE; - } - - adcStopConversion(adcReg, group); - - adcGetData(adcReg, group, data); - - xSemaphoreGive(adcConversionMutex); - - return OBC_ERR_CODE_SUCCESS; -} - -obc_error_code_t adcDigitalToAnalog(uint16_t reading, float *buffer) { - if (buffer == NULL) { - return OBC_ERR_CODE_INVALID_ARG; - } - *buffer = (float)(reading) * (REF_VOLTAGE_HIGH - REF_VOLTAGE_LOW) / ((float)(1 << RESOLUTION) + REF_VOLTAGE_LOW); - return OBC_ERR_CODE_SUCCESS; -} - -obc_error_code_t adcGetSingleData(adc_module_t adc, adc_channel_t channel, adc_group_t group, uint16_t *reading, - TickType_t blockTime) { - if (reading == NULL) { - return OBC_ERR_CODE_INVALID_ARG; - } - - if (adcConversionMutex == NULL) { - return OBC_ERR_CODE_INVALID_STATE; - } - - adcData_t adcData[MAX_GROUP_SIZE]; - - obc_error_code_t errCode; - RETURN_IF_ERROR_CODE(adcGetGroupReadings(adc, group, adcData, blockTime)); - - uint8_t index = ADC_INDEX(adc, group); - uint32_t groupSize = adcGroupLengths[index]; - - for (uint32_t i = 0; i < groupSize; i++) { - if (adcData[i].id == channel) { - *reading = adcData[i].value; - return OBC_ERR_CODE_SUCCESS; - } - } - - return OBC_ERR_CODE_ADC_INVALID_CHANNEL; -} - -obc_error_code_t adcGetGroupData(adc_module_t adc, adc_group_t group, uint16_t *readings, TickType_t blockTime) { - if (readings == NULL) { - return OBC_ERR_CODE_INVALID_ARG; - } - - if (adcConversionMutex == NULL) { - return OBC_ERR_CODE_INVALID_STATE; - } - - adcData_t adcData[MAX_GROUP_SIZE]; - - obc_error_code_t errCode; - RETURN_IF_ERROR_CODE(adcGetGroupReadings(adc, group, adcData, blockTime)); - - uint8_t index = ADC_INDEX(adc, group); - uint32_t groupSize = adcGroupLengths[index]; - - for (uint32_t i = 0; i < groupSize; i++) { - readings[i] = adcData[i].value; - } - - return OBC_ERR_CODE_SUCCESS; -} +#include "obc_logging.h" +#include "obc_errors.h" +#include "adc.h" +#include "reg_adc.h" +#include "obc_adc.h" + +#include +#include +#include +#include + +static SemaphoreHandle_t adcConversionMutex = NULL; +static StaticSemaphore_t adcConversionMutexBuffer; + +// Should be configured to max FIFO size for all groups, so that an adcData_t array of a proper size can be made +#define MAX_GROUP_SIZE 16U + +// Assumes all groups have same resolution for simplicity. Could make this more flexible/an enum. +#define RESOLUTION 12U + +// VCCAD is 3.3V +#define REF_VOLTAGE_HIGH 3.3f +#define REF_VOLTAGE_LOW 0.0f + +// Arbitrary +#define ADC_MAX_WAIT_CYCLES 1000 + +#define ADC_NUM_GROUPS 3 + +typedef enum { + ADC1_GROUP0_NUM_CHANNELS = 16U, + ADC1_GROUP1_NUM_CHANNELS = 16U, + ADC1_GROUP2_NUM_CHANNELS = 16U, + ADC2_GROUP0_NUM_CHANNELS = 16U, + ADC2_GROUP1_NUM_CHANNELS = 16U, + ADC2_GROUP2_NUM_CHANNELS = 16U +} adc_group_channels_t; + +#define ADC_INDEX(module, group) ((module) * ADC_NUM_GROUPS + (group)) + +const uint32_t adcGroupLengths[6] = {ADC1_GROUP0_NUM_CHANNELS, ADC1_GROUP1_NUM_CHANNELS, ADC1_GROUP2_NUM_CHANNELS, + ADC2_GROUP0_NUM_CHANNELS, ADC2_GROUP1_NUM_CHANNELS, ADC2_GROUP2_NUM_CHANNELS}; + +void initADC(void) { + adcInit(); + if (adcConversionMutex == NULL) { + adcConversionMutex = xSemaphoreCreateMutexStatic(&adcConversionMutexBuffer); + } + ASSERT(adcConversionMutex != NULL); +} + +static obc_error_code_t adcGetGroupReadings(adc_module_t adc, adc_group_t group, adcData_t *data, + TickType_t blockTime) { + if (data == NULL) { + return OBC_ERR_CODE_INVALID_ARG; + } + + if (xSemaphoreTake(adcConversionMutex, blockTime) != pdTRUE) { + return OBC_ERR_CODE_MUTEX_TIMEOUT; + } + + adcBASE_t *adcReg = (adc == ADC_MODULE_1) ? adcREG1 : adcREG2; + + adcStartConversion(adcReg, group); + + uint16_t totalAttempts = 0; + while (!adcIsConversionComplete(adcReg, group) && totalAttempts < ADC_MAX_WAIT_CYCLES) { + totalAttempts++; + } + + if (totalAttempts >= ADC_MAX_WAIT_CYCLES) { + xSemaphoreGive(adcConversionMutex); + return OBC_ERR_CODE_ADC_FAILURE; + } + + adcStopConversion(adcReg, group); + + adcGetData(adcReg, group, data); + + xSemaphoreGive(adcConversionMutex); + + return OBC_ERR_CODE_SUCCESS; +} + +obc_error_code_t adcDigitalToAnalog(uint16_t reading, float *buffer) { + if (buffer == NULL) { + return OBC_ERR_CODE_INVALID_ARG; + } + *buffer = (float)(reading) * (REF_VOLTAGE_HIGH - REF_VOLTAGE_LOW) / ((float)(1 << RESOLUTION) + REF_VOLTAGE_LOW); + return OBC_ERR_CODE_SUCCESS; +} + +obc_error_code_t adcGetSingleData(adc_module_t adc, adc_channel_t channel, adc_group_t group, uint16_t *reading, + TickType_t blockTime) { + if (reading == NULL) { + return OBC_ERR_CODE_INVALID_ARG; + } + + if (adcConversionMutex == NULL) { + return OBC_ERR_CODE_INVALID_STATE; + } + + adcData_t adcData[MAX_GROUP_SIZE]; + + obc_error_code_t errCode; + RETURN_IF_ERROR_CODE(adcGetGroupReadings(adc, group, adcData, blockTime)); + + uint8_t index = ADC_INDEX(adc, group); + uint32_t groupSize = adcGroupLengths[index]; + + for (uint32_t i = 0; i < groupSize; i++) { + if (adcData[i].id == channel) { + *reading = adcData[i].value; + return OBC_ERR_CODE_SUCCESS; + } + } + + return OBC_ERR_CODE_ADC_INVALID_CHANNEL; +} + +obc_error_code_t adcGetGroupData(adc_module_t adc, adc_group_t group, uint16_t *readings, TickType_t blockTime) { + if (readings == NULL) { + return OBC_ERR_CODE_INVALID_ARG; + } + + if (adcConversionMutex == NULL) { + return OBC_ERR_CODE_INVALID_STATE; + } + + adcData_t adcData[MAX_GROUP_SIZE]; + + obc_error_code_t errCode; + RETURN_IF_ERROR_CODE(adcGetGroupReadings(adc, group, adcData, blockTime)); + + uint8_t index = ADC_INDEX(adc, group); + uint32_t groupSize = adcGroupLengths[index]; + + for (uint32_t i = 0; i < groupSize; i++) { + readings[i] = adcData[i].value; + } + + return OBC_ERR_CODE_SUCCESS; +} diff --git a/obc/shared/config/peripheral_config_definitions.cmake b/obc/shared/config/peripheral_config_definitions.cmake index 15948c92d..9a6466c4e 100644 --- a/obc/shared/config/peripheral_config_definitions.cmake +++ b/obc/shared/config/peripheral_config_definitions.cmake @@ -1,19 +1,19 @@ -# board_config_definitions.cmake - -# Add Peripheral Configs - -# Add setup specific periperal configs globally here -# Ex: if you connected vn100, add -DCONFIG_VN100 inside set -set(COMMON_PERIPHERAL_CONFIG) - -# Board specifc configs - these should generally be kept as is unless the board is missing a component -if(BOARD_TYPE STREQUAL "RM46_LAUNCHPAD") - set(PERIPHERAL_CONFIG ${COMMON_PERIPHERAL_CONFIG}) -elseif(BOARD_TYPE STREQUAL "OBC_REVISION_1") - set(PERIPHERAL_CONFIG ${COMMON_PERIPHERAL_CONFIG} -DCONFIG_DS3232 -DCONFIG_SD_CARD -DCONFIG_FRAM -DCONFIG_LM75BD) -elseif(BOARD_TYPE STREQUAL "OBC_REVISION_2") - set(PERIPHERAL_CONFIG ${COMMON_PERIPHERAL_CONFIG} -DCONFIG_DS3232 -DCONFIG_SD_CARD -DCONFIG_FRAM -DCONFIG_LM75BD -DCONFIG_TPL5010) - -else() - message(FATAL_ERROR "Invalid BOARD_TYPE: ${BOARD_TYPE}. Board config not defined") -endif() +# board_config_definitions.cmake + +# Add Peripheral Configs + +# Add setup specific periperal configs globally here +# Ex: if you connected vn100, add -DCONFIG_VN100 inside set +set(COMMON_PERIPHERAL_CONFIG) + +# Board specifc configs - these should generally be kept as is unless the board is missing a component +if(BOARD_TYPE STREQUAL "RM46_LAUNCHPAD") + set(PERIPHERAL_CONFIG ${COMMON_PERIPHERAL_CONFIG}) +elseif(BOARD_TYPE STREQUAL "OBC_REVISION_1") + set(PERIPHERAL_CONFIG ${COMMON_PERIPHERAL_CONFIG} -DCONFIG_DS3232 -DCONFIG_SD_CARD -DCONFIG_FRAM -DCONFIG_LM75BD) +elseif(BOARD_TYPE STREQUAL "OBC_REVISION_2") + set(PERIPHERAL_CONFIG ${COMMON_PERIPHERAL_CONFIG} -DCONFIG_DS3232 -DCONFIG_SD_CARD -DCONFIG_FRAM -DCONFIG_LM75BD -DCONFIG_TPL5010) + +else() + message(FATAL_ERROR "Invalid BOARD_TYPE: ${BOARD_TYPE}. Board config not defined") +endif() diff --git a/python_test/test_ephemeris_parser.py b/python_test/test_ephemeris_parser.py index 144c28b99..be2650509 100644 --- a/python_test/test_ephemeris_parser.py +++ b/python_test/test_ephemeris_parser.py @@ -1,74 +1,74 @@ -import os -import struct - -import pytest -from gs.backend.sun import ephemeris -from gs.backend.sun import ephemeris_parser as ep - - -@pytest.mark.parametrize( - "ephermeris_data_type, is_float, expected", - [ - (ephemeris.DATA_FLOAT, True, 2451545.0), - (ephemeris.DATA_DOUBLE, False, 2451545.0), - (ephemeris.DATA_FLOAT, True, 20.0), - (ephemeris.DATA_DOUBLE, False, 20.0), - ], -) -def test_get_single_data_point(ephermeris_data_type, is_float, expected): - filename = "test_get_single_data_point_float.bin" - - # Write a float or double to the file - with open(filename, "wb") as f: - f.write(struct.pack(ephermeris_data_type, expected)) - - # Read the float or double from the file - with open(filename, "rb") as f: - f.seek(0) - value = ep.get_single_data_point(f, is_float) - - os.remove(filename) - assert value == expected - - -@pytest.mark.parametrize("expected", [(ep.Header(2451545.0, 20.0, 1000)), (ep.Header(5.0, 20.0, 4))]) -def test_parse_header(expected): - filename = "test_parse_header.bin" - - # Write header to file - with open(filename, "wb") as f: - f.write(struct.pack(ephemeris.DATA_DOUBLE, expected.start_time)) - f.write(struct.pack(ephemeris.DATA_DOUBLE, expected.step_size)) - f.write(struct.pack(ephemeris.DATA_UINT, expected.num_data_points)) - - # Read the header from the file and delete the file - actual = ep.parse_header(filename) - os.remove(filename) - - assert actual == expected - - -def test_parse_file(): - file = "test_parse_file.bin" - - # Write header to file - # Tested in test_write_header() - ephemeris.write_header(file, 0, 1, 3) - - # Data points to be written to the file - data_points_actual = [ - ephemeris.DataPoint(0, 1, 2, 3), - ephemeris.DataPoint(1, 4, 5, 6), - ephemeris.DataPoint(2, 7, 8, 9), - ] - - with open(file, "ab") as f: - for data_point in data_points_actual: - # Tested in test_write_data() - ephemeris.write_data(data_point, f) - - # Read the data points from the file and delete the file - data_points_expected = ep.parse_file(file) - os.remove(file) - - assert data_points_actual == data_points_expected +import os +import struct + +import pytest +from gs.backend.sun import ephemeris +from gs.backend.sun import ephemeris_parser as ep + + +@pytest.mark.parametrize( + "ephermeris_data_type, is_float, expected", + [ + (ephemeris.DATA_FLOAT, True, 2451545.0), + (ephemeris.DATA_DOUBLE, False, 2451545.0), + (ephemeris.DATA_FLOAT, True, 20.0), + (ephemeris.DATA_DOUBLE, False, 20.0), + ], +) +def test_get_single_data_point(ephermeris_data_type, is_float, expected): + filename = "test_get_single_data_point_float.bin" + + # Write a float or double to the file + with open(filename, "wb") as f: + f.write(struct.pack(ephermeris_data_type, expected)) + + # Read the float or double from the file + with open(filename, "rb") as f: + f.seek(0) + value = ep.get_single_data_point(f, is_float) + + os.remove(filename) + assert value == expected + + +@pytest.mark.parametrize("expected", [(ep.Header(2451545.0, 20.0, 1000)), (ep.Header(5.0, 20.0, 4))]) +def test_parse_header(expected): + filename = "test_parse_header.bin" + + # Write header to file + with open(filename, "wb") as f: + f.write(struct.pack(ephemeris.DATA_DOUBLE, expected.start_time)) + f.write(struct.pack(ephemeris.DATA_DOUBLE, expected.step_size)) + f.write(struct.pack(ephemeris.DATA_UINT, expected.num_data_points)) + + # Read the header from the file and delete the file + actual = ep.parse_header(filename) + os.remove(filename) + + assert actual == expected + + +def test_parse_file(): + file = "test_parse_file.bin" + + # Write header to file + # Tested in test_write_header() + ephemeris.write_header(file, 0, 1, 3) + + # Data points to be written to the file + data_points_actual = [ + ephemeris.DataPoint(0, 1, 2, 3), + ephemeris.DataPoint(1, 4, 5, 6), + ephemeris.DataPoint(2, 7, 8, 9), + ] + + with open(file, "ab") as f: + for data_point in data_points_actual: + # Tested in test_write_data() + ephemeris.write_data(data_point, f) + + # Read the data points from the file and delete the file + data_points_expected = ep.parse_file(file) + os.remove(file) + + assert data_points_actual == data_points_expected