From 1b18e5b61582e5997c9a6becd0641171fa365ab2 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:06:30 -0600 Subject: [PATCH 01/10] Format the reaction wheel null space algorithm --- .../rwNullSpace/rwNullSpaceAlgorithm.cpp | 32 +++++++++++-------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp b/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp index 67044c5375..5ed12b4a7c 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp +++ b/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp @@ -2,6 +2,7 @@ // Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder #include "rwNullSpaceAlgorithm.h" + #include #include @@ -11,8 +12,8 @@ @return void @param rwConfigInMsg Reaction Wheel constellation input message */ -void RwNullSpaceAlgorithm::reset(RWConstellationMsgPayload& rwConfigInMsg) { - this->numWheels = (uint32_t)rwConfigInMsg.numRW; +void RwNullSpaceAlgorithm::reset(RWConstellationMsgPayload &rwConfigInMsg) { + this->numWheels = (uint32_t) rwConfigInMsg.numRW; Eigen::Matrix G_s_B{}; G_s_B.setZero(); @@ -21,8 +22,8 @@ void RwNullSpaceAlgorithm::reset(RWConstellationMsgPayload& rwConfigInMsg) { } /* find the [tau] null space projection matrix [tau] = ([I] - [Gs]^T.([Gs].[Gs]^T)^-1.[Gs]) */ - this->tau = Eigen::Matrix::Identity() - - G_s_B.transpose() * (G_s_B * G_s_B.transpose()).inverse() * G_s_B; + this->tau = Eigen::Matrix::Identity() + - G_s_B.transpose() * (G_s_B * G_s_B.transpose()).inverse() * G_s_B; } /*! This method takes the input reaction wheel commands as well as the observed @@ -33,15 +34,18 @@ void RwNullSpaceAlgorithm::reset(RWConstellationMsgPayload& rwConfigInMsg) { @param rwSpeeds array of wheel speeds @param rwDesiredSpeeds array of desired wheel speeds */ -RwMotorTorqueMsgPayload RwNullSpaceAlgorithm::update(RwMotorTorqueMsgPayload& controlRequest, - RWSpeedMsgPayload& rwSpeeds, - RWSpeedMsgPayload& rwDesiredSpeeds) { +RwMotorTorqueMsgPayload RwNullSpaceAlgorithm::update( + RwMotorTorqueMsgPayload &controlRequest, + RWSpeedMsgPayload &rwSpeeds, + RWSpeedMsgPayload &rwDesiredSpeeds +) { RwMotorTorqueMsgPayload finalControl{}; /* [Nm] array of final RW motor torques containing both the control and null motion torques */ /* compute the wheel speed control vector d = -K.DeltaOmega */ - Eigen::Vector d = -this->omegaGain * (cArrayToEigenVector(rwSpeeds.wheelSpeeds) - - cArrayToEigenVector(rwDesiredSpeeds.wheelSpeeds)); + Eigen::Vector d = + -this->omegaGain + * (cArrayToEigenVector(rwSpeeds.wheelSpeeds) - cArrayToEigenVector(rwDesiredSpeeds.wheelSpeeds)); /* compute the RW null space motor torque solution to reduce the wheel speeds */ Eigen::Vector motorTorqueNullSpace = this->tau * d; @@ -59,10 +63,8 @@ RwMotorTorqueMsgPayload RwNullSpaceAlgorithm::update(RwMotorTorqueMsgPayload& co * @brief Set the gain used for the wheel speed difference. * @param gain The gain used for the wheel speed difference. */ -void RwNullSpaceAlgorithm::setOmegaGain(const double gain) { - if (gain < 0.0) { - throw std::invalid_argument("Feedback gain must not be negative"); - } +void RwNullSpaceAlgorithm::setOmegaGain(double const gain) { + if (gain < 0.0) { throw std::invalid_argument("Feedback gain must not be negative"); } this->omegaGain = gain; } @@ -70,4 +72,6 @@ void RwNullSpaceAlgorithm::setOmegaGain(const double gain) { * @brief Get the gain used for the wheel speed difference. * @return double The gain used for the wheel speed difference. */ -double RwNullSpaceAlgorithm::getOmegaGain() const { return this->omegaGain; } +double RwNullSpaceAlgorithm::getOmegaGain() const { + return this->omegaGain; +} From 45485fc4ec35bd9e6a88e9b790b6a628b59edf4a Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:06:52 -0600 Subject: [PATCH 02/10] Size the null space solution by the reaction wheel count --- .../effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp b/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp index 5ed12b4a7c..a9fd41860b 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp +++ b/src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpaceAlgorithm.cpp @@ -43,15 +43,15 @@ RwMotorTorqueMsgPayload RwNullSpaceAlgorithm::update( the control and null motion torques */ /* compute the wheel speed control vector d = -K.DeltaOmega */ - Eigen::Vector d = + Eigen::Vector d = -this->omegaGain * (cArrayToEigenVector(rwSpeeds.wheelSpeeds) - cArrayToEigenVector(rwDesiredSpeeds.wheelSpeeds)); /* compute the RW null space motor torque solution to reduce the wheel speeds */ - Eigen::Vector motorTorqueNullSpace = this->tau * d; + Eigen::Vector motorTorqueNullSpace = this->tau * d; /* add the null motion RW torque solution to the RW feedback control torque solution */ - Eigen::Vector motorTorque = + Eigen::Vector motorTorque = motorTorqueNullSpace + cArrayToEigenVector(controlRequest.motorTorque); eigenVectorToCArray(motorTorque, finalControl.motorTorque); From 7afcb6d8673baa99f46f8b3da851446d36446755 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:00 -0600 Subject: [PATCH 03/10] Add a mission parameters target to the build --- src/CMakeLists.txt | 4 ++ src/architecture/CMakeLists.txt | 4 ++ src/cmake/XmeraFramework.cmake | 74 +++++++++++++++++++++++++++++++ src/defaults/mission/parameters.h | 28 ++++++++++++ 4 files changed, 110 insertions(+) create mode 100644 src/defaults/mission/parameters.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bf9af0c974..031427daac 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -75,6 +75,10 @@ foreach(d IN LISTS XMERA_MODULE_ROOTS) add_subdirectory("${d}") endforeach() +# This call comes after the module roots, because a module root can declare its own mission +# parameters. +xmera_resolve_mission_parameters() + xmera_generate_messaging_init() install(DIRECTORY utilities DESTINATION xmera diff --git a/src/architecture/CMakeLists.txt b/src/architecture/CMakeLists.txt index 1705e753b6..5d0c6cd6db 100644 --- a/src/architecture/CMakeLists.txt +++ b/src/architecture/CMakeLists.txt @@ -15,6 +15,10 @@ target_include_directories(xmera_core PRIVATE "${CMAKE_SOURCE_DIR}" ) +# The link is PUBLIC, thus every target that uses the core also gets the mission include +# directory. Other libraries and test executables do not declare the directory again. +target_link_libraries(xmera_core PUBLIC Xmera::MissionParameters) + install(TARGETS xmera_core DESTINATION lib) add_library(Xmera::Core ALIAS xmera_core) diff --git a/src/cmake/XmeraFramework.cmake b/src/cmake/XmeraFramework.cmake index fe28f90c76..0684c9fecd 100644 --- a/src/cmake/XmeraFramework.cmake +++ b/src/cmake/XmeraFramework.cmake @@ -16,6 +16,67 @@ set(XMERA_ENABLE_INTERNAL "NO" "Whether to enable modules that are marked as INTERNAL (default NO)" ) +define_property(GLOBAL PROPERTY XMERA_MISSION_PARAMETERS_PROVIDER BRIEF_DOCS + "This property is the one directory that xmera_provide_mission_parameters() declares. That \ +function rejects a second, different directory. Thus this property has a maximum of one value." +) + +# Every target that uses a mission-sized array bound must link to this target. This target is the +# one place that gives the mission include directory. A new target that does not link to it does +# not compile. +add_library(xmera_mission_parameters INTERFACE) +add_library(Xmera::MissionParameters ALIAS xmera_mission_parameters) + +# Declare that contains the mission/parameters.h that this build must use. A module root +# calls this function from its own CMakeLists.txt. Thus the module root and its mission parameters +# are one selection and not two. +# +# This function rejects a second, different directory immediately. Thus the include path does not +# get two mission headers at the same time. Two mission headers that do not agree cause an ABI +# mismatch, and the build gives no error message. This mechanism prevents that failure. The check +# is in this function, thus it also applies to a build that does not call +# xmera_resolve_mission_parameters(). A second declaration of the same directory is permitted and +# has no effect. +function(xmera_provide_mission_parameters dir) + get_filename_component(_dir "${dir}" ABSOLUTE) + if(NOT EXISTS "${_dir}/mission/parameters.h") + message(FATAL_ERROR + "xmera_provide_mission_parameters(): there is no mission/parameters.h in '${_dir}'." + ) + endif() + + get_property(_existing GLOBAL PROPERTY XMERA_MISSION_PARAMETERS_PROVIDER) + if(_existing) + if(NOT _existing STREQUAL _dir) + message(FATAL_ERROR + "The build declares two sources of mission/parameters.h:\n ${_existing}\n ${_dir}\n" + "Only one source is permitted. If the build has two sources, different targets compile " + "against different array bounds. Remove a module root. Or make the two headers into one " + "mission header." + ) + endif() + return() + endif() + + set_property(GLOBAL PROPERTY XMERA_MISSION_PARAMETERS_PROVIDER "${_dir}") + target_include_directories(xmera_mission_parameters INTERFACE "${_dir}") +endfunction() + +# Call this function one time, after the build adds every module root. At that time there is a +# maximum of one provider. Thus this function only gives the default values when no module root +# declared a directory. +function(xmera_resolve_mission_parameters) + get_property(_provider GLOBAL PROPERTY XMERA_MISSION_PARAMETERS_PROVIDER) + + if(_provider) + message(STATUS "Mission parameters: ${_provider}/mission/parameters.h") + else() + xmera_provide_mission_parameters("${CMAKE_SOURCE_DIR}/defaults") + message(STATUS "Mission parameters: Xmera default values " + "(${CMAKE_SOURCE_DIR}/defaults/mission/parameters.h)") + endif() +endfunction() + if(APPLE) set(XMERA_RPATH_ORIGIN "@loader_path") else() @@ -106,6 +167,14 @@ function(xmera_add_swig_module module) "${CMAKE_SOURCE_DIR}/architecture/_GeneralModuleFiles" ) + # The directory is BEFORE the others, thus the compiler always reads the header of the mission + # and not an in-tree src/mission/. A generator expression is necessary here, because module + # roots declare their directory after CMake calls this function. The SWIG command line above + # also uses this directory, because SWIG reads INCLUDE_DIRECTORIES. + target_include_directories("${module}" BEFORE PRIVATE + "$" + ) + target_link_libraries("${module}" PRIVATE Xmera::Core Python3::Module @@ -213,6 +282,11 @@ function(xmera_add_swig_message message) "${CMAKE_SOURCE_DIR}" ) + # Refer to the note in xmera_add_swig_module(). + target_include_directories("${message}" BEFORE PRIVATE + "$" + ) + target_link_libraries("${message}" PRIVATE Python3::Module Eigen3::Eigen diff --git a/src/defaults/mission/parameters.h b/src/defaults/mission/parameters.h new file mode 100644 index 0000000000..65eba33545 --- /dev/null +++ b/src/defaults/mission/parameters.h @@ -0,0 +1,28 @@ +#ifndef MISSION_PARAMETERS_H +#define MISSION_PARAMETERS_H + +// These are the default mission parameters. The build uses them when no module root declares its +// own parameters with xmera_provide_mission_parameters(). A mission supplies its own header, and +// that header must give a value for every constant. The build does not add these values to that +// header. Refer to docs/source/learn/making-modules/messaging/mission-parameters.rst. + +// SWIG also reads this header, to give the constants to the Python layer. Its preprocessor reads +// the quote in a digit separator, for example 5'000, as the start of a character literal. SWIG +// then discards every constant after that quote, and it gives no error message. The +// clang-format off guard makes sure that IntegerLiteralSeparator does not add the separators +// again. + +// clang-format off +#define MAX_KEY_POINTS 5000 +#define MAX_NUM_CSS_SENSORS 32 +#define MAX_EFF_CNT 36 +#define RW_EFF_CNT 36 + +#define MAX_SICP_POINTS 5000 +#define SICP_POINT_DIM 3 +#define MAX_SICP_ITERATIONS 250 + +#define MAX_NUMBER_REGIONS 3 +// clang-format on + +#endif // MISSION_PARAMETERS_H From 20e2de015416301589b81348fc6d9341bbdc109b Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:27 -0600 Subject: [PATCH 04/10] Move the mission parameters out of the message definitions header --- docs/generate_msg_payload_rst.py | 2 +- .../msgAutoSource/msgInterfacePy.i.in | 4 ++- src/architecture/messaging/newMessaging.ih | 2 +- .../ArrayEffectorLockMsgPayload.h | 2 +- .../msgPayloadDef/ArrayMotorForceMsgPayload.h | 2 +- .../ArrayMotorTorqueMsgPayload.h | 2 +- .../msgPayloadDef/CSSArraySensorMsgPayload.h | 2 +- .../msgPayloadDef/CSSConfigMsgPayload.h | 3 +- .../msgPayloadDef/MTBArrayConfigMsgPayload.h | 2 +- .../msgPayloadDef/MTBCmdMsgPayload.h | 2 +- .../msgPayloadDef/PairedKeyPointsMsgPayload.h | 2 +- .../msgPayloadDef/PointCloudMsgPayload.h | 3 +- .../msgPayloadDef/RWArrayConfigMsgPayload.h | 2 +- .../msgPayloadDef/RWAvailabilityMsgPayload.h | 3 +- .../msgPayloadDef/RWConstellationMsgPayload.h | 3 +- .../msgPayloadDef/RWSpeedMsgPayload.h | 2 +- .../RegionsIdentifiedMsgPayload.h | 2 +- .../msgPayloadDef/RwMotorTorqueMsgPayload.h | 2 +- .../msgPayloadDef/RwMotorVoltageMsgPayload.h | 2 +- .../msgPayloadDef/SICPMsgPayload.h | 3 +- .../THRArrayCmdForceMsgPayload.h | 2 +- .../msgPayloadDef/THRArrayConfigMsgPayload.h | 3 +- .../THRArrayOnTimeCmdMsgPayload.h | 2 +- .../VSCMGArrayTorqueMsgPayload.h | 2 +- .../msgPayloadDef/VSCMGSpeedMsgPayload.h | 2 +- src/architecture/msgPayloadDef/definitions.h | 15 -------- .../mtbMomentumManagement.h | 7 ++-- .../mtbMomentumManagementSimple.h | 5 +-- .../_GeneralModuleFiles/thrustGroupData.h | 3 +- .../thrFiringRemainderAlgorithm_C.h | 35 +++++++++--------- .../thrFiringSchmittAlgorithm.h | 18 +++++----- .../regionsOfInterestAlgorithm.h | 25 +++++++------ .../regionsOfInterest/tests/CMakeLists.txt | 2 ++ .../SICP/scalingIterativeClosestPoint.h | 36 ++++++++++--------- .../initializeICP/initializeICP.h | 16 ++++----- .../deviceInterface/encoder/encoder.h | 9 ++--- 36 files changed, 116 insertions(+), 113 deletions(-) delete mode 100644 src/architecture/msgPayloadDef/definitions.h diff --git a/docs/generate_msg_payload_rst.py b/docs/generate_msg_payload_rst.py index 738f7be811..84522cde0a 100644 --- a/docs/generate_msg_payload_rst.py +++ b/docs/generate_msg_payload_rst.py @@ -23,7 +23,7 @@ def extract_struct_name(header_path): def generate_payload_rst(header_path, output_dir): """Generate a single RST file for a message payload header.""" struct_name = extract_struct_name(header_path) - # Skip headers that don't define a Payload struct (e.g., definitions.h) + # Ignore headers that do not define a Payload struct if not struct_name.endswith("Payload"): return None, struct_name diff --git a/src/architecture/messaging/msgAutoSource/msgInterfacePy.i.in b/src/architecture/messaging/msgAutoSource/msgInterfacePy.i.in index c592072eb4..88030bb973 100644 --- a/src/architecture/messaging/msgAutoSource/msgInterfacePy.i.in +++ b/src/architecture/messaging/msgAutoSource/msgInterfacePy.i.in @@ -23,7 +23,9 @@ %template(DoubleVector) std::vector>; %template(StringVector) std::vector>; -%include +// SWIG does not read #include directives. Thus this file must %include the mission parameters +// for the constants to get to the Python layer. +%include %include "fswAlgorithms/fswUtilities/fswDefinitions.h" %include "simulation/dynamics/reactionWheels/reactionWheelSupport.h" ARRAYINTASLIST(FSWdeviceAvailability) diff --git a/src/architecture/messaging/newMessaging.ih b/src/architecture/messaging/newMessaging.ih index 840745c7dd..5720bd85a0 100644 --- a/src/architecture/messaging/newMessaging.ih +++ b/src/architecture/messaging/newMessaging.ih @@ -3,7 +3,7 @@ %}; %{ #include "architecture/_GeneralModuleFiles/sys_model.h" -#include "architecture/msgPayloadDef/definitions.h" +#include #include "fswAlgorithms/fswUtilities/fswDefinitions.h" #include %} diff --git a/src/architecture/msgPayloadDef/ArrayEffectorLockMsgPayload.h b/src/architecture/msgPayloadDef/ArrayEffectorLockMsgPayload.h index ddd5bbe83d..66746f6217 100644 --- a/src/architecture/msgPayloadDef/ArrayEffectorLockMsgPayload.h +++ b/src/architecture/msgPayloadDef/ArrayEffectorLockMsgPayload.h @@ -5,7 +5,7 @@ #ifndef ARRAY_EFFECTOR_LOCK_H #define ARRAY_EFFECTOR_LOCK_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for vehicle effectors*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/ArrayMotorForceMsgPayload.h b/src/architecture/msgPayloadDef/ArrayMotorForceMsgPayload.h index f489dd1369..018914568e 100644 --- a/src/architecture/msgPayloadDef/ArrayMotorForceMsgPayload.h +++ b/src/architecture/msgPayloadDef/ArrayMotorForceMsgPayload.h @@ -5,7 +5,7 @@ #ifndef ARRAY_MOTOR_FORCE_H #define ARRAY_MOTOR_FORCE_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for vehicle effectors*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/ArrayMotorTorqueMsgPayload.h b/src/architecture/msgPayloadDef/ArrayMotorTorqueMsgPayload.h index 903f4d998c..c46e6ef521 100644 --- a/src/architecture/msgPayloadDef/ArrayMotorTorqueMsgPayload.h +++ b/src/architecture/msgPayloadDef/ArrayMotorTorqueMsgPayload.h @@ -5,7 +5,7 @@ #ifndef ARRAY_MOTOR_TORQUE_H #define ARRAY_MOTOR_TORQUE_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for vehicle effectors*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/CSSArraySensorMsgPayload.h b/src/architecture/msgPayloadDef/CSSArraySensorMsgPayload.h index 78fbba2db8..0555dcc5a0 100644 --- a/src/architecture/msgPayloadDef/CSSArraySensorMsgPayload.h +++ b/src/architecture/msgPayloadDef/CSSArraySensorMsgPayload.h @@ -5,7 +5,7 @@ #ifndef CSS_ARRAY_SENSOR_MESSAGE_H #define CSS_ARRAY_SENSOR_MESSAGE_H -#include "definitions.h" +#include /*! @brief Output structure for CSS array or constellation interface. Each element contains the raw measurement which * should be a cosine value nominally */ diff --git a/src/architecture/msgPayloadDef/CSSConfigMsgPayload.h b/src/architecture/msgPayloadDef/CSSConfigMsgPayload.h index 6aeb71fe6a..1582bd227d 100644 --- a/src/architecture/msgPayloadDef/CSSConfigMsgPayload.h +++ b/src/architecture/msgPayloadDef/CSSConfigMsgPayload.h @@ -6,7 +6,8 @@ #define CSS_CONFIG_MESSAGE_H #include "architecture/msgPayloadDef/CSSUnitConfigMsgPayload.h" -#include "definitions.h" + +#include #include /*! @brief Structure used to contain the configuration information for diff --git a/src/architecture/msgPayloadDef/MTBArrayConfigMsgPayload.h b/src/architecture/msgPayloadDef/MTBArrayConfigMsgPayload.h index a4a129a56f..d287f8fa17 100644 --- a/src/architecture/msgPayloadDef/MTBArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDef/MTBArrayConfigMsgPayload.h @@ -5,7 +5,7 @@ #ifndef MTB_ARRAY_CONFIG_MSG_H #define MTB_ARRAY_CONFIG_MSG_H -#include "definitions.h" +#include /*! @brief magnetic torque bar array configuration msg */ typedef struct { diff --git a/src/architecture/msgPayloadDef/MTBCmdMsgPayload.h b/src/architecture/msgPayloadDef/MTBCmdMsgPayload.h index 54d9bab2c8..3de4c244c9 100644 --- a/src/architecture/msgPayloadDef/MTBCmdMsgPayload.h +++ b/src/architecture/msgPayloadDef/MTBCmdMsgPayload.h @@ -5,7 +5,7 @@ #ifndef MTB_CMD_MSG_H #define MTB_CMD_MSG_H -#include "definitions.h" +#include /*! @brief Message for magnetic torque bar dipole commands. */ typedef struct { diff --git a/src/architecture/msgPayloadDef/PairedKeyPointsMsgPayload.h b/src/architecture/msgPayloadDef/PairedKeyPointsMsgPayload.h index 4011c7a715..8a4b72f82f 100644 --- a/src/architecture/msgPayloadDef/PairedKeyPointsMsgPayload.h +++ b/src/architecture/msgPayloadDef/PairedKeyPointsMsgPayload.h @@ -5,7 +5,7 @@ #ifndef KEYPOINTSMSG_H #define KEYPOINTSMSG_H -#include +#include //!@brief Optical Navigation measurement message containing the matched key points between two images /*! This message is output by the optical flow module and contains the key points shared between the two image diff --git a/src/architecture/msgPayloadDef/PointCloudMsgPayload.h b/src/architecture/msgPayloadDef/PointCloudMsgPayload.h index 48914324b9..b0bf109efd 100644 --- a/src/architecture/msgPayloadDef/PointCloudMsgPayload.h +++ b/src/architecture/msgPayloadDef/PointCloudMsgPayload.h @@ -5,7 +5,8 @@ #ifndef POINTCLOUDMSG_H #define POINTCLOUDMSG_H -#include +#include + #include //!@brief N-D point cloud diff --git a/src/architecture/msgPayloadDef/RWArrayConfigMsgPayload.h b/src/architecture/msgPayloadDef/RWArrayConfigMsgPayload.h index 71cfee89db..f5b9a11784 100644 --- a/src/architecture/msgPayloadDef/RWArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDef/RWArrayConfigMsgPayload.h @@ -5,7 +5,7 @@ #ifndef RW_CONFIG_MESSAGE_H #define RW_CONFIG_MESSAGE_H -#include "definitions.h" +#include #include /*! @brief RW array configuration FSW msg */ diff --git a/src/architecture/msgPayloadDef/RWAvailabilityMsgPayload.h b/src/architecture/msgPayloadDef/RWAvailabilityMsgPayload.h index 784ed61761..125e74dc5e 100644 --- a/src/architecture/msgPayloadDef/RWAvailabilityMsgPayload.h +++ b/src/architecture/msgPayloadDef/RWAvailabilityMsgPayload.h @@ -5,9 +5,10 @@ #ifndef _RW_AVAILABILITY_FSW_MSG_H #define _RW_AVAILABILITY_FSW_MSG_H -#include "definitions.h" #include "fswAlgorithms/fswUtilities/fswDefinitions.h" +#include + /*! @brief Array with availability of RW */ typedef struct { FSWdeviceAvailability wheelAvailability[RW_EFF_CNT]; //!< The current state of the wheel diff --git a/src/architecture/msgPayloadDef/RWConstellationMsgPayload.h b/src/architecture/msgPayloadDef/RWConstellationMsgPayload.h index 514ae8c5ba..e7c4d5db9a 100644 --- a/src/architecture/msgPayloadDef/RWConstellationMsgPayload.h +++ b/src/architecture/msgPayloadDef/RWConstellationMsgPayload.h @@ -6,7 +6,8 @@ #define _RW_CONSTELLATION_MESSAGE_H #include "RWConfigElementMsgPayload.h" -#include "definitions.h" + +#include /*! @brief Message used to define an array of RW FSW configurations */ typedef struct { diff --git a/src/architecture/msgPayloadDef/RWSpeedMsgPayload.h b/src/architecture/msgPayloadDef/RWSpeedMsgPayload.h index e8ea380974..1bd9301176 100644 --- a/src/architecture/msgPayloadDef/RWSpeedMsgPayload.h +++ b/src/architecture/msgPayloadDef/RWSpeedMsgPayload.h @@ -5,7 +5,7 @@ #ifndef RW_SPEED_MESSAGE_STRUCT_H #define RW_SPEED_MESSAGE_STRUCT_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for reaction wheel speeds*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/RegionsIdentifiedMsgPayload.h b/src/architecture/msgPayloadDef/RegionsIdentifiedMsgPayload.h index 26b41706ee..54e5a56c70 100644 --- a/src/architecture/msgPayloadDef/RegionsIdentifiedMsgPayload.h +++ b/src/architecture/msgPayloadDef/RegionsIdentifiedMsgPayload.h @@ -1,7 +1,7 @@ #ifndef REGIONS_IDENTIFIED_H #define REGIONS_IDENTIFIED_H -#include "definitions.h" +#include /*! @brief Regions of interest extracted by camera */ typedef struct { diff --git a/src/architecture/msgPayloadDef/RwMotorTorqueMsgPayload.h b/src/architecture/msgPayloadDef/RwMotorTorqueMsgPayload.h index da43169dd3..ff8619ee7f 100644 --- a/src/architecture/msgPayloadDef/RwMotorTorqueMsgPayload.h +++ b/src/architecture/msgPayloadDef/RwMotorTorqueMsgPayload.h @@ -5,7 +5,7 @@ #ifndef RW_MOTOR_TORQUE_MESSAGE_H #define RW_MOTOR_TORQUE_MESSAGE_H -#include "definitions.h" +#include /*! @brief Structure used to define the message format of the motor torque */ typedef struct { diff --git a/src/architecture/msgPayloadDef/RwMotorVoltageMsgPayload.h b/src/architecture/msgPayloadDef/RwMotorVoltageMsgPayload.h index f67b887dcd..28db8f3456 100644 --- a/src/architecture/msgPayloadDef/RwMotorVoltageMsgPayload.h +++ b/src/architecture/msgPayloadDef/RwMotorVoltageMsgPayload.h @@ -5,7 +5,7 @@ #ifndef SIM_RW_VOLTAGE_INPUT_H #define SIM_RW_VOLTAGE_INPUT_H -#include "definitions.h" +#include /*! @brief Structure used to define the message format of the motor voltage input */ typedef struct { diff --git a/src/architecture/msgPayloadDef/SICPMsgPayload.h b/src/architecture/msgPayloadDef/SICPMsgPayload.h index 414d529f04..e3239b8465 100644 --- a/src/architecture/msgPayloadDef/SICPMsgPayload.h +++ b/src/architecture/msgPayloadDef/SICPMsgPayload.h @@ -5,7 +5,8 @@ #ifndef SICP_H #define SICP_H -#include +#include + #include //!@brief Message containing the output of the Scaling Iterative Closest Point algorithm diff --git a/src/architecture/msgPayloadDef/THRArrayCmdForceMsgPayload.h b/src/architecture/msgPayloadDef/THRArrayCmdForceMsgPayload.h index af03d1f4de..61fe9a7a34 100644 --- a/src/architecture/msgPayloadDef/THRArrayCmdForceMsgPayload.h +++ b/src/architecture/msgPayloadDef/THRArrayCmdForceMsgPayload.h @@ -5,7 +5,7 @@ #ifndef THR_ARRAY_CMD_FORCE_MESSAGE_H_ #define THR_ARRAY_CMD_FORCE_MESSAGE_H_ -#include "definitions.h" +#include /*! @brief Message used to define a vector of thruster force commands */ typedef struct { diff --git a/src/architecture/msgPayloadDef/THRArrayConfigMsgPayload.h b/src/architecture/msgPayloadDef/THRArrayConfigMsgPayload.h index 1b650c895a..f422155c03 100644 --- a/src/architecture/msgPayloadDef/THRArrayConfigMsgPayload.h +++ b/src/architecture/msgPayloadDef/THRArrayConfigMsgPayload.h @@ -6,7 +6,8 @@ #define THR_ARRAY_MESSAGE_H #include "THRConfigMsgPayload.h" -#include "definitions.h" + +#include #include /*! @brief FSW message definition containing the thruster cluster information */ diff --git a/src/architecture/msgPayloadDef/THRArrayOnTimeCmdMsgPayload.h b/src/architecture/msgPayloadDef/THRArrayOnTimeCmdMsgPayload.h index d542fafd82..675e99fdf0 100644 --- a/src/architecture/msgPayloadDef/THRArrayOnTimeCmdMsgPayload.h +++ b/src/architecture/msgPayloadDef/THRArrayOnTimeCmdMsgPayload.h @@ -5,7 +5,7 @@ #ifndef THR_CMD_MESSAGE_H #define THR_CMD_MESSAGE_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for vehicle effectors*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/VSCMGArrayTorqueMsgPayload.h b/src/architecture/msgPayloadDef/VSCMGArrayTorqueMsgPayload.h index 743f0386ec..4f965b239b 100644 --- a/src/architecture/msgPayloadDef/VSCMGArrayTorqueMsgPayload.h +++ b/src/architecture/msgPayloadDef/VSCMGArrayTorqueMsgPayload.h @@ -5,7 +5,7 @@ #ifndef FSW_VSCMG_TORQUE_H #define FSW_VSCMG_TORQUE_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for vehicle effectors*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/VSCMGSpeedMsgPayload.h b/src/architecture/msgPayloadDef/VSCMGSpeedMsgPayload.h index 4d179196c9..3176164fdf 100644 --- a/src/architecture/msgPayloadDef/VSCMGSpeedMsgPayload.h +++ b/src/architecture/msgPayloadDef/VSCMGSpeedMsgPayload.h @@ -5,7 +5,7 @@ #ifndef VSCMG_SPEED_MESSAGE_STRUCT_H #define VSCMG_SPEED_MESSAGE_STRUCT_H -#include "definitions.h" +#include /*! @brief Structure used to define the output definition for VSCMG speeds*/ typedef struct { diff --git a/src/architecture/msgPayloadDef/definitions.h b/src/architecture/msgPayloadDef/definitions.h deleted file mode 100644 index 83e54f8d9c..0000000000 --- a/src/architecture/msgPayloadDef/definitions.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef MSG_DEFINITIONS_H -#define MSG_DEFINITIONS_H - -#define MAX_KEY_POINTS 5000 -#define MAX_NUM_CSS_SENSORS 32 -#define MAX_EFF_CNT 36 -#define RW_EFF_CNT 36 - -#define MAX_SICP_POINTS 5000 -#define SICP_POINT_DIM 3 -#define MAX_SICP_ITERATIONS 250 - -#define MAX_NUMBER_REGIONS 3 - -#endif // MSG_DEFINITIONS_H diff --git a/src/fswAlgorithms/attControl/mtbMomentumManagement/mtbMomentumManagement.h b/src/fswAlgorithms/attControl/mtbMomentumManagement/mtbMomentumManagement.h index 0cdb27b81d..8de99c40aa 100644 --- a/src/fswAlgorithms/attControl/mtbMomentumManagement/mtbMomentumManagement.h +++ b/src/fswAlgorithms/attControl/mtbMomentumManagement/mtbMomentumManagement.h @@ -10,14 +10,15 @@ #include #include #include -#include #include +#include #include -#include + +#include /*! @brief Top level structure for the sub-module routines. */ class MtbMomentumManagement : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; diff --git a/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/mtbMomentumManagementSimple.h b/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/mtbMomentumManagementSimple.h index 4ea06a64fc..da20d2e9cd 100644 --- a/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/mtbMomentumManagementSimple.h +++ b/src/fswAlgorithms/attControl/mtbMomentumManagementSimple/mtbMomentumManagementSimple.h @@ -10,12 +10,13 @@ #include #include #include -#include + +#include #include /*! @brief Top level structure for the sub-module routines. */ class MtbMomentumManagementSimple : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; diff --git a/src/fswAlgorithms/effectorInterfaces/_GeneralModuleFiles/thrustGroupData.h b/src/fswAlgorithms/effectorInterfaces/_GeneralModuleFiles/thrustGroupData.h index 760dd3ca59..25b0390dc1 100644 --- a/src/fswAlgorithms/effectorInterfaces/_GeneralModuleFiles/thrustGroupData.h +++ b/src/fswAlgorithms/effectorInterfaces/_GeneralModuleFiles/thrustGroupData.h @@ -7,7 +7,8 @@ #include #include -#include + +#include #include diff --git a/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder_C/thrFiringRemainderAlgorithm_C.h b/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder_C/thrFiringRemainderAlgorithm_C.h index 688fc41878..4799079b93 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder_C/thrFiringRemainderAlgorithm_C.h +++ b/src/fswAlgorithms/effectorInterfaces/thrFiringRemainder_C/thrFiringRemainderAlgorithm_C.h @@ -8,28 +8,27 @@ extern "C" { #endif -#include - -#include #include #include #include -typedef struct { - double pulseRemainder[MAX_EFF_CNT]; //!< [-] Unimplemented thrust pulses (number of minimum pulses) - double thrMinFireTime; //!< [s] Minimum fire time - uint32_t numThrusters; //!< [-] The number of thrusters available on vehicle - double maxThrust[MAX_EFF_CNT]; //!< [N] Max thrust - int baseThrustState; //!< [-] Indicates on-pulsing (0) or off-pulsing (1) - double defaultControlPeriod; //!< [s] Default control period used for first call - uint64_t prevCallTime; //!< callTime from previous function call -} ThrFiringRemainderInternalState; - -void reset(ThrFiringRemainderInternalState* moduleState, THRArrayConfigMsgPayload localThrusterData); - -THRArrayOnTimeCmdMsgPayload updateState(ThrFiringRemainderInternalState* moduleState, - uint64_t callTime, - THRArrayCmdForceMsgPayload thrForceIn); +#include +#include + + typedef struct { + double pulseRemainder[MAX_EFF_CNT]; //!< [-] Unimplemented thrust pulses (number of minimum pulses) + double thrMinFireTime; //!< [s] Minimum fire time + uint32_t numThrusters; //!< [-] The number of thrusters available on vehicle + double maxThrust[MAX_EFF_CNT]; //!< [N] Max thrust + int baseThrustState; //!< [-] Indicates on-pulsing (0) or off-pulsing (1) + double defaultControlPeriod; //!< [s] Default control period used for first call + uint64_t prevCallTime; //!< callTime from previous function call + } ThrFiringRemainderInternalState; + + void reset(ThrFiringRemainderInternalState* moduleState, THRArrayConfigMsgPayload localThrusterData); + + THRArrayOnTimeCmdMsgPayload + updateState(ThrFiringRemainderInternalState* moduleState, uint64_t callTime, THRArrayCmdForceMsgPayload thrForceIn); #ifdef __cplusplus } diff --git a/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/thrFiringSchmittAlgorithm.h b/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/thrFiringSchmittAlgorithm.h index c6cf832be0..2b731fe067 100644 --- a/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/thrFiringSchmittAlgorithm.h +++ b/src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/thrFiringSchmittAlgorithm.h @@ -5,21 +5,21 @@ #ifndef _THR_FIRING_SCHMITT_ALGORITHM_H #define _THR_FIRING_SCHMITT_ALGORITHM_H -#include - -#include - #include #include #include -#include + +#include +#include + +#include enum class PulsingRegime { ONPULSING = 0, OFFPULSING = 1 }; class ThrFiringSchmittAlgorithm { - public: - void reset(uint64_t callTime, THRArrayConfigMsgPayload const& thrusterConfigPayload); - THRArrayOnTimeCmdMsgPayload update(uint64_t callTime, THRArrayCmdForceMsgPayload& thrForceIn); +public: + void reset(uint64_t callTime, THRArrayConfigMsgPayload const &thrusterConfigPayload); + THRArrayOnTimeCmdMsgPayload update(uint64_t callTime, THRArrayCmdForceMsgPayload &thrForceIn); double getLevelOn() const; void setLevelOn(double level); double getLevelOff() const; @@ -29,7 +29,7 @@ class ThrFiringSchmittAlgorithm { PulsingRegime getPulsingRegime() const; void setPulsingRegime(PulsingRegime state); - private: +private: double levelOn{}; //!< [-] ON duty cycle fraction double levelOff{}; //!< [-] OFF duty cycle fraction double thrMinFireTime{}; //!< [s] Minimum ON time for thrusters diff --git a/src/fswAlgorithms/imageProcessing/regionsOfInterest/regionsOfInterestAlgorithm.h b/src/fswAlgorithms/imageProcessing/regionsOfInterest/regionsOfInterestAlgorithm.h index df3a030bd7..77c6ec4dc8 100644 --- a/src/fswAlgorithms/imageProcessing/regionsOfInterest/regionsOfInterestAlgorithm.h +++ b/src/fswAlgorithms/imageProcessing/regionsOfInterest/regionsOfInterestAlgorithm.h @@ -10,9 +10,10 @@ #ifndef _IMAGE_PROC_REGIONS_ALGORITHM_H_ #define _IMAGE_PROC_REGIONS_ALGORITHM_H_ +#include #include + #include -#include "architecture/msgPayloadDef/definitions.h" /** * @brief Structure representing a detected region of interest in an image @@ -40,11 +41,11 @@ struct RegionOfInterest { * detections. */ class RegionsOfInterestAlgorithm { - public: +public: RegionsOfInterestAlgorithm(); ~RegionsOfInterestAlgorithm(); - RegionOfInterest update(const std::array& regions) const; + RegionOfInterest update(std::array const ®ions) const; void reset(); void setMaxRoiSeparation(int32_t pixelSeparation); @@ -53,24 +54,26 @@ class RegionsOfInterestAlgorithm { int32_t getCameraId() const; void setMinimumDetectionSize(int32_t pixels); int32_t getMinimumDetectionSize() const; - void setWindowCenter(const Eigen::Vector2i& center); + void setWindowCenter(Eigen::Vector2i const ¢er); Eigen::Vector2i getWindowCenter() const; void setWindowSize(int32_t width, int32_t height); Eigen::Vector2i getWindowSize() const; void setImageSize(int32_t width, int32_t height); Eigen::Vector2i getImageSize() const; - private: +private: void computeWindow(); - bool regionInWindow(const RegionOfInterest& region) const; + bool regionInWindow(RegionOfInterest const ®ion) const; std::array applyWindow( - const std::array& regions) const; - RegionOfInterest identifyRoi(const std::array& regions) const; + std::array const ®ions + ) const; + RegionOfInterest identifyRoi(std::array const ®ions) const; static std::array orderRegions( - const std::array& regions); + std::array const ®ions + ); RegionOfInterest regionOfInterest{}; //!< [ROI] Final region of interest - Eigen::Vector2i imageSize = Eigen::Vector2i(1024, 1024); //!< [bool] Size the incoming image + Eigen::Vector2i imageSize = Eigen::Vector2i(1'024, 1'024); //!< [bool] Size the incoming image int32_t minDetectionPixel = 2; //!< [int] Minimum pixel detection size int32_t maxSeparation{500}; //!< [px] width of mask to be used for windowing int32_t cameraId{}; //!< [-] Id of the camera imaging @@ -78,7 +81,7 @@ class RegionsOfInterestAlgorithm { int32_t windowWidth{}; //!< [px] width of mask to be used for windowing int32_t windowHeight{}; //!< [px] height of mask to be used for windowing Eigen::Vector2i windowPointTopLeft = Eigen::Vector2i(0, 0); //!< [px] top left point of window - Eigen::Vector2i windowPointBottomRight = Eigen::Vector2i(1024, 1024); + Eigen::Vector2i windowPointBottomRight = Eigen::Vector2i(1'024, 1'024); ; //!< [px] bottom right point of window }; diff --git a/src/fswAlgorithms/imageProcessing/regionsOfInterest/tests/CMakeLists.txt b/src/fswAlgorithms/imageProcessing/regionsOfInterest/tests/CMakeLists.txt index f4ed76d72f..b7a49c959d 100644 --- a/src/fswAlgorithms/imageProcessing/regionsOfInterest/tests/CMakeLists.txt +++ b/src/fswAlgorithms/imageProcessing/regionsOfInterest/tests/CMakeLists.txt @@ -10,6 +10,7 @@ target_include_directories(test_regionsOfInterest PRIVATE "${CMAKE_SOURCE_DIR}") target_link_libraries(test_regionsOfInterest PRIVATE Eigen3::Eigen GTest::gtest_main + Xmera::MissionParameters ) gtest_discover_tests(test_regionsOfInterest) @@ -30,6 +31,7 @@ if(XMERA_ENABLE_FUZZTESTS) Eigen3::Eigen fuzztest::fuzztest fuzztest::fuzztest_gtest_main + Xmera::MissionParameters ) gtest_discover_tests(test_regionsOfInterest_fuzz diff --git a/src/fswAlgorithms/pointCloudProcessing/SICP/scalingIterativeClosestPoint.h b/src/fswAlgorithms/pointCloudProcessing/SICP/scalingIterativeClosestPoint.h index 5810076ff3..1b27e9a186 100644 --- a/src/fswAlgorithms/pointCloudProcessing/SICP/scalingIterativeClosestPoint.h +++ b/src/fswAlgorithms/pointCloudProcessing/SICP/scalingIterativeClosestPoint.h @@ -4,22 +4,22 @@ #ifndef _SICP_H_ #define _SICP_H_ +#include #include -#include -#include -#include - #include #include - -#include #include #include #include +#include +#include + +#include + /*! @brief Scaling iterative Closest Point Algorithm */ class ScalingIterativeClosestPoint : public SysModel { - public: +public: ScalingIterativeClosestPoint(); ~ScalingIterativeClosestPoint(); @@ -44,16 +44,18 @@ class ScalingIterativeClosestPoint : public SysModel { Eigen::MatrixXd t_init = Eigen::VectorXd::Zero(SICP_POINT_DIM); double s_init = 1; - private: - void computePointCorrespondance(const Eigen::MatrixXd& R_kmin1, - const Eigen::MatrixXd& t_kmin1, - const double s_kmin1, - const Eigen::MatrixXd& measuredPoints, - const Eigen::MatrixXd& referencePoints); - void centerCloud(const Eigen::MatrixXd& measuredPoints); - Eigen::MatrixXd computeRk(const double s_kmin1, const Eigen::MatrixXd& R_kmin1); - double computeSk(const Eigen::MatrixXd& R_kmin1); - Eigen::MatrixXd computeTk(const double s_k, const Eigen::MatrixXd& R_k, const Eigen::MatrixXd& measuredPoints); +private: + void computePointCorrespondance( + Eigen::MatrixXd const &R_kmin1, + Eigen::MatrixXd const &t_kmin1, + double const s_kmin1, + Eigen::MatrixXd const &measuredPoints, + Eigen::MatrixXd const &referencePoints + ); + void centerCloud(Eigen::MatrixXd const &measuredPoints); + Eigen::MatrixXd computeRk(double const s_kmin1, Eigen::MatrixXd const &R_kmin1); + double computeSk(Eigen::MatrixXd const &R_kmin1); + Eigen::MatrixXd computeTk(double const s_k, Eigen::MatrixXd const &R_k, Eigen::MatrixXd const &measuredPoints); PointCloudMsgPayload outputCloudBuffer; PointCloudMsgPayload measuredCloudBuffer; diff --git a/src/fswAlgorithms/pointCloudProcessing/initializeICP/initializeICP.h b/src/fswAlgorithms/pointCloudProcessing/initializeICP/initializeICP.h index adf5f79e7a..e2df1fc4a9 100644 --- a/src/fswAlgorithms/pointCloudProcessing/initializeICP/initializeICP.h +++ b/src/fswAlgorithms/pointCloudProcessing/initializeICP/initializeICP.h @@ -4,24 +4,24 @@ #ifndef _INITSICP_H_ #define _INITSICP_H_ +#include #include -#include -#include -#include - #include #include #include #include - -#include #include #include #include +#include +#include + +#include + /*! @brief Scaling iterative Closest Point Algorithm */ class InitializeICP : public SysModel { - public: +public: InitializeICP(); ~InitializeICP(); @@ -40,7 +40,7 @@ class InitializeICP : public SysModel { double maxTimeBetweenMeasurements = 600; bool normalizeMeasuredCloud = false; - private: +private: void normalizePointCloud(); void setInitialConditions(uint64_t currentSimNanos); void writeOutputMessages(uint64_t currentSimNanos); diff --git a/src/simulation/deviceInterface/encoder/encoder.h b/src/simulation/deviceInterface/encoder/encoder.h index 275eab89b9..8a1a713f1f 100644 --- a/src/simulation/deviceInterface/encoder/encoder.h +++ b/src/simulation/deviceInterface/encoder/encoder.h @@ -8,12 +8,13 @@ #include #include #include -#include #include +#include + /*! @brief wheel speed encoder module class */ class Encoder : public SysModel { - public: +public: Encoder(); ~Encoder(); @@ -23,7 +24,7 @@ class Encoder : public SysModel { void writeOutputMessages(uint64_t CurrentClock); void encode(uint64_t currentSimNanos); - public: +public: Message rwSpeedOutMsg; //!< [rad/s] reaction wheel speed output message ReadFunctor rwSpeedInMsg; //!< [rad/s] reaction wheel speed input message int rwSignalState[RW_EFF_CNT]; //!< vector of reaction wheel signal states @@ -31,7 +32,7 @@ class Encoder : public SysModel { int numRW; //!< number of reaction wheels BSKLogger bskLogger; //!< -- BSK Logging - private: +private: RWSpeedMsgPayload rwSpeedBuffer; //!< reaction wheel speed buffer for internal calculations RWSpeedMsgPayload rwSpeedConverted; //!< reaction wheel speed buffer for converted values double remainingClicks[RW_EFF_CNT]; //!< remaining clicks from the previous iteration From c8d6cfe08c7ae2f025a6d403ddd2c272cb90e87c Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:35 -0600 Subject: [PATCH 05/10] Size the sunline filter residuals by the sun sensor count --- .../msgPayloadDef/SunlineFilterMsgPayload.h | 5 +- .../attDetermination/CSSEst/cssWlsEst.cpp | 6 +- .../attDetermination/okeefeEKF/okeefeEKF.cpp | 70 +++++----- .../attDetermination/okeefeEKF/okeefeEKF.h | 129 ++++++++++-------- .../sunlineEKF/sunlineEKF.cpp | 70 +++++----- .../attDetermination/sunlineEKF/sunlineEKF.h | 121 ++++++++-------- .../sunlineSEKF/sunlineSEKF.cpp | 70 +++++----- .../sunlineSEKF/sunlineSEKF.h | 105 +++++++------- .../sunlineSuKF/sunlineSuKF.cpp | 29 ++-- .../sunlineSuKF/sunlineSuKF.h | 17 ++- .../sunlineUKF/sunlineUKF.cpp | 29 ++-- .../attDetermination/sunlineUKF/sunlineUKF.h | 15 +- 12 files changed, 348 insertions(+), 318 deletions(-) diff --git a/src/architecture/msgPayloadDef/SunlineFilterMsgPayload.h b/src/architecture/msgPayloadDef/SunlineFilterMsgPayload.h index dbac7997f6..5713387512 100644 --- a/src/architecture/msgPayloadDef/SunlineFilterMsgPayload.h +++ b/src/architecture/msgPayloadDef/SunlineFilterMsgPayload.h @@ -5,11 +5,12 @@ #ifndef SUNLINE_FILTER_MESSAGE_H #define SUNLINE_FILTER_MESSAGE_H +#include + #define SKF_N_STATES 6 #define SKF_N_STATES_SWITCH 6 #define EKF_N_STATES_SWITCH 5 #define SKF_N_STATES_HALF 3 -#define MAX_N_CSS_MEAS 32 /*! @brief structure for filter-states output for the unscented kalman filter implementation of the sunline state estimator*/ @@ -18,7 +19,7 @@ typedef struct { double covar[SKF_N_STATES * SKF_N_STATES]; //!< [-] Current covariance of the filter double state[SKF_N_STATES]; //!< [-] Current estimated state of the filter double stateError[SKF_N_STATES]; //!< [-] Current deviation of the state from the reference state - double postFitRes[MAX_N_CSS_MEAS]; //!< [-] PostFit Residuals + double postFitRes[MAX_NUM_CSS_SENSORS]; //!< [-] PostFit Residuals int numObs; //!< [-] Valid observation count for this frame } SunlineFilterMsgPayload; diff --git a/src/fswAlgorithms/attDetermination/CSSEst/cssWlsEst.cpp b/src/fswAlgorithms/attDetermination/CSSEst/cssWlsEst.cpp index b8e35c99fa..486a2a1100 100644 --- a/src/fswAlgorithms/attDetermination/CSSEst/cssWlsEst.cpp +++ b/src/fswAlgorithms/attDetermination/CSSEst/cssWlsEst.cpp @@ -28,11 +28,11 @@ void CssWlsEst::reset(uint64_t callTime) { } this->cssConfigInBuffer = this->cssConfigInMsg(); - if (this->cssConfigInBuffer.nCSS > MAX_N_CSS_MEAS) { + if (this->cssConfigInBuffer.nCSS > MAX_NUM_CSS_SENSORS) { this->bskLogger.bskLog( BSK_ERROR, "cssWIsEst.cssDataInMsg.nCSS must not be greater than " - "MAX_N_CSS_MEAS value." + "MAX_NUM_CSS_SENSORS value." ); } @@ -42,7 +42,7 @@ void CssWlsEst::reset(uint64_t callTime) { this->filtStatus.numObs = 0; this->filtStatus.timeTag = 0.0; v3SetZero(this->filtStatus.state); - vSetZero(this->filtStatus.postFitRes, MAX_N_CSS_MEAS); + vSetZero(this->filtStatus.postFitRes, MAX_NUM_CSS_SENSORS); /* Reset the prior time flag state. If zero, control time step not evaluated on the first function call */ diff --git a/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.cpp b/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.cpp index 6e2cf3b348..34a9ad2c6d 100644 --- a/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.cpp +++ b/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.cpp @@ -32,11 +32,11 @@ void OkeefeEKF::reset(uint64_t callTime) { /*! - Read in coarse sun sensor configuration information.*/ cssConfigInBuffer = this->cssConfigInMsg(); - if (cssConfigInBuffer.nCSS > MAX_N_CSS_MEAS) { + if (cssConfigInBuffer.nCSS > MAX_NUM_CSS_SENSORS) { this->bskLogger.bskLog( BSK_ERROR, "okeefeEKF.cssConfigInMsg.nCSS must not be greater than " - "MAX_N_CSS_MEAS value." + "MAX_NUM_CSS_SENSORS value." ); } @@ -52,7 +52,7 @@ void OkeefeEKF::reset(uint64_t callTime) { this->timeTag = callTime * NANO2SEC; this->dt = 0.0; this->numStates = SKF_N_STATES_HALF; - this->numObs = MAX_N_CSS_MEAS; + this->numObs = MAX_NUM_CSS_SENSORS; /*! - Ensure that all internal filter matrices are zeroed*/ vSetZero(this->obs, this->numObs); @@ -81,7 +81,7 @@ void OkeefeEKF::reset(uint64_t callTime) { */ void OkeefeEKF::updateState(uint64_t callTime) { double newTimeTag; - double Hx[MAX_N_CSS_MEAS]; + double Hx[MAX_NUM_CSS_SENSORS]; uint64_t timeOfMsgWritten; int isWritten; SunlineFilterMsgPayload sunlineDataOutBuffer; @@ -124,7 +124,7 @@ void OkeefeEKF::updateState(uint64_t callTime) { memmove(sunlineDataOutBuffer.covar, this->covar, SKF_N_STATES_HALF * SKF_N_STATES_HALF * sizeof(double)); memmove(sunlineDataOutBuffer.state, this->state, SKF_N_STATES * sizeof(double)); memmove(sunlineDataOutBuffer.stateError, this->x, SKF_N_STATES * sizeof(double)); - memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_N_CSS_MEAS * sizeof(double)); + memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_NUM_CSS_SENSORS * sizeof(double)); this->filtDataOutMsg.write(sunlineDataOutBuffer, this->moduleID, callTime); return; @@ -377,32 +377,32 @@ void sunlineDynMatrixOkeefe(double omega[SKF_N_STATES_HALF], double dt, double* void sunlineCKFUpdateOkeefe( double xBar[SKF_N_STATES_HALF], - double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS], + double kalmanGain[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS], double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], double qObsVal, int numObsInt, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], double* x, double* covar ) { - double measMatx[MAX_N_CSS_MEAS], innov[MAX_N_CSS_MEAS], kInnov[SKF_N_STATES_HALF]; + double measMatx[MAX_NUM_CSS_SENSORS], innov[MAX_NUM_CSS_SENSORS], kInnov[SKF_N_STATES_HALF]; double eye[SKF_N_STATES_HALF * SKF_N_STATES_HALF], kH[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; double eyeKalH[SKF_N_STATES_HALF * SKF_N_STATES_HALF], eyeKalHT[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; - double eyeKalHCovarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], kalR[SKF_N_STATES_HALF * MAX_N_CSS_MEAS]; - double kalT[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], kalRKalT[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; - double noiseMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double eyeKalHCovarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], kalR[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS]; + double kalT[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], kalRKalT[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; + double noiseMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; size_t numObs = (size_t) numObsInt; /* Set variables to zero */ mSetZero(kH, SKF_N_STATES_HALF, SKF_N_STATES_HALF); mSetZero(eyeKalH, SKF_N_STATES_HALF, SKF_N_STATES_HALF); mSetZero(eyeKalHT, SKF_N_STATES_HALF, SKF_N_STATES_HALF); - mSetZero(noiseMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(noiseMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mSetZero(eye, SKF_N_STATES_HALF, SKF_N_STATES_HALF); mSetZero(kalRKalT, SKF_N_STATES_HALF, SKF_N_STATES_HALF); - mSetZero(kalT, MAX_N_CSS_MEAS, SKF_N_STATES_HALF); - mSetZero(kalR, SKF_N_STATES_HALF, MAX_N_CSS_MEAS); + mSetZero(kalT, MAX_NUM_CSS_SENSORS, SKF_N_STATES_HALF); + mSetZero(kalR, SKF_N_STATES_HALF, MAX_NUM_CSS_SENSORS); mSetZero(eyeKalHCovarBar, SKF_N_STATES_HALF, SKF_N_STATES_HALF); /* Set noise matrix given number of observations */ @@ -459,32 +459,32 @@ void sunlineCKFUpdateOkeefe( @param covar Pointer to the covariance after update */ void okeefeEKFUpdate( - double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS], + double kalmanGain[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS], double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], double qObsVal, int numObsInt, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], double* states, double* x, double* covar ) { double eye[SKF_N_STATES_HALF * SKF_N_STATES_HALF], kH[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; double eyeKalH[SKF_N_STATES_HALF * SKF_N_STATES_HALF], eyeKalHT[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; - double eyeKalHCovarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], kalR[SKF_N_STATES_HALF * MAX_N_CSS_MEAS]; - double kalT[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], kalRKalT[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; - double noiseMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double eyeKalHCovarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], kalR[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS]; + double kalT[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], kalRKalT[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; + double noiseMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; size_t numObs = (size_t) numObsInt; /* Set variables to zero */ mSetZero(kH, SKF_N_STATES_HALF, SKF_N_STATES_HALF); mSetZero(eyeKalH, SKF_N_STATES_HALF, SKF_N_STATES_HALF); mSetZero(eyeKalHT, SKF_N_STATES_HALF, SKF_N_STATES_HALF); - mSetZero(noiseMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(noiseMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mSetZero(eye, SKF_N_STATES_HALF, SKF_N_STATES_HALF); mSetZero(kalRKalT, SKF_N_STATES_HALF, SKF_N_STATES_HALF); - mSetZero(kalT, MAX_N_CSS_MEAS, SKF_N_STATES_HALF); - mSetZero(kalR, SKF_N_STATES_HALF, MAX_N_CSS_MEAS); + mSetZero(kalT, MAX_NUM_CSS_SENSORS, SKF_N_STATES_HALF); + mSetZero(kalR, SKF_N_STATES_HALF, MAX_NUM_CSS_SENSORS); mSetZero(eyeKalHCovarBar, SKF_N_STATES_HALF, SKF_N_STATES_HALF); /* Set noise matrix given number of observations */ @@ -547,7 +547,7 @@ void okeefeEKFUpdate( void sunlineHMatrixYMeas( double states[SKF_N_STATES_HALF], size_t numCSS, - double cssSensorCos[MAX_N_CSS_MEAS], + double cssSensorCos[MAX_NUM_CSS_SENSORS], double sensorUseThresh, double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], double CBias[MAX_NUM_CSS_SENSORS], @@ -589,24 +589,24 @@ void sunlineHMatrixYMeas( void sunlineKalmanGainOkeefe( double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], double qObsVal, int numObsInt, double* kalmanGain ) { - double hObsT[SKF_N_STATES_HALF * MAX_N_CSS_MEAS]; - double covHT[SKF_N_STATES_HALF * MAX_N_CSS_MEAS]; - double hCovar[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], hCovarHT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double rMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double hObsT[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS]; + double covHT[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS]; + double hCovar[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], hCovarHT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double rMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; size_t numObs; numObs = (size_t) numObsInt; /* Setting all local variables to zero */ - mSetZero(hObsT, SKF_N_STATES_HALF, MAX_N_CSS_MEAS); - mSetZero(covHT, SKF_N_STATES_HALF, MAX_N_CSS_MEAS); - mSetZero(hCovar, MAX_N_CSS_MEAS, SKF_N_STATES_HALF); - mSetZero(hCovarHT, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); - mSetZero(rMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(hObsT, SKF_N_STATES_HALF, MAX_NUM_CSS_SENSORS); + mSetZero(covHT, SKF_N_STATES_HALF, MAX_NUM_CSS_SENSORS); + mSetZero(hCovar, MAX_NUM_CSS_SENSORS, SKF_N_STATES_HALF); + mSetZero(hCovarHT, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); + mSetZero(rMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mTranspose(hObs, (size_t) numObs, SKF_N_STATES_HALF, hObsT); diff --git a/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.h b/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.h index a06f5e55e2..f9d02c79a4 100644 --- a/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.h +++ b/src/fswAlgorithms/attDetermination/okeefeEKF/okeefeEKF.h @@ -5,23 +5,22 @@ #ifndef _SUNLINE_EKF_H_ #define _SUNLINE_EKF_H_ -#include - #include #include #include #include #include #include - #include + +#include #include /*!@brief Data structure for CSS Extended kalman filter estimator without gyros measurements. */ class OkeefeEKF : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; void sunlineTimeUpdate(double updateTime); @@ -46,17 +45,17 @@ class OkeefeEKF : public SysModel { double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] Time updated covariance */ double covar[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] covariance */ double stateTransition[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] covariance */ - double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS]; /*!< Kalman Gain */ + double kalmanGain[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS]; /*!< Kalman Gain */ - double dynMat[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] Dynamics Matrix, A */ - double measMat[MAX_N_CSS_MEAS * SKF_N_STATES_HALF]; /*!< [-] Measurement Matrix, H*/ + double dynMat[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] Dynamics Matrix, A */ + double measMat[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF]; /*!< [-] Measurement Matrix, H*/ - double obs[MAX_N_CSS_MEAS]; /*!< [-] Observation vector for frame*/ - double yMeas[MAX_N_CSS_MEAS]; /*!< [-] Linearized measurement model data */ + double obs[MAX_NUM_CSS_SENSORS]; /*!< [-] Observation vector for frame*/ + double yMeas[MAX_NUM_CSS_SENSORS]; /*!< [-] Linearized measurement model data */ - double procNoise[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] process noise matrix */ - double measNoise[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; /*!< [-] Maximally sized obs noise matrix*/ - double postFits[MAX_N_CSS_MEAS]; /*!< [-] PostFit residuals */ + double procNoise[SKF_N_STATES_HALF * SKF_N_STATES_HALF]; /*!< [-] process noise matrix */ + double measNoise[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /*!< [-] Maximally sized obs noise matrix*/ + double postFits[MAX_NUM_CSS_SENSORS]; /*!< [-] PostFit residuals */ double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]; /*!< [-] CSS normal vectors converted over to body*/ double CBias[MAX_NUM_CSS_SENSORS]; /*!< [-] CSS individual calibration coefficients */ @@ -73,55 +72,67 @@ class OkeefeEKF : public SysModel { BSKLogger bskLogger = {}; //!< BSK Logging }; -void sunlineStateSTMProp(double dynMat[SKF_N_STATES_HALF * SKF_N_STATES_HALF], - double dt, - double omega[SKF_N_STATES_HALF], - double* stateInOut, - double* prevstates, - double* stateTransition); - -void sunlineHMatrixYMeas(double states[SKF_N_STATES_HALF], - size_t numCSS, - double cssSensorCos[MAX_N_CSS_MEAS], - double sensorUseThresh, - double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], - double CBias[MAX_NUM_CSS_SENSORS], - double* obs, - double* yMeas, - int* numObs, - double* measMat); - -void sunlineKalmanGainOkeefe(double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], - double qObsVal, - int numObsInt, - double* kalmanGain); - -void sunlineRateCompute(double states[SKF_N_STATES_HALF], - double dt, - double prev_states[SKF_N_STATES_HALF], - double* omega); +void sunlineStateSTMProp( + double dynMat[SKF_N_STATES_HALF * SKF_N_STATES_HALF], + double dt, + double omega[SKF_N_STATES_HALF], + double* stateInOut, + double* prevstates, + double* stateTransition +); + +void sunlineHMatrixYMeas( + double states[SKF_N_STATES_HALF], + size_t numCSS, + double cssSensorCos[MAX_NUM_CSS_SENSORS], + double sensorUseThresh, + double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], + double CBias[MAX_NUM_CSS_SENSORS], + double* obs, + double* yMeas, + int* numObs, + double* measMat +); + +void sunlineKalmanGainOkeefe( + double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], + double qObsVal, + int numObsInt, + double* kalmanGain +); + +void sunlineRateCompute( + double states[SKF_N_STATES_HALF], + double dt, + double prev_states[SKF_N_STATES_HALF], + double* omega +); void sunlineDynMatrixOkeefe(double omega[SKF_N_STATES_HALF], double dt, double* dynMat); -void sunlineCKFUpdateOkeefe(double xBar[SKF_N_STATES_HALF], - double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS], - double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], - double qObsVal, - int numObsInt, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], - double* x, - double* covar); - -void okeefeEKFUpdate(double kalmanGain[SKF_N_STATES_HALF * MAX_N_CSS_MEAS], - double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], - double qObsVal, - int numObsInt, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES_HALF], - double* states, - double* x, - double* covar); +void sunlineCKFUpdateOkeefe( + double xBar[SKF_N_STATES_HALF], + double kalmanGain[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS], + double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], + double qObsVal, + int numObsInt, + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], + double* x, + double* covar +); + +void okeefeEKFUpdate( + double kalmanGain[SKF_N_STATES_HALF * MAX_NUM_CSS_SENSORS], + double covarBar[SKF_N_STATES_HALF * SKF_N_STATES_HALF], + double qObsVal, + int numObsInt, + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES_HALF], + double* states, + double* x, + double* covar +); #endif diff --git a/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.cpp b/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.cpp index 7b8891d4ad..1eaff75fde 100644 --- a/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.cpp +++ b/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.cpp @@ -32,11 +32,11 @@ void SunlineEKF::reset(uint64_t callTime) { /*! - Read in mass properties and coarse sun sensor configuration information.*/ cssConfigInBuffer = this->cssConfigInMsg(); - if (cssConfigInBuffer.nCSS > MAX_N_CSS_MEAS) { + if (cssConfigInBuffer.nCSS > MAX_NUM_CSS_SENSORS) { this->bskLogger.bskLog( BSK_ERROR, "sunlineEKF.cssConfigInMsg.nCSS must not be greater than " - "MAX_N_CSS_MEAS value." + "MAX_NUM_CSS_SENSORS value." ); } @@ -52,7 +52,7 @@ void SunlineEKF::reset(uint64_t callTime) { this->timeTag = callTime * NANO2SEC; this->dt = 0.0; this->numStates = SKF_N_STATES; - this->numObs = MAX_N_CSS_MEAS; + this->numObs = MAX_NUM_CSS_SENSORS; /*! - Ensure that all internal filter matrices are zeroed*/ vSetZero(this->obs, (size_t) this->numObs); @@ -80,7 +80,7 @@ void SunlineEKF::reset(uint64_t callTime) { */ void SunlineEKF::updateState(uint64_t callTime) { double newTimeTag; - double Hx[MAX_N_CSS_MEAS]; + double Hx[MAX_NUM_CSS_SENSORS]; uint64_t timeOfMsgWritten; int isWritten; SunlineFilterMsgPayload sunlineDataOutBuffer; @@ -123,7 +123,7 @@ void SunlineEKF::updateState(uint64_t callTime) { memmove(sunlineDataOutBuffer.covar, this->covar, SKF_N_STATES * SKF_N_STATES * sizeof(double)); memmove(sunlineDataOutBuffer.state, this->state, SKF_N_STATES * sizeof(double)); memmove(sunlineDataOutBuffer.stateError, this->x, SKF_N_STATES * sizeof(double)); - memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_N_CSS_MEAS * sizeof(double)); + memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_NUM_CSS_SENSORS * sizeof(double)); this->filtDataOutMsg.write(sunlineDataOutBuffer, this->moduleID, callTime); return; @@ -354,31 +354,31 @@ void SunlineEKF::sunlineMeasUpdate(double updateTime) { void sunlineCKFUpdate( double xBar[SKF_N_STATES], - double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS], + double kalmanGain[SKF_N_STATES * MAX_NUM_CSS_SENSORS], double covarBar[SKF_N_STATES * SKF_N_STATES], double qObsVal, int numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES], double* x, double* covar ) { - double measMatx[MAX_N_CSS_MEAS], innov[MAX_N_CSS_MEAS], kInnov[SKF_N_STATES]; + double measMatx[MAX_NUM_CSS_SENSORS], innov[MAX_NUM_CSS_SENSORS], kInnov[SKF_N_STATES]; double eye[SKF_N_STATES * SKF_N_STATES], kH[SKF_N_STATES * SKF_N_STATES]; double eyeKalH[SKF_N_STATES * SKF_N_STATES], eyeKalHT[SKF_N_STATES * SKF_N_STATES]; - double eyeKalHCovarBar[SKF_N_STATES * SKF_N_STATES], kalR[SKF_N_STATES * MAX_N_CSS_MEAS]; - double kalT[MAX_N_CSS_MEAS * SKF_N_STATES], kalRKalT[SKF_N_STATES * SKF_N_STATES]; - double noiseMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double eyeKalHCovarBar[SKF_N_STATES * SKF_N_STATES], kalR[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; + double kalT[MAX_NUM_CSS_SENSORS * SKF_N_STATES], kalRKalT[SKF_N_STATES * SKF_N_STATES]; + double noiseMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /* Set variables to zero */ mSetZero(kH, SKF_N_STATES, SKF_N_STATES); mSetZero(eyeKalH, SKF_N_STATES, SKF_N_STATES); mSetZero(eyeKalHT, SKF_N_STATES, SKF_N_STATES); - mSetZero(noiseMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(noiseMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mSetZero(eye, SKF_N_STATES, SKF_N_STATES); mSetZero(kalRKalT, SKF_N_STATES, SKF_N_STATES); - mSetZero(kalT, MAX_N_CSS_MEAS, SKF_N_STATES); - mSetZero(kalR, SKF_N_STATES, MAX_N_CSS_MEAS); + mSetZero(kalT, MAX_NUM_CSS_SENSORS, SKF_N_STATES); + mSetZero(kalR, SKF_N_STATES, MAX_NUM_CSS_SENSORS); mSetZero(eyeKalHCovarBar, SKF_N_STATES, SKF_N_STATES); /* Set noise matrix given number of observations */ @@ -419,31 +419,31 @@ void sunlineCKFUpdate( @param covar Pointer to the covariance after update */ void sunlineEKFUpdate( - double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS], + double kalmanGain[SKF_N_STATES * MAX_NUM_CSS_SENSORS], double covarBar[SKF_N_STATES * SKF_N_STATES], double qObsVal, int numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES], double* states, double* x, double* covar ) { double eye[SKF_N_STATES * SKF_N_STATES], kH[SKF_N_STATES * SKF_N_STATES]; double eyeKalH[SKF_N_STATES * SKF_N_STATES], eyeKalHT[SKF_N_STATES * SKF_N_STATES]; - double eyeKalHCovarBar[SKF_N_STATES * SKF_N_STATES], kalR[SKF_N_STATES * MAX_N_CSS_MEAS]; - double kalT[MAX_N_CSS_MEAS * SKF_N_STATES], kalRKalT[SKF_N_STATES * SKF_N_STATES]; - double noiseMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double eyeKalHCovarBar[SKF_N_STATES * SKF_N_STATES], kalR[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; + double kalT[MAX_NUM_CSS_SENSORS * SKF_N_STATES], kalRKalT[SKF_N_STATES * SKF_N_STATES]; + double noiseMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /* Set variables to zero */ mSetZero(kH, SKF_N_STATES, SKF_N_STATES); mSetZero(eyeKalH, SKF_N_STATES, SKF_N_STATES); mSetZero(eyeKalHT, SKF_N_STATES, SKF_N_STATES); - mSetZero(noiseMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(noiseMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mSetZero(eye, SKF_N_STATES, SKF_N_STATES); mSetZero(kalRKalT, SKF_N_STATES, SKF_N_STATES); - mSetZero(kalT, MAX_N_CSS_MEAS, SKF_N_STATES); - mSetZero(kalR, SKF_N_STATES, MAX_N_CSS_MEAS); + mSetZero(kalT, MAX_NUM_CSS_SENSORS, SKF_N_STATES); + mSetZero(kalR, SKF_N_STATES, MAX_NUM_CSS_SENSORS); mSetZero(eyeKalHCovarBar, SKF_N_STATES, SKF_N_STATES); /* Set noise matrix given number of observations */ @@ -490,7 +490,7 @@ void sunlineEKFUpdate( void sunlineHMatrixYMeas( double states[SKF_N_STATES], int numCSS, - double cssSensorCos[MAX_N_CSS_MEAS], + double cssSensorCos[MAX_NUM_CSS_SENSORS], double sensorUseThresh, double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], double CBias[MAX_NUM_CSS_SENSORS], @@ -533,22 +533,22 @@ void sunlineHMatrixYMeas( void sunlineKalmanGain( double covarBar[SKF_N_STATES * SKF_N_STATES], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES], double qObsVal, int numObs, double* kalmanGain ) { - double hObsT[SKF_N_STATES * MAX_N_CSS_MEAS]; - double covHT[SKF_N_STATES * MAX_N_CSS_MEAS]; - double hCovar[MAX_N_CSS_MEAS * SKF_N_STATES], hCovarHT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double rMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double hObsT[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; + double covHT[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; + double hCovar[MAX_NUM_CSS_SENSORS * SKF_N_STATES], hCovarHT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double rMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /* Setting all local variables to zero */ - mSetZero(hObsT, SKF_N_STATES, MAX_N_CSS_MEAS); - mSetZero(covHT, SKF_N_STATES, MAX_N_CSS_MEAS); - mSetZero(hCovar, MAX_N_CSS_MEAS, SKF_N_STATES); - mSetZero(hCovarHT, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); - mSetZero(rMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(hObsT, SKF_N_STATES, MAX_NUM_CSS_SENSORS); + mSetZero(covHT, SKF_N_STATES, MAX_NUM_CSS_SENSORS); + mSetZero(hCovar, MAX_NUM_CSS_SENSORS, SKF_N_STATES); + mSetZero(hCovarHT, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); + mSetZero(rMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mTranspose(hObs, (size_t) numObs, SKF_N_STATES, hObsT); diff --git a/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.h b/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.h index d3fd299f9e..b82503f201 100644 --- a/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.h +++ b/src/fswAlgorithms/attDetermination/sunlineEKF/sunlineEKF.h @@ -5,22 +5,21 @@ #ifndef _SUNLINE_EKF_H_ #define _SUNLINE_EKF_H_ -#include -#include - #include #include #include #include #include #include - #include +#include +#include + /*! @brief Top level structure for the CSS-based Extended Kalman Filter. Used to estimate the sun state in the vehicle body frame. */ class SunlineEKF : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; void sunlineTimeUpdate(double updateTime); @@ -37,23 +36,23 @@ class SunlineEKF : public SysModel { double dt; /*!< [s] seconds since last data epoch */ double timeTag; /*!< [s] Time tag for state/covar */ - double state[SKF_N_STATES]; /*!< [-] State estimate for time TimeTag*/ - double x[SKF_N_STATES]; /*!< State errors */ - double xBar[SKF_N_STATES]; /*!< [-] Current mean time updated state estimate*/ - double covarBar[SKF_N_STATES * SKF_N_STATES]; /*!< [-] Time updated covariance */ - double covar[SKF_N_STATES * SKF_N_STATES]; /*!< [-] covariance */ - double stateTransition[SKF_N_STATES * SKF_N_STATES]; /*!< [-] State Transtion Matrix */ - double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS]; /*!< Kalman Gain */ + double state[SKF_N_STATES]; /*!< [-] State estimate for time TimeTag*/ + double x[SKF_N_STATES]; /*!< State errors */ + double xBar[SKF_N_STATES]; /*!< [-] Current mean time updated state estimate*/ + double covarBar[SKF_N_STATES * SKF_N_STATES]; /*!< [-] Time updated covariance */ + double covar[SKF_N_STATES * SKF_N_STATES]; /*!< [-] covariance */ + double stateTransition[SKF_N_STATES * SKF_N_STATES]; /*!< [-] State Transtion Matrix */ + double kalmanGain[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; /*!< Kalman Gain */ - double dynMat[SKF_N_STATES * SKF_N_STATES]; /*!< [-] Dynamics Matrix, A */ - double measMat[MAX_N_CSS_MEAS * SKF_N_STATES]; /*!< [-] Measurement Matrix H*/ + double dynMat[SKF_N_STATES * SKF_N_STATES]; /*!< [-] Dynamics Matrix, A */ + double measMat[MAX_NUM_CSS_SENSORS * SKF_N_STATES]; /*!< [-] Measurement Matrix H*/ - double obs[MAX_N_CSS_MEAS]; /*!< [-] Observation vector for frame*/ - double yMeas[MAX_N_CSS_MEAS]; /*!< [-] Linearized measurement model data */ + double obs[MAX_NUM_CSS_SENSORS]; /*!< [-] Observation vector for frame*/ + double yMeas[MAX_NUM_CSS_SENSORS]; /*!< [-] Linearized measurement model data */ - double procNoise[SKF_N_STATES / 2 * SKF_N_STATES / 2]; /*!< [-] process noise matrix */ - double measNoise[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; /*!< [-] Maximally sized obs noise matrix*/ - double postFits[MAX_N_CSS_MEAS]; /*!< [-] PostFit residuals */ + double procNoise[SKF_N_STATES / 2 * SKF_N_STATES / 2]; /*!< [-] process noise matrix */ + double measNoise[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /*!< [-] Maximally sized obs noise matrix*/ + double postFits[MAX_NUM_CSS_SENSORS]; /*!< [-] PostFit residuals */ double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]; /*!< [-] CSS normal vectors converted over to body*/ double CBias[MAX_NUM_CSS_SENSORS]; /*!< [-] CSS individual calibration coefficients */ @@ -70,42 +69,52 @@ class SunlineEKF : public SysModel { BSKLogger bskLogger = {}; //!< BSK Logging }; -void sunlineStateSTMProp(double dynMat[SKF_N_STATES * SKF_N_STATES], - double dt, - double* stateInOut, - double* stateTransition); -void sunlineHMatrixYMeas(double states[SKF_N_STATES], - int numCSS, - double cssSensorCos[MAX_N_CSS_MEAS], - double sensorUseThresh, - double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], - double CBias[MAX_NUM_CSS_SENSORS], - double* obs, - double* yMeas, - int* numObs, - double* measMat); -void sunlineKalmanGain(double covarBar[SKF_N_STATES * SKF_N_STATES], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], - double qObsVal, - int numObs, - double* kalmanGain); +void sunlineStateSTMProp( + double dynMat[SKF_N_STATES * SKF_N_STATES], + double dt, + double* stateInOut, + double* stateTransition +); +void sunlineHMatrixYMeas( + double states[SKF_N_STATES], + int numCSS, + double cssSensorCos[MAX_NUM_CSS_SENSORS], + double sensorUseThresh, + double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], + double CBias[MAX_NUM_CSS_SENSORS], + double* obs, + double* yMeas, + int* numObs, + double* measMat +); +void sunlineKalmanGain( + double covarBar[SKF_N_STATES * SKF_N_STATES], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES], + double qObsVal, + int numObs, + double* kalmanGain +); void sunlineDynMatrix(double stateInOut[SKF_N_STATES], double dt, double* dynMat); -void sunlineCKFUpdate(double xBar[SKF_N_STATES], - double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS], - double covarBar[SKF_N_STATES * SKF_N_STATES], - double qObsVal, - int numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], - double* x, - double* covar); -void sunlineEKFUpdate(double kalmanGain[SKF_N_STATES * MAX_N_CSS_MEAS], - double covarBar[SKF_N_STATES * SKF_N_STATES], - double qObsVal, - int numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * SKF_N_STATES], - double* states, - double* x, - double* covar); +void sunlineCKFUpdate( + double xBar[SKF_N_STATES], + double kalmanGain[SKF_N_STATES * MAX_NUM_CSS_SENSORS], + double covarBar[SKF_N_STATES * SKF_N_STATES], + double qObsVal, + int numObs, + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES], + double* x, + double* covar +); +void sunlineEKFUpdate( + double kalmanGain[SKF_N_STATES * MAX_NUM_CSS_SENSORS], + double covarBar[SKF_N_STATES * SKF_N_STATES], + double qObsVal, + int numObs, + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * SKF_N_STATES], + double* states, + double* x, + double* covar +); #endif diff --git a/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.cpp b/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.cpp index 8b60743291..9663baffe3 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.cpp +++ b/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.cpp @@ -33,11 +33,11 @@ void SunlineSEKF::reset(uint64_t callTime) { /*! - Read coarse sun sensor configuration information.*/ cssConfigInBuffer = this->cssConfigInMsg(); - if (cssConfigInBuffer.nCSS > MAX_N_CSS_MEAS) { + if (cssConfigInBuffer.nCSS > MAX_NUM_CSS_SENSORS) { this->bskLogger.bskLog( BSK_ERROR, "sunlineSEKF.cssConfigInMsg.nCSS must not be greater than " - "MAX_N_CSS_MEAS value." + "MAX_NUM_CSS_SENSORS value." ); } @@ -52,7 +52,7 @@ void SunlineSEKF::reset(uint64_t callTime) { this->timeTag = callTime * NANO2SEC; this->dt = 0.0; this->numStates = EKF_N_STATES_SWITCH; - this->numObs = MAX_N_CSS_MEAS; + this->numObs = MAX_NUM_CSS_SENSORS; /*! Initalize the filter to use b_1 of the body frame to make frame*/ v3Set(1, 0, 0, this->bVec_B); @@ -85,7 +85,7 @@ void SunlineSEKF::reset(uint64_t callTime) { */ void SunlineSEKF::updateState(uint64_t callTime) { double newTimeTag; - double Hx[MAX_N_CSS_MEAS]; + double Hx[MAX_NUM_CSS_SENSORS]; double states_BN[EKF_N_STATES_SWITCH]; double sunheading_hat[SKF_N_STATES_HALF]; uint64_t timeOfMsgWritten; @@ -141,7 +141,7 @@ void SunlineSEKF::updateState(uint64_t callTime) { memmove(sunlineDataOutBuffer.covar, this->covar, EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH * sizeof(double)); memmove(sunlineDataOutBuffer.state, states_BN, EKF_N_STATES_SWITCH * sizeof(double)); memmove(sunlineDataOutBuffer.stateError, this->x, SKF_N_STATES * sizeof(double)); - memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_N_CSS_MEAS * sizeof(double)); + memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_NUM_CSS_SENSORS * sizeof(double)); this->filtDataOutMsg.write(sunlineDataOutBuffer, this->moduleID, callTime); return; @@ -402,31 +402,31 @@ void sunlineMeasUpdate(SunlineSEKF* data, double updateTime) { void sunlineCKFUpdate( double xBar[EKF_N_STATES_SWITCH], - double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], + double kalmanGain[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS], double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], double qObsVal, size_t numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], double* x, double* covar ) { - double measMatx[MAX_N_CSS_MEAS], innov[MAX_N_CSS_MEAS], kInnov[EKF_N_STATES_SWITCH]; + double measMatx[MAX_NUM_CSS_SENSORS], innov[MAX_NUM_CSS_SENSORS], kInnov[EKF_N_STATES_SWITCH]; double eye[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], kH[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; double eyeKalH[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], eyeKalHT[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; - double eyeKalHCovarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], kalR[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; - double kalT[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], kalRKalT[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; - double noiseMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double eyeKalHCovarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], kalR[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; + double kalT[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], kalRKalT[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; + double noiseMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /* Set variables to zero */ mSetZero(kH, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); mSetZero(eyeKalH, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); mSetZero(eyeKalHT, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); - mSetZero(noiseMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(noiseMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mSetZero(eye, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); mSetZero(kalRKalT, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); - mSetZero(kalT, MAX_N_CSS_MEAS, EKF_N_STATES_SWITCH); - mSetZero(kalR, EKF_N_STATES_SWITCH, MAX_N_CSS_MEAS); + mSetZero(kalT, MAX_NUM_CSS_SENSORS, EKF_N_STATES_SWITCH); + mSetZero(kalR, EKF_N_STATES_SWITCH, MAX_NUM_CSS_SENSORS); mSetZero(eyeKalHCovarBar, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); /* Set noise matrix given number of observations */ @@ -483,31 +483,31 @@ void sunlineCKFUpdate( @param covar Pointer to the covariance after update */ void sunlineSEKFUpdate( - double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], + double kalmanGain[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS], double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], double qObsVal, size_t numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], double* states, double* x, double* covar ) { double eye[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], kH[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; double eyeKalH[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], eyeKalHT[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; - double eyeKalHCovarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], kalR[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; - double kalT[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], kalRKalT[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; - double noiseMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double eyeKalHCovarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], kalR[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; + double kalT[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], kalRKalT[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; + double noiseMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /* Set variables to zero */ mSetZero(kH, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); mSetZero(eyeKalH, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); mSetZero(eyeKalHT, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); - mSetZero(noiseMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(noiseMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mSetZero(eye, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); mSetZero(kalRKalT, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); - mSetZero(kalT, MAX_N_CSS_MEAS, EKF_N_STATES_SWITCH); - mSetZero(kalR, EKF_N_STATES_SWITCH, MAX_N_CSS_MEAS); + mSetZero(kalT, MAX_NUM_CSS_SENSORS, EKF_N_STATES_SWITCH); + mSetZero(kalR, EKF_N_STATES_SWITCH, MAX_NUM_CSS_SENSORS); mSetZero(eyeKalHCovarBar, EKF_N_STATES_SWITCH, EKF_N_STATES_SWITCH); /* Set noise matrix given number of observations */ @@ -569,7 +569,7 @@ void sunlineSEKFUpdate( void sunlineHMatrixYMeas( double states[EKF_N_STATES_SWITCH], size_t numCSS, - double cssSensorCos[MAX_N_CSS_MEAS], + double cssSensorCos[MAX_NUM_CSS_SENSORS], double sensorUseThresh, double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], double* obs, @@ -610,22 +610,22 @@ void sunlineHMatrixYMeas( void sunlineKalmanGain( double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], - double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], + double hObs[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], double qObsVal, size_t numObs, double* kalmanGain ) { - double hObsT[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; - double covHT[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; - double hCovar[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], hCovarHT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double rMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; + double hObsT[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; + double covHT[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; + double hCovar[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], hCovarHT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double rMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /* Setting all local variables to zero */ - mSetZero(hObsT, EKF_N_STATES_SWITCH, MAX_N_CSS_MEAS); - mSetZero(covHT, EKF_N_STATES_SWITCH, MAX_N_CSS_MEAS); - mSetZero(hCovar, MAX_N_CSS_MEAS, EKF_N_STATES_SWITCH); - mSetZero(hCovarHT, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); - mSetZero(rMat, MAX_N_CSS_MEAS, MAX_N_CSS_MEAS); + mSetZero(hObsT, EKF_N_STATES_SWITCH, MAX_NUM_CSS_SENSORS); + mSetZero(covHT, EKF_N_STATES_SWITCH, MAX_NUM_CSS_SENSORS); + mSetZero(hCovar, MAX_NUM_CSS_SENSORS, EKF_N_STATES_SWITCH); + mSetZero(hCovarHT, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); + mSetZero(rMat, MAX_NUM_CSS_SENSORS, MAX_NUM_CSS_SENSORS); mTranspose(hObs, numObs, EKF_N_STATES_SWITCH, hObsT); diff --git a/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.h b/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.h index 1487662a2f..177e7ec744 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.h +++ b/src/fswAlgorithms/attDetermination/sunlineSEKF/sunlineSEKF.h @@ -5,22 +5,21 @@ #ifndef _SUNLINE_EKF_H_ #define _SUNLINE_EKF_H_ -#include - #include #include #include #include #include #include - #include + +#include #include /*! @brief Top level structure for the CSS-based Switch Extended Kalman Filter. Used to estimate the sun state in the vehicle body frame. */ class SunlineSEKF : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; @@ -46,19 +45,19 @@ class SunlineSEKF : public SysModel { double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; /*!< [-] Time updated covariance */ double covar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; /*!< [-] covariance */ double stateTransition[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; /*!< [-] State transition Matrix */ - double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; /*!< Kalman Gain */ + double kalmanGain[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; /*!< Kalman Gain */ - double dynMat[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; /*!< [-] Dynamics Matrix, A */ - double measMat[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH]; /*!< [-] Measurement Matrix, H*/ + double dynMat[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; /*!< [-] Dynamics Matrix, A */ + double measMat[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH]; /*!< [-] Measurement Matrix, H*/ double W_BS[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH]; /*!< [-] Switch Matrix to bring states and covariance to new S-frame when switch occurs*/ - double obs[MAX_N_CSS_MEAS]; /*!< [-] Observation vector for frame*/ - double yMeas[MAX_N_CSS_MEAS]; /*!< [-] Linearized measurement model data */ - double postFits[MAX_N_CSS_MEAS]; /*!< [-] PostFit residuals */ + double obs[MAX_NUM_CSS_SENSORS]; /*!< [-] Observation vector for frame*/ + double yMeas[MAX_NUM_CSS_SENSORS]; /*!< [-] Linearized measurement model data */ + double postFits[MAX_NUM_CSS_SENSORS]; /*!< [-] PostFit residuals */ double procNoise[(EKF_N_STATES_SWITCH - 3) * (EKF_N_STATES_SWITCH - 3)]; /*!< [-] process noise matrix */ - double measNoise[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; /*!< [-] Maximally sized obs noise matrix*/ + double measNoise[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /*!< [-] Maximally sized obs noise matrix*/ double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]; /*!< [-] CSS normal vectors converted over to body*/ uint32_t numStates; /*!< [-] Number of states for this filter*/ @@ -75,44 +74,54 @@ class SunlineSEKF : public SysModel { void sunlineTimeUpdate(SunlineSEKF* data, double updateTime); void sunlineMeasUpdate(SunlineSEKF* data, double updateTime); -void sunlineStateSTMProp(double dynMat[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], - double bVec[SKF_N_STATES], - double dt, - double* stateInOut, - double* stateTransition); -void sunlineHMatrixYMeas(double states[EKF_N_STATES_SWITCH], - size_t numCSS, - double cssSensorCos[MAX_N_CSS_MEAS], - double sensorUseThresh, - double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], - double* obs, - double* yMeas, - int* numObs, - double* measMat); -void sunlineKalmanGain(double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], - double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], - double qObsVal, - size_t numObs, - double* kalmanGain); +void sunlineStateSTMProp( + double dynMat[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], + double bVec[SKF_N_STATES], + double dt, + double* stateInOut, + double* stateTransition +); +void sunlineHMatrixYMeas( + double states[EKF_N_STATES_SWITCH], + size_t numCSS, + double cssSensorCos[MAX_NUM_CSS_SENSORS], + double sensorUseThresh, + double cssNHat_B[MAX_NUM_CSS_SENSORS * 3], + double* obs, + double* yMeas, + int* numObs, + double* measMat +); +void sunlineKalmanGain( + double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], + double hObs[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], + double qObsVal, + size_t numObs, + double* kalmanGain +); void sunlineDynMatrix(double states[EKF_N_STATES_SWITCH], double bVec[SKF_N_STATES], double dt, double* dynMat); -void sunlineCKFUpdate(double xBar[EKF_N_STATES_SWITCH], - double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], - double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], - double qObsVal, - size_t numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], - double* x, - double* covar); -void sunlineSEKFUpdate(double kalmanGain[EKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], - double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], - double qObsVal, - size_t numObs, - double yObs[MAX_N_CSS_MEAS], - double hObs[MAX_N_CSS_MEAS * EKF_N_STATES_SWITCH], - double* states, - double* x, - double* covar); +void sunlineCKFUpdate( + double xBar[EKF_N_STATES_SWITCH], + double kalmanGain[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS], + double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], + double qObsVal, + size_t numObs, + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], + double* x, + double* covar +); +void sunlineSEKFUpdate( + double kalmanGain[EKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS], + double covarBar[EKF_N_STATES_SWITCH * EKF_N_STATES_SWITCH], + double qObsVal, + size_t numObs, + double yObs[MAX_NUM_CSS_SENSORS], + double hObs[MAX_NUM_CSS_SENSORS * EKF_N_STATES_SWITCH], + double* states, + double* x, + double* covar +); void sunlineSEKFSwitch(double* bVec_B, double* states, double* covar); void sunlineSEKFComputeDCM_BS(double sunheading[SKF_N_STATES_HALF], double bVec[SKF_N_STATES_HALF], double* dcm); diff --git a/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.cpp b/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.cpp index 3e79061f50..23982c21a3 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.cpp +++ b/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.cpp @@ -36,11 +36,11 @@ void SunlineSuKF::reset(uint64_t callTime) { /*! - Read in mass properties and coarse sun sensor configuration information.*/ cssConfigInBuffer = this->cssConfigInMsg(); - if (cssConfigInBuffer.nCSS > MAX_N_CSS_MEAS) { + if (cssConfigInBuffer.nCSS > MAX_NUM_CSS_SENSORS) { this->bskLogger.bskLog( BSK_ERROR, "sunlineSuKF.cssConfigInMsg.nCSS must not be greater than " - "MAX_N_CSS_MEAS value." + "MAX_NUM_CSS_SENSORS value." ); } @@ -57,7 +57,7 @@ void SunlineSuKF::reset(uint64_t callTime) { this->timeTag = callTime * NANO2SEC; this->numStates = SKF_N_STATES_SWITCH; this->countHalfSPs = SKF_N_STATES_SWITCH; - this->numObs = MAX_N_CSS_MEAS; + this->numObs = MAX_NUM_CSS_SENSORS; /*! Initalize the filter to use b_1 of the body frame to make frame*/ v3Set(1, 0, 0, this->bVec_B); @@ -111,8 +111,8 @@ void SunlineSuKF::reset(uint64_t callTime) { */ void SunlineSuKF::updateState(uint64_t callTime) { double newTimeTag; - double yBar[MAX_N_CSS_MEAS]; - double tempYVec[MAX_N_CSS_MEAS]; + double yBar[MAX_NUM_CSS_SENSORS]; + double tempYVec[MAX_NUM_CSS_SENSORS]; double sunheading_hat[3]; double states_BN[SKF_N_STATES_SWITCH]; uint64_t i; @@ -205,7 +205,7 @@ void SunlineSuKF::updateState(uint64_t callTime) { sunlineDataOutBuffer.numObs = (int) this->numObs; memmove(sunlineDataOutBuffer.covar, this->covar, SKF_N_STATES_SWITCH * SKF_N_STATES_SWITCH * sizeof(double)); memmove(sunlineDataOutBuffer.state, states_BN, SKF_N_STATES_SWITCH * sizeof(double)); - memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_N_CSS_MEAS * sizeof(double)); + memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_NUM_CSS_SENSORS * sizeof(double)); this->filtDataOutMsg.write(sunlineDataOutBuffer, this->moduleID, callTime); return; @@ -407,14 +407,15 @@ void sunlineSuKFMeasModel(SunlineSuKF* data) { int sunlineSuKFMeasUpdate(SunlineSuKF* data, double updateTime) { uint32_t i; int32_t badUpdate; - double yBar[MAX_N_CSS_MEAS], syInv[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double kMat[SKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; - double xHat[SKF_N_STATES_SWITCH], sBarT[SKF_N_STATES_SWITCH * SKF_N_STATES_SWITCH], tempYVec[MAX_N_CSS_MEAS]; - double AT[(2 * SKF_N_STATES_SWITCH + MAX_N_CSS_MEAS) * MAX_N_CSS_MEAS], qChol[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double rAT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS], syT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double sy[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS], Ucol[SKF_N_STATES_SWITCH]; - double updMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS], pXY[SKF_N_STATES_SWITCH * MAX_N_CSS_MEAS], - Umat[SKF_N_STATES_SWITCH * MAX_N_CSS_MEAS]; + double yBar[MAX_NUM_CSS_SENSORS], syInv[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double kMat[SKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; + double xHat[SKF_N_STATES_SWITCH], sBarT[SKF_N_STATES_SWITCH * SKF_N_STATES_SWITCH], tempYVec[MAX_NUM_CSS_SENSORS]; + double AT[(2 * SKF_N_STATES_SWITCH + MAX_NUM_CSS_SENSORS) * MAX_NUM_CSS_SENSORS], + qChol[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double rAT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS], syT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double sy[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS], Ucol[SKF_N_STATES_SWITCH]; + double updMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS], pXY[SKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS], + Umat[SKF_N_STATES_SWITCH * MAX_NUM_CSS_SENSORS]; badUpdate = 0; vCopy(data->state, data->numStates, data->statePrev); diff --git a/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.h b/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.h index bef68252e4..41d661a22d 100644 --- a/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.h +++ b/src/fswAlgorithms/attDetermination/sunlineSuKF/sunlineSuKF.h @@ -5,18 +5,17 @@ #ifndef _SUNLINE_UKF_H_ #define _SUNLINE_UKF_H_ -#include -#include - #include #include #include #include #include #include - #include +#include +#include + /*! structure containing the fitting parameters */ typedef struct { @@ -28,7 +27,7 @@ typedef struct { /*!@brief Data structure for CSS Switch unscented kalman filter estimator. */ class SunlineSuKF : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; @@ -67,16 +66,16 @@ class SunlineSuKF : public SysModel { double covarPrev[SKF_N_STATES_SWITCH * SKF_N_STATES_SWITCH]; //!< [-] Covariance logged for clean double xBar[SKF_N_STATES_SWITCH]; //!< [-] Current mean state estimate - double obs[MAX_N_CSS_MEAS]; //!< [-] Observation vector for frame - double yMeas[MAX_N_CSS_MEAS * (2 * SKF_N_STATES_SWITCH + 1)]; //!< [-] Measurement model data - double postFits[MAX_N_CSS_MEAS]; //!< [-] PostFit residuals + double obs[MAX_NUM_CSS_SENSORS]; //!< [-] Observation vector for frame + double yMeas[MAX_NUM_CSS_SENSORS * (2 * SKF_N_STATES_SWITCH + 1)]; //!< [-] Measurement model data + double postFits[MAX_NUM_CSS_SENSORS]; //!< [-] PostFit residuals double SP[(2 * SKF_N_STATES_SWITCH + 1) * SKF_N_STATES_SWITCH]; //!< [-] sigma point matrix double qNoise[SKF_N_STATES_SWITCH * SKF_N_STATES_SWITCH]; //!< [-] process noise matrix double sQnoise[SKF_N_STATES_SWITCH * SKF_N_STATES_SWITCH]; //!< [-] cholesky of Qnoise - double qObs[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; //!< [-] Maximally sized obs noise matrix + double qObs[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; //!< [-] Maximally sized obs noise matrix double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]; //!< [-] CSS normal vectors converted over to body double CBias[MAX_NUM_CSS_SENSORS]; //!< [-] CSS individual calibration coefficients diff --git a/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.cpp b/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.cpp index e4e81e2688..b358938989 100644 --- a/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.cpp +++ b/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.cpp @@ -40,11 +40,11 @@ void SunlineUKF::reset(uint64_t callTime) { /*! - Read in mass properties and coarse sun sensor configuration information.*/ cssConfigInBuffer = this->cssConfigInMsg(); - if (cssConfigInBuffer.nCSS > MAX_N_CSS_MEAS) { + if (cssConfigInBuffer.nCSS > MAX_NUM_CSS_SENSORS) { this->bskLogger.bskLog( BSK_ERROR, "sunlineUKF.cssConfigInMsg.nCSS must not be greater than " - "MAX_N_CSS_MEAS value." + "MAX_NUM_CSS_SENSORS value." ); } @@ -61,7 +61,7 @@ void SunlineUKF::reset(uint64_t callTime) { this->dt = 0.0; this->numStates = SKF_N_STATES; this->countHalfSPs = SKF_N_STATES; - this->numObs = MAX_N_CSS_MEAS; + this->numObs = MAX_NUM_CSS_SENSORS; /*! - Ensure that all internal filter matrices are zeroed*/ vSetZero(this->obs, this->numObs); @@ -101,8 +101,8 @@ void SunlineUKF::reset(uint64_t callTime) { */ void SunlineUKF::updateState(uint64_t callTime) { double newTimeTag; - double yBar[MAX_N_CSS_MEAS]; - double tempYVec[MAX_N_CSS_MEAS]; + double yBar[MAX_NUM_CSS_SENSORS]; + double tempYVec[MAX_NUM_CSS_SENSORS]; int i; uint64_t timeOfMsgWritten; int isWritten; @@ -138,7 +138,7 @@ void SunlineUKF::updateState(uint64_t callTime) { } /*! - The post fits are y- ybar*/ - mSubtract(this->obs, MAX_N_CSS_MEAS, 1, yBar, this->postFits); + mSubtract(this->obs, MAX_NUM_CSS_SENSORS, 1, yBar, this->postFits); /*! - Write the sunline estimate into the copy of the navigation message structure*/ v3Copy(this->state, this->outputSunline.vehSunPntBdy); @@ -151,7 +151,7 @@ void SunlineUKF::updateState(uint64_t callTime) { sunlineDataOutBuffer.numObs = this->numObs; memmove(sunlineDataOutBuffer.covar, this->covar, SKF_N_STATES * SKF_N_STATES * sizeof(double)); memmove(sunlineDataOutBuffer.state, this->state, SKF_N_STATES * sizeof(double)); - memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_N_CSS_MEAS * sizeof(double)); + memmove(sunlineDataOutBuffer.postFitRes, this->postFits, MAX_NUM_CSS_SENSORS * sizeof(double)); this->filtDataOutMsg.write(sunlineDataOutBuffer, this->moduleID, callTime); return; @@ -316,13 +316,14 @@ void sunlineUKFMeasModel(SunlineUKF* configData) { @param updateTime The time that we need to fix the filter to (seconds) */ void sunlineUKFMeasUpdate(SunlineUKF* configData, double updateTime) { - double yBar[MAX_N_CSS_MEAS], syInv[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double kMat[SKF_N_STATES * MAX_N_CSS_MEAS]; - double xHat[SKF_N_STATES], sBarT[SKF_N_STATES * SKF_N_STATES], tempYVec[MAX_N_CSS_MEAS]; - double AT[(2 * SKF_N_STATES + MAX_N_CSS_MEAS) * MAX_N_CSS_MEAS], qChol[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double rAT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS], syT[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double sy[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; - double updMat[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS], pXY[SKF_N_STATES * MAX_N_CSS_MEAS]; + double yBar[MAX_NUM_CSS_SENSORS], syInv[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double kMat[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; + double xHat[SKF_N_STATES], sBarT[SKF_N_STATES * SKF_N_STATES], tempYVec[MAX_NUM_CSS_SENSORS]; + double AT[(2 * SKF_N_STATES + MAX_NUM_CSS_SENSORS) * MAX_NUM_CSS_SENSORS], + qChol[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double rAT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS], syT[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double sy[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; + double updMat[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS], pXY[SKF_N_STATES * MAX_NUM_CSS_SENSORS]; /*! - Compute the valid observations and the measurement model for all observations*/ sunlineUKFMeasModel(configData); diff --git a/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.h b/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.h index b5a8e4c361..528892c593 100644 --- a/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.h +++ b/src/fswAlgorithms/attDetermination/sunlineUKF/sunlineUKF.h @@ -5,21 +5,20 @@ #ifndef _SUNLINE_UKF_H_ #define _SUNLINE_UKF_H_ -#include - #include #include #include #include #include #include - #include +#include + /*! @brief Top level structure for the CSS-based unscented Kalman Filter. Used to estimate the sun state in the vehicle body frame. */ class SunlineUKF : public SysModel { - public: +public: void reset(uint64_t callTime) override; void updateState(uint64_t callTime) override; @@ -49,16 +48,16 @@ class SunlineUKF : public SysModel { double covar[SKF_N_STATES * SKF_N_STATES]; /*!< [-] covariance */ double xBar[SKF_N_STATES]; /*!< [-] Current mean state estimate*/ - double obs[MAX_N_CSS_MEAS]; /*!< [-] Observation vector for frame*/ - double yMeas[MAX_N_CSS_MEAS * (2 * SKF_N_STATES + 1)]; /*!< [-] Measurement model data */ - double postFits[MAX_N_CSS_MEAS]; /*!< [-] PostFit residuals */ + double obs[MAX_NUM_CSS_SENSORS]; /*!< [-] Observation vector for frame*/ + double yMeas[MAX_NUM_CSS_SENSORS * (2 * SKF_N_STATES + 1)]; /*!< [-] Measurement model data */ + double postFits[MAX_NUM_CSS_SENSORS]; /*!< [-] PostFit residuals */ double SP[(2 * SKF_N_STATES + 1) * SKF_N_STATES]; /*!< [-] sigma point matrix */ double qNoise[SKF_N_STATES * SKF_N_STATES]; /*!< [-] process noise matrix */ double sQnoise[SKF_N_STATES * SKF_N_STATES]; /*!< [-] cholesky of Qnoise */ - double qObs[MAX_N_CSS_MEAS * MAX_N_CSS_MEAS]; /*!< [-] Maximally sized obs noise matrix*/ + double qObs[MAX_NUM_CSS_SENSORS * MAX_NUM_CSS_SENSORS]; /*!< [-] Maximally sized obs noise matrix*/ double cssNHat_B[MAX_NUM_CSS_SENSORS * 3]; /*!< [-] CSS normal vectors converted over to body*/ double CBias[MAX_NUM_CSS_SENSORS]; /*!< [-] CSS individual calibration coefficients */ From da3fe7a71e38356e4c57f8adce4bdcb85337be75 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:53 -0600 Subject: [PATCH 06/10] Make the CSS comm test size-aware --- .../sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py b/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py index 0d279b6c96..080c0395a6 100644 --- a/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py +++ b/src/fswAlgorithms/sensorInterfaces/CSSSensorData/_UnitTest/test_cssComm.py @@ -99,7 +99,7 @@ def cssCommTestFunction(numSensors, sensorData): if numSensors==4: trueCssList[0:4] = [0.0, 0.45791653042, 1.0, 0.615444781018] if numSensors==MAX_NUM_CSS_SENSORS+1: - trueCssList = [0.45791653042]*32 + trueCssList = [0.45791653042]*MAX_NUM_CSS_SENSORS # Create the true array trueCss = [ From 54f86d2fdd48f62b79e42383cf600a95a6fb8746 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:54 -0600 Subject: [PATCH 07/10] Make the CSS WLS estimator test size-aware --- .../attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py b/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py index 019bc46f87..c437419f34 100644 --- a/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py +++ b/src/fswAlgorithms/attDetermination/CSSEst/_UnitTest/test_cssWlsEstUnitTest.py @@ -47,7 +47,7 @@ def checkNumActiveAccuracy(measVec, numActiveUse, numActiveFailCriteria, thresh) numActivePred = 0 testFailCount = 0 # Iterate through measVec and find all valid signals - for i in range(0, 32): + for i in range(len(measVec.CosValue)): obsVal = measVec.CosValue[i] if (obsVal > thresh): numActivePred += 1 From 8683376794b3f31697f40a5709e30a7bb8926872 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:54 -0600 Subject: [PATCH 08/10] Make reaction wheel motor torque tests size-aware --- .../rwMotorTorque/_UnitTest/test_rwMotorTorque.py | 2 +- .../_UnitTest/test_rwMotorTorqueParametrized_C.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py index 75a413283a..0f57b9e140 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py +++ b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque/_UnitTest/test_rwMotorTorque.py @@ -216,7 +216,7 @@ def compute_true_torque(C, Gs_B, Lr, avail_msg): if __name__ == "__main__": test_rw_motor_torque(False, 3, # numControlAxes - 36, # numWheels + messaging.RW_EFF_CNT, # numWheels 2, # numInputCmdTorques "NO" # RWAvailMsg ("NO", "ON", "OFF") ) diff --git a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque_C/_UnitTest/test_rwMotorTorqueParametrized_C.py b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque_C/_UnitTest/test_rwMotorTorqueParametrized_C.py index e80be6e0dc..9ef80c33b1 100644 --- a/src/fswAlgorithms/effectorInterfaces/rwMotorTorque_C/_UnitTest/test_rwMotorTorqueParametrized_C.py +++ b/src/fswAlgorithms/effectorInterfaces/rwMotorTorque_C/_UnitTest/test_rwMotorTorqueParametrized_C.py @@ -142,7 +142,7 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, numInputCmdTorques, 0.6839787443692367, 0.6598940110591041, 0.31098709204629277, 0.35743175000357147, 0.8343049491885353, 0.4197353878920623, 0.8124751056450826, 0.35669421673672336, 0.46114362020262967, - 0.04721328350343224, 0.8901899787392832, 0.45313652204714083] + 0.04721328350343224, 0.8901899787392832, 0.45313652204714083][:numWheels * 3] else: rwConfigParams.GsMatrix_B = [ 1.0, 0.0, 0.0, @@ -270,7 +270,7 @@ def rwMotorTorqueTest(show_plots, numControlAxes, numWheels, numInputCmdTorques, if __name__ == "__main__": test_rwMotorTorque_C(False, 3, # numControlAxes - 36, # numWheels + messaging.RW_EFF_CNT, # numWheels 2, # numInputCmdTorques "NO" # RWAvailMsg ("NO", "ON", "OFF") ) From 06e3b6ecf38d5e19a85e39609e65a4c69fa88d8c Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:54 -0600 Subject: [PATCH 09/10] Check that C++ and Python agree on the mission parameters --- .../_UnitTest/test_missionParameters.py | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 src/architecture/messaging/_UnitTest/test_missionParameters.py diff --git a/src/architecture/messaging/_UnitTest/test_missionParameters.py b/src/architecture/messaging/_UnitTest/test_missionParameters.py new file mode 100644 index 0000000000..47f9e16e1e --- /dev/null +++ b/src/architecture/messaging/_UnitTest/test_missionParameters.py @@ -0,0 +1,73 @@ +"""The mission parameters get to the build along two paths. + +The two paths can give different values, and the build gives no error message. + +The C++ compiler reads the ``#include`` directives and finds ``mission/parameters.h``. SWIG does +not read ``#include`` directives. SWIG reads only the files that a ``.i`` file ``%include``s. +Thus the constants that SWIG gives to Python come from a different preprocessor. Only convention +makes sure that the two preprocessors read the same header. + +When the two paths give different values, no failure occurs. The compiler sets the layout of the +payload structs, and Python shows a different bound. The results are then incorrect, but only +after a struct with the incorrect layout moves across an FFI boundary. The known cause is a digit +separator in the header. SWIG discards every constant after the separator, and Python then has a +missing attribute. + +Each payload field below is a Python list. The length of the list comes from the compiled struct, +but the constant comes from the SWIG preprocessor. Thus a comparison of the two values makes sure +that the two paths agree. +""" + +import pytest + +from xmera.architecture import messaging + +# One (payload type, field, constant) triple for each constant that gives an array bound. +SIZED_FIELDS = [ + ("RWSpeedMsgPayload", "wheelSpeeds", "RW_EFF_CNT"), + ("RWArrayConfigMsgPayload", "JsList", "RW_EFF_CNT"), + ("RwMotorTorqueMsgPayload", "motorTorque", "RW_EFF_CNT"), + ("CSSArraySensorMsgPayload", "CosValue", "MAX_NUM_CSS_SENSORS"), + ("THRArrayOnTimeCmdMsgPayload", "OnTimeRequest", "MAX_EFF_CNT"), + ("SunlineFilterMsgPayload", "postFitRes", "MAX_NUM_CSS_SENSORS"), + ("RegionsIdentifiedMsgPayload", "timeTag", "MAX_NUMBER_REGIONS"), +] + +CONSTANTS = [ + "MAX_KEY_POINTS", + "MAX_NUM_CSS_SENSORS", + "MAX_EFF_CNT", + "RW_EFF_CNT", + "MAX_SICP_POINTS", + "SICP_POINT_DIM", + "MAX_SICP_ITERATIONS", + "MAX_NUMBER_REGIONS", +] + + +@pytest.mark.parametrize("name", CONSTANTS) +def test_constant_reaches_python(name): + """SWIG gives every mission constant to Python. + + If a constant is missing, the SWIG preprocessor discarded it. Usually the cause is text + before that constant in the header, and not a constant that no code uses. + """ + assert hasattr(messaging, name), ( + f"{name} did not get to the Python layer. SWIG stopped at some line in " + f"mission/parameters.h and discarded the constants after that line." + ) + assert isinstance(getattr(messaging, name), int) + assert getattr(messaging, name) > 0 + + +@pytest.mark.parametrize("payload,field,constant", SIZED_FIELDS) +def test_payload_extent_matches_constant(payload, field, constant): + """The compiled struct and the SWIG constant agree on the array bound.""" + expected = getattr(messaging, constant) + actual = len(getattr(getattr(messaging, payload)(), field)) + assert actual == expected, ( + f"The build compiled {payload}.{field} with {actual} elements, but Python shows " + f"{constant} == {expected}. C++ and SWIG resolved different mission parameters. Make " + f"sure that the build selected the correct mission/parameters.h. Then examine the " + f"%include lines in the .i template." + ) From df44f77257031242da70e36923ca42bdda253f5f Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Wed, 9 Sep 2026 21:07:55 -0600 Subject: [PATCH 10/10] Document configuring mission parameters --- docs/source/learn/making-modules.rst | 1 + .../messaging/mission-parameters.rst | 115 ++++++++++++++++++ 2 files changed, 116 insertions(+) create mode 100644 docs/source/learn/making-modules/messaging/mission-parameters.rst diff --git a/docs/source/learn/making-modules.rst b/docs/source/learn/making-modules.rst index 149c7775d6..f29e524306 100644 --- a/docs/source/learn/making-modules.rst +++ b/docs/source/learn/making-modules.rst @@ -36,6 +36,7 @@ and subclasses of either: making-modules/messaging/overview making-modules/messaging/message-objects making-modules/messaging/creating-message-types + making-modules/messaging/mission-parameters making-modules/module-adapter making-modules/module-python making-modules/module-testing diff --git a/docs/source/learn/making-modules/messaging/mission-parameters.rst b/docs/source/learn/making-modules/messaging/mission-parameters.rst new file mode 100644 index 0000000000..84c86383d5 --- /dev/null +++ b/docs/source/learn/making-modules/messaging/mission-parameters.rst @@ -0,0 +1,115 @@ +.. _messaging-mission-parameters: + +Configuring Mission Parameters +============================== + +Several message payloads and modules get their array bounds from a small set of +project-wide constants. These constants include sensor counts and effector +counts. They are in one header: + +``mission/parameters.h`` + +One header only is in use for a build. That header gives every constant. The +build does not add values to it, and it does not give a default value for a +missing constant. + +The Mission Parameters +---------------------- + +============================ ======= ======================================== +Constant Default Meaning +============================ ======= ======================================== +``MAX_KEY_POINTS`` 5000 Optical flow key points +``MAX_NUM_CSS_SENSORS`` 32 Coarse sun sensors in a constellation +``MAX_EFF_CNT`` 36 Generic effectors (thrusters and others) +``RW_EFF_CNT`` 36 Reaction wheels +``MAX_SICP_POINTS`` 5000 Point-cloud points +``SICP_POINT_DIM`` 3 Point-cloud point dimension +``MAX_SICP_ITERATIONS`` 250 Point-cloud registration iterations +``MAX_NUMBER_REGIONS`` 3 Regions of interest +============================ ======= ======================================== + +Each constant gives a bound for a message payload array. Thus all the constants +are part of the messaging ABI, and SWIG gives all of them to the Python layer. +The default values are large. Thus a build with no mission of its own operates +correctly. + +Where the Values Come From +-------------------------- + +The array bounds belong to the mission, and a mission comes to the build as a +module root. A module root declares its own values with this call: + +.. code-block:: cmake + + xmera_provide_mission_parameters("${CMAKE_CURRENT_SOURCE_DIR}") + +Put this call in the ``CMakeLists.txt`` of the module root. The argument is the +directory that *contains* a ``mission`` folder. When you add that root to +``XMERA_MODULE_ROOTS``, you also select the array bounds that its code uses. +The two are one selection. Thus they cannot become different, and there is no +second control to set. + +The configure step shows the header that it resolved: + +.. code-block:: text + + -- Mission parameters: /path/to/myMission/mission/parameters.h + +One source only is permitted. If two module roots declare parameters, the +configure step stops and gives the two directories. This check is necessary. +Before this check, a build compiled its Python message payloads against one set +of bounds, and its algorithm objects against a different set. The build gave no +error message. The build showed incorrect results only when a struct moved +across an FFI boundary. + +If no module root declares a directory, the build uses +``src/defaults/mission/parameters.h``. That header gives the default values in +the table above. + +Supplying Your Own Values +------------------------- + +Make a ``mission/parameters.h`` file in your module root. Give a value for +**every** constant in the table. Some of the constants are not in your own +code, but a payload in a different part of the build uses them: + +.. code-block:: c + + #ifndef MISSION_PARAMETERS_H + #define MISSION_PARAMETERS_H + + // clang-format off + #define MAX_NUM_CSS_SENSORS 12 + #define MAX_EFF_CNT 12 + #define RW_EFF_CNT 4 + + #define MAX_KEY_POINTS 5000 + #define MAX_SICP_POINTS 5000 + #define SICP_POINT_DIM 3 + #define MAX_SICP_ITERATIONS 250 + #define MAX_NUMBER_REGIONS 3 + // clang-format on + + #endif + +Then declare the directory from the ``CMakeLists.txt`` of the module root, as +in the example above. The configure step does not find a missing constant. The +first payload or algorithm that uses that constant gives a compilation error. + +Writing the Header +------------------ + +Write the values as plain integers, in the ``clang-format off`` guard in the +example above. SWIG also reads this header, to give the constants to the Python +layer. Its preprocessor reads the quote in a digit separator, for example +``5'000``, as the start of a character literal. SWIG then discards every +constant after that quote. The result is a missing attribute on +``xmera.architecture.messaging``, and not a build error. + +The guard is necessary because the ``.clang-format`` file of this project sets +``IntegerLiteralSeparator``, which adds those separators for you. + +``test_missionParameters.py`` prevents that class of failure. The test compares +the compiled array extent of each payload against the constant that SWIG gives +to Python.