eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Merge remote-tracking branch 'origin/develop' into mueller/pus-15-tm-storage
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2023-02-08 17:43:43 +01:00
commit a4d551e420
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
64 changed files with 2916 additions and 2029 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
CHANGELOG.md
View File

@ -17,6 +17,61 @@ change warranting a new major release:
# [unreleased] # [unreleased]
# [v1.26.1] 2023-02-08
- Initialize parameter helper in ACS controller.
# [v1.26.0] 2023-02-08
eive-tmtc v2.12.1
## Changed
### ACS
- Readded calibration matrices for MGM calibration.
- Added calculation of satellite velocity vector from GPS position data
- Added detumble mode using GYR values
- Added inertial pointing mode
- Added nadir pointing mode
- Added ground station target mode
- Added antistiction for RWs
- Added `sunTargetSafe` differentiation for LEOP
- Added check for existance of `SD_0_SKEWED_PTG_FILE` and `SD_1_SKEWED_PTG_FILE` to determine
which `sunTargetSafe` to use
- Added `gpsVelocity` and `gpsPosition` to `gpsProcessed`
- Removed deprecated `OutputValues`
- Added `HasParametersIF` to `AcsParameters`
- Added `ReceivesParameterMessagesIF` and `ParameterHelper` to `AcsController`
- Updated `AcsParameters` with actual values and changed structure
- Sun vector model and magnetic field vector model calculations are always executed now
- `domainId` is now used as identifier for parameter structs
- Changed onboard GYR value handling from deg/s to rad/s
## Fixed
- Single sourcing the version information into `CMakeLists.txt`. The `git describe` functionality
is only used to retrieve the git SHA hash now. Also removed `OBSWVersion.h` accordingly.
- Build system: Fixed small bug, where the version itself was
stored as the git SHA hash in `commonConfig.h`. This will be
an empty string now for regular versions.
- Bump FSFW for important fix in PUS mode service.
### ACS
- Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being
transformed in body RF. However, the calibration values are in the body RF. Also fixed the
validity flag of 'mgmVecTotDerivative'.
- Fixed calculation of model sun vector
- Fixed calculation of model magnetic field vector
- Fixed MEKF algorithm
- Fixed several variable initializations
- Fixed several variable types
- Fixed use of `sunMagAngleMin` for safe mode
- Fixed MEKF not using correct `sampleTime`
- Fixed assignment of `SUS0` and `SUS6` calibration matrices due to wiring being mixed up
- Various smaller bugfixes
# [v1.25.0] 2023-02-06 # [v1.25.0] 2023-02-06
eive-tmtc version: v2.12.0 eive-tmtc version: v2.12.0
@ -52,6 +107,10 @@ eive-tmtc version: v2.12.0
## Changed ## Changed
- Update ACS scheduling to represent the actual ACS design. There is one ACS PST now for all
timing sensitive ACS operations. In the debug builds, the new ACS polling sequence table
will have a period of 0.6 seconds, but will remain 0.4 seconds for the release build.
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365
- `ACS::SensorValues` is now an ACS controller member to reduce the risk of stack overflow. - `ACS::SensorValues` is now an ACS controller member to reduce the risk of stack overflow.
- ACS Subsystem Sequence Mode IDs updated. - ACS Subsystem Sequence Mode IDs updated.
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365 PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365

23
CMakeLists.txt
View File

@ -9,9 +9,9 @@
# ############################################################################## # ##############################################################################
cmake_minimum_required(VERSION 3.13) cmake_minimum_required(VERSION 3.13)
set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1) set(OBSW_VERSION_MAJOR 1)
set(OBSW_VERSION_MINOR_IF_GIT_FAILS 25) set(OBSW_VERSION_MINOR 26)
set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0) set(OBSW_VERSION_REVISION 1)
# set(CMAKE_VERBOSE TRUE) # set(CMAKE_VERBOSE TRUE)
@ -168,10 +168,13 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git)
set(GIT_INFO set(GIT_INFO
${GIT_INFO} ${GIT_INFO}
CACHE STRING "Version information retrieved with git describe") CACHE STRING "Version information retrieved with git describe")
list(GET GIT_INFO 1 OBSW_VERSION_MAJOR) # CMakeLists.txt is now single source of information. list(GET GIT_INFO 1
list(GET GIT_INFO 2 OBSW_VERSION_MINOR) # OBSW_VERSION_MAJOR) list(GET GIT_INFO 2 OBSW_VERSION_MINOR) list(GET
list(GET GIT_INFO 3 OBSW_VERSION_REVISION) # GIT_INFO 3 OBSW_VERSION_REVISION)
list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1) list(LENGTH GIT_INFO LIST_LEN)
if(LIST_LEN GREATER 4)
list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1)
endif()
if(NOT OBSW_VERSION_MAJOR) if(NOT OBSW_VERSION_MAJOR)
set(OBSW_VERSION_MAJOR ${OBSW_VERSION_MAJOR_IF_GIT_FAILS}) set(OBSW_VERSION_MAJOR ${OBSW_VERSION_MAJOR_IF_GIT_FAILS})
endif() endif()
@ -302,6 +305,9 @@ else()
endif() endif()
endif() endif()
include(BuildType)
set_build_type()
# Configuration files # Configuration files
configure_file(${COMMON_CONFIG_PATH}/commonConfig.h.in commonConfig.h) configure_file(${COMMON_CONFIG_PATH}/commonConfig.h.in commonConfig.h)
configure_file(${FSFW_CONFIG_PATH}/FSFWConfig.h.in FSFWConfig.h) configure_file(${FSFW_CONFIG_PATH}/FSFWConfig.h.in FSFWConfig.h)
@ -560,6 +566,3 @@ add_custom_command(
POST_BUILD POST_BUILD
COMMAND ${CMAKE_SIZE} ${OBSW_BIN_NAME}${FILE_SUFFIX} COMMAND ${CMAKE_SIZE} ${OBSW_BIN_NAME}${FILE_SUFFIX}
COMMENT ${POST_BUILD_COMMENT}) COMMENT ${POST_BUILD_COMMENT})
include(BuildType)
set_build_type()

1
bsp_hosted/OBSWConfig.h.in
View File

@ -7,7 +7,6 @@
#define FSFWCONFIG_OBSWCONFIG_H_ #define FSFWCONFIG_OBSWCONFIG_H_
#include "commonConfig.h" #include "commonConfig.h"
#include "OBSWVersion.h"
/*******************************************************************/ /*******************************************************************/
/** All of the following flags should be enabled for mission code */ /** All of the following flags should be enabled for mission code */

3
bsp_q7s/OBSWConfig.h.in
View File

@ -8,7 +8,8 @@
#include "commonConfig.h" #include "commonConfig.h"
#include "q7sConfig.h" #include "q7sConfig.h"
#include "OBSWVersion.h"
#cmakedefine RELEASE_BUILD
/*******************************************************************/ /*******************************************************************/
/** All of the following flags should be enabled for mission code */ /** All of the following flags should be enabled for mission code */

28
bsp_q7s/core/CoreController.cpp
View File

@ -5,7 +5,7 @@
#include <fsfw/ipc/QueueFactory.h> #include <fsfw/ipc/QueueFactory.h>
#include <fsfw/tasks/TaskFactory.h> #include <fsfw/tasks/TaskFactory.h>
#include "OBSWVersion.h" #include "commonConfig.h"
#include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/timemanager/Stopwatch.h" #include "fsfw/timemanager/Stopwatch.h"
#include "fsfw/version.h" #include "fsfw/version.h"
@ -179,6 +179,26 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() {
ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) { const uint8_t *data, size_t size) {
switch (actionId) { switch (actionId) {
case (ANNOUNCE_VERSION): {
uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) |
(common::OBSW_VERSION_REVISION << 8);
uint32_t p2 = 0;
if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) {
p1 |= 1;
auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1);
size_t posDash = shaAsStr.find("-");
auto gitHash = shaAsStr.substr(posDash + 2, 4);
// Only copy first 4 letters of git hash
memcpy(&p2, gitHash.c_str(), 4);
}
triggerEvent(VERSION_INFO, p1, p2);
return HasActionsIF::EXECUTION_FINISHED;
}
case (ANNOUNCE_CURRENT_IMAGE): {
triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY);
return HasActionsIF::EXECUTION_FINISHED;
}
case (LIST_DIRECTORY_INTO_FILE): { case (LIST_DIRECTORY_INTO_FILE): {
return actionListDirectoryIntoFile(actionId, commandedBy, data, size); return actionListDirectoryIntoFile(actionId, commandedBy, data, size);
} }
@ -673,9 +693,9 @@ ReturnValue_t CoreController::initVersionFile() {
sif::warning << "CoreController::versionFileInit: Retrieving uname line failed" << std::endl; sif::warning << "CoreController::versionFileInit: Retrieving uname line failed" << std::endl;
} }
std::string fullObswVersionString = "OBSW: v" + std::to_string(SW_VERSION) + "." + std::string fullObswVersionString = "OBSW: v" + std::to_string(common::OBSW_VERSION_MAJOR) + "." +
std::to_string(SW_SUBVERSION) + "." + std::to_string(common::OBSW_VERSION_MINOR) + "." +
std::to_string(SW_REVISION); std::to_string(common::OBSW_VERSION_REVISION);
char versionString[16] = {}; char versionString[16] = {};
fsfw::FSFW_VERSION.getVersion(versionString, sizeof(versionString)); fsfw::FSFW_VERSION.getVersion(versionString, sizeof(versionString));
std::string fullFsfwVersionString = "FSFW: v" + std::string(versionString); std::string fullFsfwVersionString = "FSFW: v" + std::string(versionString);

8
bsp_q7s/core/CoreController.h
View File

@ -74,6 +74,8 @@ class CoreController : public ExtendedControllerBase {
static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000; static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000;
static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0; static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0;
static constexpr ActionId_t ANNOUNCE_VERSION = 1;
static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2;
static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5; static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5;
static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6; static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6;
static constexpr ActionId_t SWITCH_IMG_LOCK = 7; static constexpr ActionId_t SWITCH_IMG_LOCK = 7;
@ -109,6 +111,12 @@ class CoreController : public ExtendedControllerBase {
//! [EXPORT] : [COMMENT] No SD card was active. Core controller will attempt to re-initialize //! [EXPORT] : [COMMENT] No SD card was active. Core controller will attempt to re-initialize
//! a SD card. //! a SD card.
static constexpr Event NO_SD_CARD_ACTIVE = event::makeEvent(SUBSYSTEM_ID, 4, severity::HIGH); static constexpr Event NO_SD_CARD_ACTIVE = event::makeEvent(SUBSYSTEM_ID, 4, severity::HIGH);
//! [EXPORT] : [COMMENT]
//! P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash
//! P2: First four letters of Git SHA is the last byte of P1 is set.
static constexpr Event VERSION_INFO = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO);
//! [EXPORT] : [COMMENT] P1: Current Chip, P2: Current Copy
static constexpr Event CURRENT_IMAGE_INFO = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO);
CoreController(object_id_t objectId); CoreController(object_id_t objectId);
virtual ~CoreController(); virtual ~CoreController();

96
bsp_q7s/core/scheduling.cpp
View File

@ -144,11 +144,12 @@ void scheduling::initTasks() {
#endif #endif
PeriodicTaskIF* comTask = factory->createPeriodicTask( PeriodicTaskIF* comTask = factory->createPeriodicTask(
"CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); "COM_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc);
result = comTask->addComponent(objects::COM_SUBSYSTEM); result = comTask->addComponent(objects::COM_SUBSYSTEM);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
scheduling::printAddObjectError("COM subsystem", objects::COM_SUBSYSTEM); scheduling::printAddObjectError("COM subsystem", objects::COM_SUBSYSTEM);
} }
#if OBSW_ADD_CCSDS_IP_CORES == 1 #if OBSW_ADD_CCSDS_IP_CORES == 1
result = comTask->addComponent(objects::CCSDS_HANDLER); result = comTask->addComponent(objects::CCSDS_HANDLER);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
@ -183,15 +184,8 @@ void scheduling::initTasks() {
#endif /* OBSW_ADD_GPS_CTRL */ #endif /* OBSW_ADD_GPS_CTRL */
PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
"SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); "SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
static_cast<void>(acsSysTask); static_cast<void>(acsSysTask);
// To be removed soon because it will be part of the ACS PST.
#if OBSW_ADD_ACS_CTRL == 1
acsSysTask->addComponent(objects::ACS_CONTROLLER);
if (result != returnvalue::OK) {
scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER);
}
#endif
#if OBSW_ADD_ACS_BOARD == 1 #if OBSW_ADD_ACS_BOARD == 1
result = acsSysTask->addComponent(objects::ACS_BOARD_ASS); result = acsSysTask->addComponent(objects::ACS_BOARD_ASS);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
@ -215,35 +209,6 @@ void scheduling::initTasks() {
scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM); scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
} }
#if OBSW_Q7S_EM == 1
acsSysTask->addComponent(objects::MGM_0_LIS3_HANDLER);
acsSysTask->addComponent(objects::MGM_1_RM3100_HANDLER);
acsSysTask->addComponent(objects::MGM_2_LIS3_HANDLER);
acsSysTask->addComponent(objects::MGM_3_RM3100_HANDLER);
acsSysTask->addComponent(objects::IMTQ_HANDLER);
acsSysTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
acsSysTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
acsSysTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
acsSysTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
acsSysTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
acsSysTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
acsSysTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
acsSysTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
acsSysTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
acsSysTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
acsSysTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
acsSysTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
acsSysTask->addComponent(objects::GYRO_0_ADIS_HANDLER);
acsSysTask->addComponent(objects::GYRO_1_L3G_HANDLER);
acsSysTask->addComponent(objects::GYRO_2_ADIS_HANDLER);
acsSysTask->addComponent(objects::GYRO_3_L3G_HANDLER);
acsSysTask->addComponent(objects::GPS_CONTROLLER);
acsSysTask->addComponent(objects::STAR_TRACKER);
acsSysTask->addComponent(objects::RW1);
acsSysTask->addComponent(objects::RW2);
acsSysTask->addComponent(objects::RW3);
acsSysTask->addComponent(objects::RW4);
#endif
#if OBSW_ADD_RTD_DEVICES == 1 #if OBSW_ADD_RTD_DEVICES == 1
PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask( PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask(
"TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc); "TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc);
@ -440,11 +405,30 @@ void scheduling::initTasks() {
void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc,
std::vector<PeriodicTaskIF*>& taskVec) { std::vector<PeriodicTaskIF*>& taskVec) {
ReturnValue_t result = returnvalue::OK; ReturnValue_t result = returnvalue::OK;
#ifdef RELEASE_BUILD
static constexpr float acsPstPeriod = 0.4;
#else
static constexpr float acsPstPeriod = 0.6;
#endif
FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask(
"ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
result = pst::pstAcs(acsPst);
if (result != returnvalue::OK) {
if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
sif::warning << "scheduling::initTasks: ACS PST is empty" << std::endl;
} else {
sif::error << "scheduling::initTasks: Creating ACS PST failed!" << std::endl;
}
} else {
taskVec.push_back(acsPst);
}
/* Polling Sequence Table Default */ /* Polling Sequence Table Default */
#if OBSW_ADD_SPI_TEST_CODE == 0 #if OBSW_ADD_SPI_TEST_CODE == 0
FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask( FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask(
"MAIN_SPI", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); "MAIN_SPI", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc);
result = pst::pstSpi(spiPst); result = pst::pstSpiAndSyrlinks(spiPst);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl; sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl;
@ -456,37 +440,9 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
} }
#endif #endif
#if OBSW_ADD_RW == 1
FixedTimeslotTaskIF* rwPstTask = factory.createFixedTimeslotTask(
"RW_SPI", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc);
result = pst::pstSpiRw(rwPstTask);
if (result != returnvalue::OK) {
if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl;
} else {
sif::error << "scheduling::initTasks: Creating SPI PST failed!" << std::endl;
}
} else {
taskVec.push_back(rwPstTask);
}
#endif
FixedTimeslotTaskIF* uartPst = factory.createFixedTimeslotTask(
"UART_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
result = pst::pstUart(uartPst);
if (result != returnvalue::OK) {
if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
sif::warning << "scheduling::initTasks: UART PST is empty" << std::endl;
} else {
sif::error << "scheduling::initTasks: Creating UART PST failed!" << std::endl;
}
} else {
taskVec.push_back(uartPst);
}
#if OBSW_ADD_I2C_TEST_CODE == 0 #if OBSW_ADD_I2C_TEST_CODE == 0
FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask( FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask(
"I2C_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc); "I2C_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
result = pst::pstI2c(i2cPst); result = pst::pstI2c(i2cPst);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
@ -501,7 +457,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
#if OBSW_ADD_GOMSPACE_PCDU == 1 #if OBSW_ADD_GOMSPACE_PCDU == 1
FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask( FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask(
"GS_PST_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc); "GS_PST_TASK", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
result = pst::pstGompaceCan(gomSpacePstTask); result = pst::pstGompaceCan(gomSpacePstTask);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {

3
cmake/BuildType.cmake
View File

@ -21,10 +21,13 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
) )
endif() endif()
set(RELEASE_BUILD 1 PARENT_SCOPE)
if(${CMAKE_BUILD_TYPE} MATCHES "Debug") if(${CMAKE_BUILD_TYPE} MATCHES "Debug")
message(STATUS message(STATUS
"Building Debug application with flags: ${CMAKE_C_FLAGS_DEBUG}" "Building Debug application with flags: ${CMAKE_C_FLAGS_DEBUG}"
) )
set(RELEASE_BUILD 0 PARENT_SCOPE)
elseif(${CMAKE_BUILD_TYPE} MATCHES "RelWithDebInfo") elseif(${CMAKE_BUILD_TYPE} MATCHES "RelWithDebInfo")
message(STATUS message(STATUS
"Building Release (Debug) application with " "Building Release (Debug) application with "

6
cmake/EiveHelpers.cmake
View File

@ -4,7 +4,7 @@
# 2. Major version # 2. Major version
# 3. Minor version # 3. Minor version
# 4. Revision # 4. Revision
# 5. git SHA hash and commits since tag # 5. (Optional) git SHA hash and commits since tag when applicable
function(determine_version_with_git) function(determine_version_with_git)
include(GetGitRevisionDescription) include(GetGitRevisionDescription)
git_describe(VERSION ${ARGN}) git_describe(VERSION ${ARGN})
@ -22,7 +22,9 @@ function(determine_version_with_git)
list(APPEND GIT_INFO ${_VERSION_MAJOR}) list(APPEND GIT_INFO ${_VERSION_MAJOR})
list(APPEND GIT_INFO ${_VERSION_MINOR}) list(APPEND GIT_INFO ${_VERSION_MINOR})
list(APPEND GIT_INFO ${_VERSION_PATCH}) list(APPEND GIT_INFO ${_VERSION_PATCH})
list(APPEND GIT_INFO ${VERSION_SHA1}) if(NOT VERSION_SHA1 STREQUAL VERSION)
list(APPEND GIT_INFO ${VERSION_SHA1})
endif()
set(GIT_INFO ${GIT_INFO} PARENT_SCOPE) set(GIT_INFO ${GIT_INFO} PARENT_SCOPE)
message(STATUS "eive | Set git version info into GIT_INFO from the git tag ${VERSION}") message(STATUS "eive | Set git version info into GIT_INFO from the git tag ${VERSION}")
endfunction() endfunction()

10
common/config/OBSWVersion.h
View File

@ -1,10 +0,0 @@
#ifndef COMMON_CONFIG_OBSWVERSION_H_
#define COMMON_CONFIG_OBSWVERSION_H_
const char* const SW_NAME = "eive";
#define SW_VERSION 1
#define SW_SUBVERSION 12
#define SW_REVISION 1
#endif /* COMMON_CONFIG_OBSWVERSION_H_ */

5
common/config/eive/resultClassIds.h
View File

@ -35,6 +35,11 @@ enum commonClassIds : uint8_t {
SA_DEPL_HANDLER, // SADPL SA_DEPL_HANDLER, // SADPL
MPSOC_RETURN_VALUES_IF, // MPSOCRTVIF MPSOC_RETURN_VALUES_IF, // MPSOCRTVIF
SUPV_RETURN_VALUES_IF, // SPVRTVIF SUPV_RETURN_VALUES_IF, // SPVRTVIF
ACS_KALMAN, // ACSKAL
ACS_SAFE, // ACSSAF
ACS_PTG, // ACSPTG
ACS_DETUMBLE, // ACSDTB
ACS_MEKF, // ACSMEK
COMMON_CLASS_ID_END // [EXPORT] : [END] COMMON_CLASS_ID_END // [EXPORT] : [END]
}; };

2
fsfw

@ -1 +1 @@
Subproject commit a1567de9e8d84a044b4a33ccdaffd15f0f4f54f1 Subproject commit 7f907fb9d36be411d98ff05f3efc8cbd642904f2

3
generators/bsp_q7s_events.csv
View File

@ -198,6 +198,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/objects/SusAssembly.h 12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/objects/SusAssembly.h
13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;;mission/system/objects/TcsBoardAssembly.h 13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;;mission/system/objects/TcsBoardAssembly.h
13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/devices/devicedefinitions/GPSDefinitions.h 13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/devices/devicedefinitions/GPSDefinitions.h
13101;0x332d;CANT_GET_FIX;LOW;Could not get fix in maximum allowed time. P1: Maximum allowed time to get a fix after the GPS was switched on.;mission/devices/devicedefinitions/GPSDefinitions.h
13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/devices/P60DockHandler.h 13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/devices/P60DockHandler.h
13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/devices/P60DockHandler.h 13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/devices/P60DockHandler.h
13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/devices/P60DockHandler.h 13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/devices/P60DockHandler.h
@ -244,3 +245,5 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h
14003;0x36b3;REBOOT_HW;MEDIUM;;bsp_q7s/core/CoreController.h 14003;0x36b3;REBOOT_HW;MEDIUM;;bsp_q7s/core/CoreController.h
14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h
14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h
14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h

1 Event ID (dec) Event ID (hex) Name Severity Description File Path
198 12903 0x3267 SIDE_SWITCH_TRANSITION_NOT_ALLOWED LOW Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination mission/system/objects/SusAssembly.h
199 13000 0x32c8 CHILDREN_LOST_MODE MEDIUM mission/system/objects/TcsBoardAssembly.h
200 13100 0x332c GPS_FIX_CHANGE INFO Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix mission/devices/devicedefinitions/GPSDefinitions.h
201 13101 0x332d CANT_GET_FIX LOW Could not get fix in maximum allowed time. P1: Maximum allowed time to get a fix after the GPS was switched on. mission/devices/devicedefinitions/GPSDefinitions.h
202 13200 0x3390 P60_BOOT_COUNT INFO P60 boot count is broadcasted once at SW startup. P1: Boot count mission/devices/P60DockHandler.h
203 13201 0x3391 BATT_MODE INFO Battery mode is broadcasted at startup. P1: Mode mission/devices/P60DockHandler.h
204 13202 0x3392 BATT_MODE_CHANGED MEDIUM Battery mode has changed. P1: Old mode. P2: New mode mission/devices/P60DockHandler.h
245 14002 0x36b2 REBOOT_MECHANISM_TRIGGERED MEDIUM The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots bsp_q7s/core/CoreController.h
246 14003 0x36b3 REBOOT_HW MEDIUM bsp_q7s/core/CoreController.h
247 14004 0x36b4 NO_SD_CARD_ACTIVE HIGH No SD card was active. Core controller will attempt to re-initialize a SD card. bsp_q7s/core/CoreController.h
248 14005 0x36b5 VERSION_INFO INFO P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set. bsp_q7s/core/CoreController.h
249 14006 0x36b6 CURRENT_IMAGE_INFO INFO P1: Current Chip, P2: Current Copy bsp_q7s/core/CoreController.h

31
generators/bsp_q7s_returnvalues.csv
View File

@ -51,9 +51,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h
0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h
0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h
0x6001;CCSDS_KalmanNoGyrMeas;;1;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a01;ACSSAF_SafectrlMekfInputInvalid;;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
0x6002;CCSDS_KalmanNoModel;;2;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6b01;ACSPTG_PtgctrlMekfInputInvalid;;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
0x6003;CCSDS_KalmanInversionFailed;;3;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6c01;ACSDTB_DetumbleNoSensordata;;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
0x6901;ACSKAL_KalmanNoGyrMeas;;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
0x6902;ACSKAL_KalmanNoModel;;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
0x6903;ACSKAL_KalmanInversionFailed;;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
@ -468,16 +471,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h 0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h 0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h 0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
0x6b00;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h 0x7000;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h
0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h
0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h
0x6a00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a03;SDMA_AlreadyOff;;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f03;SDMA_AlreadyOff;;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a0a;SDMA_StatusFileNexists;;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f0a;SDMA_StatusFileNexists;;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a0b;SDMA_StatusFileFormatInvalid;;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f0b;SDMA_StatusFileFormatInvalid;;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a0c;SDMA_MountError;;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f0c;SDMA_MountError;;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
0x6a0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6f0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h

1 Full ID (hex) Name Description Unique ID Subsytem Name File Path
51 0x4fa4 HEATER_MainSwitchSetTimeout 164 HEATER_HANDLER mission/devices/HeaterHandler.h
52 0x4fa5 HEATER_CommandAlreadyWaiting 165 HEATER_HANDLER mission/devices/HeaterHandler.h
53 0x60a0 CCSDS_CommandNotImplemented Received action message with unknown action id 160 CCSDS_HANDLER mission/tmtc/CcsdsIpCoreHandler.h
54 0x6001 0x6a01 CCSDS_KalmanNoGyrMeas ACSSAF_SafectrlMekfInputInvalid 1 CCSDS_HANDLER ACS_SAFE mission/controller/acs/MultiplicativeKalmanFilter.h mission/controller/acs/control/SafeCtrl.h
55 0x6002 0x6b01 CCSDS_KalmanNoModel ACSPTG_PtgctrlMekfInputInvalid 2 1 CCSDS_HANDLER ACS_PTG mission/controller/acs/MultiplicativeKalmanFilter.h mission/controller/acs/control/PtgCtrl.h
56 0x6003 0x6c01 CCSDS_KalmanInversionFailed ACSDTB_DetumbleNoSensordata 3 1 CCSDS_HANDLER ACS_DETUMBLE mission/controller/acs/MultiplicativeKalmanFilter.h mission/controller/acs/control/Detumble.h
57 0x6901 ACSKAL_KalmanNoGyrMeas 1 ACS_KALMAN mission/controller/acs/MultiplicativeKalmanFilter.h
58 0x6902 ACSKAL_KalmanNoModel 2 ACS_KALMAN mission/controller/acs/MultiplicativeKalmanFilter.h
59 0x6903 ACSKAL_KalmanInversionFailed 3 ACS_KALMAN mission/controller/acs/MultiplicativeKalmanFilter.h
60 0x4500 HSPI_OpeningFileFailed 0 HAL_SPI fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
61 0x4501 HSPI_FullDuplexTransferFailed 1 HAL_SPI fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
62 0x4502 HSPI_HalfDuplexTransferFailed 2 HAL_SPI fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
471 0x1d03 ATC_IllegalApplicationData 3 ACCEPTS_TELECOMMANDS_IF fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
472 0x1d04 ATC_SendTmFailed 4 ACCEPTS_TELECOMMANDS_IF fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
473 0x1d05 ATC_Timeout 5 ACCEPTS_TELECOMMANDS_IF fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
474 0x6b00 0x7000 SCBU_KeyNotFound 0 SCRATCH_BUFFER bsp_q7s/memory/scratchApi.h
475 0x64a0 FSHLP_SdNotMounted SD card specified with path string not mounted 160 FILE_SYSTEM_HELPER bsp_q7s/fs/FilesystemHelper.h
476 0x64a1 FSHLP_FileNotExists Specified file does not exist on filesystem 161 FILE_SYSTEM_HELPER bsp_q7s/fs/FilesystemHelper.h
477 0x6a00 0x6f00 SDMA_OpOngoing 0 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
478 0x6a01 0x6f01 SDMA_AlreadyOn 1 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
479 0x6a02 0x6f02 SDMA_AlreadyMounted 2 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
480 0x6a03 0x6f03 SDMA_AlreadyOff 3 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
481 0x6a0a 0x6f0a SDMA_StatusFileNexists 10 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
482 0x6a0b 0x6f0b SDMA_StatusFileFormatInvalid 11 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
483 0x6a0c 0x6f0c SDMA_MountError 12 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
484 0x6a0d 0x6f0d SDMA_UnmountError 13 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
485 0x6a0e 0x6f0e SDMA_SystemCallError 14 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h
486 0x6a0f 0x6f0f SDMA_PopenCallError 15 SD_CARD_MANAGER bsp_q7s/fs/SdCardManager.h

5
generators/events/event_parser.py
View File

@ -29,6 +29,7 @@ GENERATE_CSV = True
COPY_CPP_FILE = True COPY_CPP_FILE = True
COPY_CPP_H_FILE = True COPY_CPP_H_FILE = True
MOVE_CSV_FILE = True MOVE_CSV_FILE = True
PRINT_EVENTS = False
PARSE_HOST_BSP = True PARSE_HOST_BSP = True
@ -80,13 +81,13 @@ LOGGER = get_console_logger()
def parse_events( def parse_events(
generate_csv: bool = True, generate_cpp: bool = True, print_events: bool = True generate_csv: bool = True, generate_cpp: bool = True
): ):
LOGGER.info("EventParser: Parsing events: ") LOGGER.info("EventParser: Parsing events: ")
# Small delay for clean printout # Small delay for clean printout
time.sleep(0.01) time.sleep(0.01)
event_list = generate_event_list() event_list = generate_event_list()
if print_events: if PRINT_EVENTS:
PrettyPrinter.pprint(event_list) PrettyPrinter.pprint(event_list)
# Delay for clean printout # Delay for clean printout
time.sleep(0.1) time.sleep(0.1)

13
generators/events/translateEvents.cpp
View File

@ -1,7 +1,7 @@
/** /**
* @brief Auto-generated event translation file. Contains 245 translations. * @brief Auto-generated event translation file. Contains 248 translations.
* @details * @details
* Generated on: 2023-02-03 10:52:53 * Generated on: 2023-02-08 14:09:40
*/ */
#include "translateEvents.h" #include "translateEvents.h"
@ -200,6 +200,7 @@ const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT";
const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED"; const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED";
const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE"; const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE"; const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT"; const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
const char *BATT_MODE_STRING = "BATT_MODE"; const char *BATT_MODE_STRING = "BATT_MODE";
const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED"; const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
@ -245,6 +246,8 @@ const char *REBOOT_SW_STRING = "REBOOT_SW";
const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED";
const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *REBOOT_HW_STRING = "REBOOT_HW";
const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
const char *VERSION_INFO_STRING = "VERSION_INFO";
const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
const char *translateEvents(Event event) { const char *translateEvents(Event event) {
switch ((event & 0xFFFF)) { switch ((event & 0xFFFF)) {
@ -638,6 +641,8 @@ const char *translateEvents(Event event) {
return CHILDREN_LOST_MODE_STRING; return CHILDREN_LOST_MODE_STRING;
case (13100): case (13100):
return GPS_FIX_CHANGE_STRING; return GPS_FIX_CHANGE_STRING;
case (13101):
return CANT_GET_FIX_STRING;
case (13200): case (13200):
return P60_BOOT_COUNT_STRING; return P60_BOOT_COUNT_STRING;
case (13201): case (13201):
@ -728,6 +733,10 @@ const char *translateEvents(Event event) {
return REBOOT_HW_STRING; return REBOOT_HW_STRING;
case (14004): case (14004):
return NO_SD_CARD_ACTIVE_STRING; return NO_SD_CARD_ACTIVE_STRING;
case (14005):
return VERSION_INFO_STRING;
case (14006):
return CURRENT_IMAGE_INFO_STRING;
default: default:
return "UNKNOWN_EVENT"; return "UNKNOWN_EVENT";
} }

2
generators/objects/translateObjects.cpp
View File

@ -2,7 +2,7 @@
* @brief Auto-generated object translation file. * @brief Auto-generated object translation file.
* @details * @details
* Contains 152 translations. * Contains 152 translations.
* Generated on: 2023-02-03 10:52:53 * Generated on: 2023-02-08 14:09:40
*/ */
#include "translateObjects.h" #include "translateObjects.h"

2
generators/returnvalues/returnvalues_parser.py
View File

@ -22,7 +22,7 @@ LOGGER = get_console_logger()
EXPORT_TO_FILE = True EXPORT_TO_FILE = True
COPY_CSV_FILE = True COPY_CSV_FILE = True
EXPORT_TO_SQL = True EXPORT_TO_SQL = True
PRINT_TABLES = True PRINT_TABLES = False
FILE_SEPARATOR = ";" FILE_SEPARATOR = ";"

4
linux/ObjectFactory.cpp
View File

@ -363,6 +363,6 @@ void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) {
void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler, void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler,
PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) { PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) {
cfdpFunnel.addDestination(ipCoreHandler, config::LIVE_TM); cfdpFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM);
pusFunnel.addDestination(ipCoreHandler, config::LIVE_TM); pusFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM);
} }

173
linux/devices/GpsHyperionLinuxController.cpp
View File

@ -34,7 +34,6 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
uint32_t *msToReachTheMode) { uint32_t *msToReachTheMode) {
if (not modeCommanded) { if (not modeCommanded) {
if (mode == MODE_ON or mode == MODE_OFF) { if (mode == MODE_ON or mode == MODE_OFF) {
gpsNotOpenSwitch = true;
// 5h time to reach fix // 5h time to reach fix
*msToReachTheMode = MAX_SECONDS_TO_REACH_FIX; *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
maxTimeToReachFix.resetTimer(); maxTimeToReachFix.resetTimer();
@ -44,7 +43,12 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
} }
} }
if (mode == MODE_OFF) { if (mode == MODE_OFF) {
PoolReadGuard pg(&gpsSet);
gpsSet.setValidity(false, true); gpsSet.setValidity(false, true);
// There can't be a fix with a device that is off.
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
oneShotSwitches.reset();
modeCommanded = false;
} }
return returnvalue::OK; return returnvalue::OK;
} }
@ -101,6 +105,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
if (not callAgainImmediately) { if (not callAgainImmediately) {
handleQueue(); handleQueue();
poolManager.performHkOperation(); poolManager.performHkOperation();
TaskFactory::delayTask(250);
} }
} }
// Should never be reached. // Should never be reached.
@ -113,25 +118,24 @@ ReturnValue_t GpsHyperionLinuxController::initialize() {
return result; return result;
} }
auto openError = [&](const char *type, int error) { auto openError = [&](const char *type, int error) {
if (gpsNotOpenSwitch) { // Opening failed
// Opening failed
#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_VERBOSE_LEVEL >= 1
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
<< " failed | Error " << error << " | " << gps_errstr(error) << std::endl; << " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
#endif #endif
gpsNotOpenSwitch = false;
}
}; };
if (readMode == ReadModes::SOCKET) { if (readMode == ReadModes::SOCKET) {
int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps); int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps);
if (retval != 0) { if (retval != 0) {
openError("Socket", retval); openError("Socket", retval);
return ObjectManager::CHILD_INIT_FAILED;
} }
gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr); gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
} else if (readMode == ReadModes::SHM) { } else if (readMode == ReadModes::SHM) {
int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps); int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
if (retval != 0) { if (retval != 0) {
openError("SHM", retval); openError("SHM", retval);
return ObjectManager::CHILD_INIT_FAILED;
} }
} }
return result; return result;
@ -145,32 +149,33 @@ void GpsHyperionLinuxController::performControlOperation() {}
bool GpsHyperionLinuxController::readGpsDataFromGpsd() { bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
auto readError = [&]() { auto readError = [&]() {
if (gpsReadFailedSwitch) { if (oneShotSwitches.gpsReadFailedSwitch) {
gpsReadFailedSwitch = false; oneShotSwitches.gpsReadFailedSwitch = false;
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | " sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
"Error " "Error "
<< errno << " | " << gps_errstr(errno) << std::endl; << errno << " | " << gps_errstr(errno) << std::endl;
} }
}; };
// GPS is off, no point in reading data from GPSD.
if (mode == MODE_OFF) {
return false;
}
if (readMode == ReadModes::SOCKET) { if (readMode == ReadModes::SOCKET) {
// Perform other necessary handling if not data seen for 0.2 seconds. // Poll the GPS.
if (gps_waiting(&gps, 200000)) { if (gps_waiting(&gps, 0)) {
if (-1 == gps_read(&gps)) { if (-1 == gps_read(&gps)) {
readError(); readError();
return false; return false;
} }
oneShotSwitches.gpsReadFailedSwitch = true;
if (MODE_SET != (MODE_SET & gps.set)) { if (MODE_SET != (MODE_SET & gps.set)) {
if (mode == MODE_ON) { if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and
if (noModeSetCntr >= 0) { oneShotSwitches.cantGetFixSwitch) {
noModeSetCntr++; sif::warning
} << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
if (noModeSetCntr == 10) { << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
// TODO: Trigger event here triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " oneShotSwitches.cantGetFixSwitch = false;
"read for 10 consecutive reads"
<< std::endl;
noModeSetCntr = -1;
}
// did not event get mode, nothing to see. // did not event get mode, nothing to see.
return false; return false;
} }
@ -198,51 +203,58 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
} }
bool validFix = false; bool validFix = false;
static_cast<void>(validFix);
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
int newFixMode = gps.fix.mode; if (gps.fix.mode == 2 or gps.fix.mode == 3) {
if (newFixMode == 2 or newFixMode == 3) {
validFix = true; validFix = true;
} }
if (gpsSet.fixMode.value != newFixMode) { if (gpsSet.fixMode.value != gps.fix.mode) {
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode); triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
} }
gpsSet.fixMode.value = newFixMode; gpsSet.fixMode.value = gps.fix.mode;
if (gps.fix.mode == 0 or gps.fix.mode == 1) { if (gps.fix.mode == 0 or gps.fix.mode == 1) {
if (modeCommanded and maxTimeToReachFix.hasTimedOut()) { if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
// We are supposed to be on and functioning, but not fix was found // We are supposed to be on and functioning, but no fix was found
if (mode == MODE_ON or mode == MODE_NORMAL) { if (mode == MODE_ON or mode == MODE_NORMAL) {
mode = MODE_OFF; mode = MODE_OFF;
} }
modeCommanded = false; modeCommanded = false;
} }
gpsSet.setValidity(false, true); gpsSet.setValidity(false, true);
} else if (gps.satellites_used > 0) { } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
gpsSet.setValidity(true, true); gpsSet.setValidity(true, true);
} }
gpsSet.satInUse.value = gps.satellites_used; gpsSet.satInUse.value = gps.satellites_used;
gpsSet.satInView.value = gps.satellites_visible; gpsSet.satInView.value = gps.satellites_visible;
bool latValid = false;
if (std::isfinite(gps.fix.latitude)) { if (std::isfinite(gps.fix.latitude)) {
// Negative latitude -> South direction // Negative latitude -> South direction
gpsSet.latitude.value = gps.fix.latitude; gpsSet.latitude.value = gps.fix.latitude;
} else { if (gps.fix.mode >= 2) {
gpsSet.latitude.setValid(false); latValid = true;
}
} }
gpsSet.latitude.setValid(latValid);
bool longValid = false;
if (std::isfinite(gps.fix.longitude)) { if (std::isfinite(gps.fix.longitude)) {
// Negative longitude -> West direction // Negative longitude -> West direction
gpsSet.longitude.value = gps.fix.longitude; gpsSet.longitude.value = gps.fix.longitude;
} else { if (gps.fix.mode >= 2) {
gpsSet.longitude.setValid(false); longValid = true;
}
} }
gpsSet.latitude.setValid(longValid);
bool altitudeValid = false;
if (std::isfinite(gps.fix.altitude)) { if (std::isfinite(gps.fix.altitude)) {
gpsSet.altitude.value = gps.fix.altitude; gpsSet.altitude.value = gps.fix.altitude;
} else { if (gps.fix.mode == 3) {
gpsSet.altitude.setValid(false); altitudeValid = true;
}
} }
gpsSet.altitude.setValid(altitudeValid);
if (std::isfinite(gps.fix.speed)) { if (std::isfinite(gps.fix.speed)) {
gpsSet.speed.value = gps.fix.speed; gpsSet.speed.value = gps.fix.speed;
@ -250,59 +262,44 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
gpsSet.speed.setValid(false); gpsSet.speed.setValid(false);
} }
if (TIME_SET == (TIME_SET & gps.set)) {
timeval time = {};
#if LIBGPS_VERSION_MINOR <= 17 #if LIBGPS_VERSION_MINOR <= 17
gpsSet.unixSeconds.value = gps.fix.time; gpsSet.unixSeconds.value = std::floor(gps.fix.time);
double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
time.tv_usec = fractionalPart * 1000.0 * 1000.0;
#else #else
gpsSet.unixSeconds.value = gps.fix.time.tv_sec; gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
time.tv_usec = gps.fix.time.tv_nsec / 1000;
#endif #endif
timeval time = {}; time.tv_sec = gpsSet.unixSeconds.value;
time.tv_sec = gpsSet.unixSeconds.value; // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
#if LIBGPS_VERSION_MINOR <= 17 // and no time file available) we set it with the roughly valid time from the GPS.
double fractionalPart = gps.fix.time - std::floor(gps.fix.time); // NTP might only work if the time difference between sys time and current time is not too
time.tv_usec = fractionalPart * 1000.0 * 1000.0; // large.
#else overwriteTimeIfNotSane(time, validFix);
time.tv_usec = gps.fix.time.tv_nsec / 1000; Clock::TimeOfDay_t timeOfDay = {};
#endif Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
std::time_t t = std::time(nullptr); gpsSet.year = timeOfDay.year;
if (time.tv_sec == t) { gpsSet.month = timeOfDay.month;
timeIsConstantCounter++; gpsSet.day = timeOfDay.day;
gpsSet.hours = timeOfDay.hour;
gpsSet.minutes = timeOfDay.minute;
gpsSet.seconds = timeOfDay.second;
} else { } else {
timeIsConstantCounter = 0; gpsSet.unixSeconds.setValid(false);
} gpsSet.year.setValid(false);
if (timeInit and validFix) { gpsSet.month.setValid(false);
if (not utility::timeSanityCheck()) { gpsSet.day.setValid(false);
#if OBSW_VERBOSE_LEVEL >= 1 gpsSet.hours.setValid(false);
time_t timeRaw = time.tv_sec; gpsSet.minutes.setValid(false);
std::tm *timeTm = std::gmtime(&timeRaw); gpsSet.seconds.setValid(false);
sif::info << "Setting invalid system time from GPS data directly: "
<< std::put_time(timeTm, "%c %Z") << std::endl;
#endif
// For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
Clock::setClock(&time);
}
timeInit = false;
}
// If the received time does not change anymore for whatever reason, do not set it here
// to avoid stale times. Also, don't do it too often often to avoid jumping times
if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) {
// Update the system time here for now. NTP seems to be unable to do so for whatever reason.
// Further tests have shown that the time seems to be set by NTPD after some time..
// Clock::setClock(&time);
timeUpdateCd.resetTimer();
} }
Clock::TimeOfDay_t timeOfDay = {};
Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
gpsSet.year = timeOfDay.year;
gpsSet.month = timeOfDay.month;
gpsSet.day = timeOfDay.day;
gpsSet.hours = timeOfDay.hour;
gpsSet.minutes = timeOfDay.minute;
gpsSet.seconds = timeOfDay.second;
if (debugHyperionGps) { if (debugHyperionGps) {
sif::info << "-- Hyperion GPS Data --" << std::endl; sif::info << "-- Hyperion GPS Data --" << std::endl;
#if LIBGPS_VERSION_MINOR <= 17 #if LIBGPS_VERSION_MINOR <= 17
time_t timeRaw = gps.fix.time; time_t timeRaw = gpsSet.unixSeconds.value;
#else #else
time_t timeRaw = gps.fix.time.tv_sec; time_t timeRaw = gps.fix.time.tv_sec;
#endif #endif
@ -325,3 +322,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
} }
return returnvalue::OK; return returnvalue::OK;
} }
void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
if (not timeInit and validFix) {
if (not utility::timeSanityCheck()) {
#if OBSW_VERBOSE_LEVEL >= 1
time_t timeRaw = time.tv_sec;
std::tm *timeTm = std::gmtime(&timeRaw);
sif::info << "Overwriting invalid system time from GPS data directly: "
<< std::put_time(timeTm, "%c %Z") << std::endl;
#endif
// For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
Clock::setClock(&time);
}
timeInit = true;
}
}

22
linux/devices/GpsHyperionLinuxController.h
View File

@ -58,18 +58,30 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
const char* currentClientBuf = nullptr; const char* currentClientBuf = nullptr;
ReadModes readMode = ReadModes::SOCKET; ReadModes readMode = ReadModes::SOCKET;
Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
bool modeCommanded = true; bool modeCommanded = false;
bool timeInit = true; bool timeInit = false;
bool gpsNotOpenSwitch = true;
bool gpsReadFailedSwitch = true; struct OneShotSwitches {
void reset() {
gpsReadFailedSwitch = true;
cantGetFixSwitch = true;
}
bool gpsReadFailedSwitch = true;
bool cantGetFixSwitch = true;
} oneShotSwitches;
bool debugHyperionGps = false; bool debugHyperionGps = false;
int32_t noModeSetCntr = 0; int32_t noModeSetCntr = 0;
uint32_t timeIsConstantCounter = 0;
Countdown timeUpdateCd = Countdown(60); Countdown timeUpdateCd = Countdown(60);
// Returns true if the function should be called again or false if other // Returns true if the function should be called again or false if other
// controller handling can be done. // controller handling can be done.
bool readGpsDataFromGpsd(); bool readGpsDataFromGpsd();
// If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC)
// we set it with the roughly valid time from the GPS. For some reason, NTP might only work
// if the time difference between sys time and current time is not too large
void overwriteTimeIfNotSane(timeval time, bool validFix);
}; };
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */

13
linux/fsfwconfig/events/translateEvents.cpp
View File

@ -1,7 +1,7 @@
/** /**
* @brief Auto-generated event translation file. Contains 245 translations. * @brief Auto-generated event translation file. Contains 248 translations.
* @details * @details
* Generated on: 2023-02-03 10:52:53 * Generated on: 2023-02-08 14:09:40
*/ */
#include "translateEvents.h" #include "translateEvents.h"
@ -200,6 +200,7 @@ const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT";
const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED"; const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED";
const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE"; const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE"; const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT"; const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
const char *BATT_MODE_STRING = "BATT_MODE"; const char *BATT_MODE_STRING = "BATT_MODE";
const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED"; const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
@ -245,6 +246,8 @@ const char *REBOOT_SW_STRING = "REBOOT_SW";
const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED";
const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *REBOOT_HW_STRING = "REBOOT_HW";
const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
const char *VERSION_INFO_STRING = "VERSION_INFO";
const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
const char *translateEvents(Event event) { const char *translateEvents(Event event) {
switch ((event & 0xFFFF)) { switch ((event & 0xFFFF)) {
@ -638,6 +641,8 @@ const char *translateEvents(Event event) {
return CHILDREN_LOST_MODE_STRING; return CHILDREN_LOST_MODE_STRING;
case (13100): case (13100):
return GPS_FIX_CHANGE_STRING; return GPS_FIX_CHANGE_STRING;
case (13101):
return CANT_GET_FIX_STRING;
case (13200): case (13200):
return P60_BOOT_COUNT_STRING; return P60_BOOT_COUNT_STRING;
case (13201): case (13201):
@ -728,6 +733,10 @@ const char *translateEvents(Event event) {
return REBOOT_HW_STRING; return REBOOT_HW_STRING;
case (14004): case (14004):
return NO_SD_CARD_ACTIVE_STRING; return NO_SD_CARD_ACTIVE_STRING;
case (14005):
return VERSION_INFO_STRING;
case (14006):
return CURRENT_IMAGE_INFO_STRING;
default: default:
return "UNKNOWN_EVENT"; return "UNKNOWN_EVENT";
} }

2
linux/fsfwconfig/objects/translateObjects.cpp
View File

@ -2,7 +2,7 @@
* @brief Auto-generated object translation file. * @brief Auto-generated object translation file.
* @details * @details
* Contains 152 translations. * Contains 152 translations.
* Generated on: 2023-02-03 10:52:53 * Generated on: 2023-02-08 14:09:40
*/ */
#include "translateObjects.h" #include "translateObjects.h"

829
linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
View File

@ -15,38 +15,17 @@
#define RPI_TEST_GPS_HANDLER 0 #define RPI_TEST_GPS_HANDLER 0
#endif #endif
ReturnValue_t pst::pstSpiRw(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs(); uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length);
thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * 0.2, DeviceHandlerIF::SEND_WRITE); #if OBSW_ADD_SYRLINKS == 1
thisSequence->addSlot(objects::RW2, length * 0.2, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * 0.2, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * 0.2, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_READ);
return thisSequence->checkSequence();
}
ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length); static_cast<void>(length);
#if OBSW_ADD_PL_PCDU == 1 #if OBSW_ADD_PL_PCDU == 1
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
@ -56,271 +35,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
#endif #endif
#if OBSW_ADD_SUN_SENSORS == 1
bool addSus0 = true;
bool addSus1 = true;
bool addSus2 = true;
bool addSus3 = true;
bool addSus4 = true;
bool addSus5 = true;
bool addSus6 = true;
bool addSus7 = true;
bool addSus8 = true;
bool addSus9 = true;
bool addSus10 = true;
bool addSus11 = true;
if (addSus0) {
/* Write setup */
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus1) {
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus2) {
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus3) {
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus4) {
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus5) {
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus6) {
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus7) {
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus8) {
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus9) {
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus10) {
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
DeviceHandlerIF::GET_READ);
}
if (addSus11) {
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
DeviceHandlerIF::GET_READ);
}
#endif /* OBSW_ADD_SUN_SENSORS == 1 */
#if OBSW_ADD_RAD_SENSORS == 1 #if OBSW_ADD_RAD_SENSORS == 1
/* Radiation sensor */ /* Radiation sensor */
thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
@ -330,74 +44,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ);
#endif #endif
#if OBSW_ADD_ACS_BOARD == 1
bool enableAside = true;
bool enableBside = true;
if (enableAside) {
// A side
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.25,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
}
if (enableBside) {
// B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.25,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
}
#endif /* OBSW_ADD_ACS_BOARD == 1 */
return thisSequence->checkSequence(); return thisSequence->checkSequence();
} }
@ -407,23 +53,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
// Length of a communication cycle // Length of a communication cycle
uint32_t length = thisSequence->getPeriodMs(); uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length); static_cast<void>(length);
#if OBSW_ADD_MGT == 1
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_BPX_BATTERY_HANDLER == 1 #if OBSW_ADD_BPX_BATTERY_HANDLER == 1
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
@ -481,29 +110,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
return thisSequence->checkSequence(); return thisSequence->checkSequence();
} }
ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
// Length of a communication cycle
uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length);
#if OBSW_ADD_SYRLINKS == 1
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_STAR_TRACKER == 1
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
return thisSequence->checkSequence();
}
ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs(); uint32_t length = thisSequence->getPeriodMs();
// PCDU handlers receives two messages and both must be handled // PCDU handlers receives two messages and both must be handled
@ -576,3 +182,422 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
} }
return returnvalue::OK; return returnvalue::OK;
} }
ReturnValue_t pst::pstAcs(FixedTimeslotTaskIF *thisSequence) {
/* Length of a communication cycle */
uint32_t length = thisSequence->getPeriodMs();
#if OBSW_ADD_ACS_BOARD == 1
bool enableAside = true;
bool enableBside = true;
if (enableAside) {
// A side
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
}
if (enableBside) {
// B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
}
#endif /* OBSW_ADD_ACS_BOARD == 1 */
// SUS: 16 ms
#if OBSW_ADD_SUN_SENSORS == 1
bool addSus0 = true;
bool addSus1 = true;
bool addSus2 = true;
bool addSus3 = true;
bool addSus4 = true;
bool addSus5 = true;
bool addSus6 = true;
bool addSus7 = true;
bool addSus8 = true;
bool addSus9 = true;
bool addSus10 = true;
bool addSus11 = true;
if (addSus0) {
/* Write setup */
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus1) {
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus2) {
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus3) {
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus4) {
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus5) {
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus6) {
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus7) {
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus8) {
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus9) {
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus10) {
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
if (addSus11) {
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
DeviceHandlerIF::GET_READ);
}
#endif /* OBSW_ADD_SUN_SENSORS == 1 */
#if OBSW_ADD_STAR_TRACKER == 1
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_MGT == 1
// This is the MTM measurement cycle
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
#endif
thisSequence->addSlot(objects::ACS_CONTROLLER, length * 0.1, 0);
#if OBSW_ADD_MGT == 1
// This is the torquing cycle.
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_RW == 1
thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::GET_READ);
#endif
return returnvalue::OK;
}

6
linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h
View File

@ -39,11 +39,9 @@ namespace pst {
*/ */
ReturnValue_t pstGompaceCan(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstGompaceCan(FixedTimeslotTaskIF* thisSequence);
ReturnValue_t pstUart(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstSpiAndSyrlinks(FixedTimeslotTaskIF* thisSequence);
ReturnValue_t pstSpi(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstAcs(FixedTimeslotTaskIF* thisSequence);
ReturnValue_t pstSpiRw(FixedTimeslotTaskIF* thisSequence);
ReturnValue_t pstI2c(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstI2c(FixedTimeslotTaskIF* thisSequence);

9
mission/acsDefs.h
View File

@ -6,15 +6,16 @@
namespace acs { namespace acs {
enum CtrlSubmode { // These modes are the submodes of the ACS controller and the modes of the ACS subsystem.
enum AcsMode {
OFF = HasModesIF::MODE_OFF, OFF = HasModesIF::MODE_OFF,
SAFE = 10, SAFE = 10,
DETUMBLE = 11, DETUMBLE = 11,
IDLE = 12, PTG_IDLE = 12,
PTG_TARGET_NADIR = 13, PTG_NADIR = 13,
PTG_TARGET = 14, PTG_TARGET = 14,
PTG_TARGET_GS = 15, PTG_TARGET_GS = 15,
PTG_TARGET_INERTIAL = 16, PTG_INERTIAL = 16,
}; };
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;

208
mission/controller/AcsController.cpp
View File

@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
detumble(&acsParameters), detumble(&acsParameters),
ptgCtrl(&acsParameters), ptgCtrl(&acsParameters),
detumbleCounter{0}, detumbleCounter{0},
parameterHelper(this),
mgmDataRaw(this), mgmDataRaw(this),
mgmDataProcessed(this), mgmDataProcessed(this),
susDataRaw(this), susDataRaw(this),
@ -27,7 +28,25 @@ AcsController::AcsController(object_id_t objectId)
actuatorCmdData(this) {} actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
return returnvalue::OK; ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) {
return result;
}
result = parameterHelper.handleParameterMessage(message);
if (result == returnvalue::OK) {
return result;
}
return result;
}
MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues,
uint16_t startAtIndex) {
return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues,
startAtIndex);
} }
void AcsController::performControlOperation() { void AcsController::performControlOperation() {
@ -52,9 +71,11 @@ void AcsController::performControlOperation() {
case acs::DETUMBLE: case acs::DETUMBLE:
performDetumble(); performDetumble();
break; break;
case acs::PTG_IDLE:
case acs::PTG_TARGET: case acs::PTG_TARGET:
case acs::PTG_TARGET_NADIR: case acs::PTG_TARGET_GS:
case acs::PTG_TARGET_INERTIAL: case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl(); performPointingCtrl();
break; break;
} }
@ -86,10 +107,6 @@ void AcsController::performControlOperation() {
} }
void AcsController::performSafe() { void AcsController::performSafe() {
// Concept: SAFE MODE WITH MEKF
// -do the sensor processing, maybe is does make more sense do call this class function in
// another place since we have to do it for every mode regardless of safe or not
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
timeval now; timeval now;
@ -128,10 +145,10 @@ void AcsController::performSafe() {
{ {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
double zeroQuat[4] = {0, 0, 0, 0}; double unitQuat[4] = {0, 0, 0, 1};
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false); ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false); ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng; ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true); ctrlValData.errAng.setValid(true);
@ -153,7 +170,7 @@ void AcsController::performSafe() {
} }
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0; detumbleCounter = 0;
// Triggers detubmle mode transition in subsystem // Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION); triggerEvent(acs::SAFE_RATE_VIOLATION);
} }
@ -174,7 +191,8 @@ void AcsController::performSafe() {
// PoolReadGuard pg(&dipoleSet); // PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock()); // MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true; // torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration); // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
// torqueDuration);
// } // }
} }
@ -252,29 +270,146 @@ void AcsController::performPointingCtrl() {
&mekfData, &validMekf); &mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); double quatRef[4] = {0, 0, 0, 0};
uint8_t enableAntiStiction = true;
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), switch (submode) {
&(sensorValues.rw2Set.currSpeed.value), case acs::PTG_IDLE:
&(sensorValues.rw3Set.currSpeed.value), guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes); targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
}
if (enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
@ -371,6 +506,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// GPS Processed // GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
// MEKF // MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@ -426,7 +563,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE; return INVALID_SUBMODE;
} }
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode > 6) || (submode < 2)) { if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) {
return INVALID_SUBMODE; return INVALID_SUBMODE;
} else { } else {
return returnvalue::OK; return returnvalue::OK;
@ -488,7 +625,6 @@ void AcsController::copyMgmData() {
void AcsController::copySusData() { void AcsController::copySusData() {
{ {
PoolReadGuard pg(&sensorValues.susSets[0]); PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
6 * sizeof(uint16_t)); 6 * sizeof(uint16_t));
@ -585,6 +721,14 @@ void AcsController::copySusData() {
} }
} }
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}
void AcsController::copyGyrData() { void AcsController::copyGyrData() {
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
{ {

14
mission/controller/AcsController.h
View File

@ -3,6 +3,8 @@
#include <fsfw/controller/ExtendedControllerBase.h> #include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include "acs/ActuatorCmd.h" #include "acs/ActuatorCmd.h"
#include "acs/Guidance.h" #include "acs/Guidance.h"
@ -18,12 +20,17 @@
#include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
class AcsController : public ExtendedControllerBase { class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public: public:
static constexpr dur_millis_t INIT_DELAY = 500; static constexpr dur_millis_t INIT_DELAY = 500;
AcsController(object_id_t objectId); AcsController(object_id_t objectId);
MessageQueueId_t getCommandQueue() const;
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
uint16_t startAtIndex) override;
protected: protected:
void performSafe(); void performSafe();
void performDetumble(); void performDetumble();
@ -42,10 +49,13 @@ class AcsController : public ExtendedControllerBase {
uint8_t detumbleCounter; uint8_t detumbleCounter;
ParameterHelper parameterHelper;
enum class InternalState { STARTUP, INITIAL_DELAY, READY }; enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP; InternalState internalState = InternalState::STARTUP;
ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override; ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override; void performControlOperation() override;
@ -134,6 +144,8 @@ class AcsController : public ExtendedControllerBase {
acsctrl::GpsDataProcessed gpsDataProcessed; acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>(); PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>(); PoolEntry<double> gdLongitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
// MEKF // MEKF
acsctrl::MekfData mekfData; acsctrl::MekfData mekfData;

1133
mission/controller/acs/AcsParameters.cpp
View File

File diff suppressed because it is too large Load Diff

441
mission/controller/acs/AcsParameters.h
View File

@ -12,22 +12,36 @@
typedef unsigned char uint8_t; typedef unsigned char uint8_t;
class AcsParameters /*: public HasParametersIF*/ { class AcsParameters : public HasParametersIF {
public: public:
AcsParameters(); AcsParameters();
virtual ~AcsParameters(); virtual ~AcsParameters();
/*
virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
const ParameterWrapper *newValues, uint16_t startAtIndex); uint16_t startAtIndex) override;
*/
struct OnBoardParams { struct OnBoardParams {
double sampleTime = 0.1; // [s] double sampleTime = 0.4; // [s]
} onBoardParams; } onBoardParams;
struct InertiaEIVE { struct InertiaEIVE {
double inertiaMatrix[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.5, 1.0}}; double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
double inertiaMatrixInverse[3][3]; {-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021
// Possible inertia matrices
double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021
double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008},
{-0.0001798426, 0.162240, 0.000475596},
{-0.005008, 0.000475596, 0.060136}}; // 19.11.2021
double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207},
{-0.0001836122, 0.16619787, 0.0083537},
{-0.00501207, 0.0083537, 0.07192588}}; // 19.11.2021
double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767},
{-0.000178376, 0.166172, -0.007403},
{-0.005009767, -0.007403, 0.07195314}};
} inertiaEIVE; } inertiaEIVE;
struct MgmHandlingParameters { struct MgmHandlingParameters {
@ -37,40 +51,41 @@ class AcsParameters /*: public HasParametersIF*/ {
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
float mgm0hardIronOffset[3] = {0.0, 0.0, 0.0}; //{19.89364, -29.94111, -31.07508}; float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060};
float mgm1hardIronOffset[3] = {0.0, 0.0, 0.0}; //{10.95500, -8.053403, -33.36383}; float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483};
float mgm2hardIronOffset[3] = {0.0, 0.0, 0.0}; //{15.72181, -26.87090, -62.19010}; float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297};
float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0}; float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036};
float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0}; float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229};
float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522},
{-0.188413, 1.642303, -0.033184},
{-0.161522, -0.033184, 0.943904}};
float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678},
{-0.170225, 1.274465, -0.040231},
{-0.041678, -0.040231, 1.086352}};
float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084},
{0.172675, 1.541296, 0.065489},
{-0.043084, 0.065489, 1.001238}};
float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832},
{0.177266, 1.262156, 0.010478},
{-0.058832, 0.010478, 1.068345}};
float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072},
{-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}};
float mgm0softIronInverse[3][3] = {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{1420.727e-3, 9.352825e-3,
-127.1979e-3}, {9.352825e-3, 1031.965e-3, -80.02734e-3},
{-127.1979e-3, -80.02734e-3, 934.8899e-3}};*/
float mgm1softIronInverse[3][3] = {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{126.7325e-2, -4.146410e-2, -18.37963e-2},
{-4.146410e-2, 109.3310e-2, -5.246314e-2},
{-18.37963e-2, -5.246314e-2, 105.7300e-2}};*/
float mgm2softIronInverse[3][3] = {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{143.0438e-2, 7.095763e-2,
15.67482e-2}, {7.095763e-2, 99.65167e-2, -6.958760e-2},
{15.67482e-2, -6.958760e-2, 94.50124e-2}};*/
float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float mgm02variance[3] = {1, 1, 1}; float mgm02variance[3] = {1, 1, 1};
float mgm13variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1};
float mgm4variance[3] = {1, 1, 1}; float mgm4variance[3] = {1, 1, 1};
} mgmHandlingParameters; } mgmHandlingParameters;
struct SusHandlingParameters { struct SusHandlingParameters {
float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10
float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM06 float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM06
float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM13 float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM13
float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM14 float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM14
float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM05 float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM05
float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM02 float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM02
float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10 float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07
float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM01 float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM01
float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM03 float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM03
float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11 float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11
@ -78,61 +93,61 @@ class AcsParameters /*: public HasParametersIF*/ {
float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08 float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08
float sus0coeffAlpha[9][10] = { float sus0coeffAlpha[9][10] = {
{10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293,
-0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146,
-0.000124096561459262, 0.00040790566176981}, 0.00091428505258307, 0.000259857066722932},
{6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417, {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859,
0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415, 0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05,
1.63114413539632e-05, -2.0187452317724e-05}, -1.06065583714065e-05, -1.43899892666699e-05},
{-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334, {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577,
0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995, 0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412,
-0.000249094601806692, 0.000231466668650876}, -4.88501228194031e-06, -0.000434861113274231},
{-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384, {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253,
-0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05, 0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05,
-9.78534772816957e-06, -4.72083745991256e-05}, 6.09619017310129e-05, 4.23908862836885e-05},
{0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541, {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607,
-0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463, 0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257,
0.00010150571088124, -0.000367705461590854}, 0.000150795563555828, -0.000279246491927943},
{3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526, {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557,
-0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05, -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05,
6.97244631313601e-05, 2.50519145070895e-05}, 9.13233708460098e-05, -0.000206717750924323},
{-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191, {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216,
-0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928, -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597,
-1.78466217018177e-05, -0.00058976234038192}, -0.000532190902882765, 9.36018171121253e-05},
{1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089, {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645,
-0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06, -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05,
-2.73407888222758e-05, 5.45131881032866e-06}, 1.02609175615251e-05, -9.64717658954667e-06},
{43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675, {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358,
0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078, -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05,
-0.000889196131314391, -0.000509766491897672}}; 0.000868163357631254, 0.00120099606910313}};
float sus0coeffBeta[9][10] = { float sus0coeffBeta[9][10] = {
{1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235, {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917,
-0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05, 0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05,
-5.52178824394723e-06, 5.73935295303589e-05}, 0.000821479971871302, -4.5330528329465e-05},
{3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133, {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823,
0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05, 0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05,
-7.58501977656551e-05, -8.79809730730992e-05}, -4.04514487800062e-05, -7.6296149087237e-05},
{14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518, {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941,
0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341, -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412,
-0.00020861690864945}, 0.0011723869613426, 0.000122882034141316},
{1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335, {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172,
-0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06, -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05,
-6.00508568552101e-05, 0.000101583822589461}, -5.87601932564404e-05, -3.92033314627704e-05},
{-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729, {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898,
-0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05, 0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264,
0.000316835483508523, 0.000151442890664899}, 0.000259925974231328, 0.000222437797823852},
{-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231, {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167,
0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05, 0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05,
-9.83662017231755e-05, 1.87078667116619e-05}, -4.33518182614169e-05, 4.66993119419691e-05},
{27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016, {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108,
-0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772, 0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429,
-0.000285438191474895, -5.71327627917386e-05}, 0.000401546410974577, -0.000147563886502726},
{-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328, {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231,
-0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05, 0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05,
-9.77555098179959e-05, 2.09624089449761e-05}, -5.3309466243918e-05, 6.20289310356821e-06},
{-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785,
-0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741,
0.000141998709314758, 0.000101051304611037}}; -0.000186887396585446, 0.00119710704771344}};
float sus1coeffAlpha[9][10] = { float sus1coeffAlpha[9][10] = {
{-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084,
-0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176,
@ -414,61 +429,61 @@ class AcsParameters /*: public HasParametersIF*/ {
-0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, -0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245,
-0.000169538153750085, -0.000336529204350355}}; -0.000169538153750085, -0.000336529204350355}};
float sus6coeffAlpha[9][10] = { float sus6coeffAlpha[9][10] = {
{13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707,
0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396,
0.00091428505258307, 0.000259857066722932}, -0.000124096561459262, 0.00040790566176981},
{1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859, {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417,
0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05, 0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415,
-1.06065583714065e-05, -1.43899892666699e-05}, 1.63114413539632e-05, -2.0187452317724e-05},
{8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577, {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334,
0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412, 0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995,
-4.88501228194031e-06, -0.000434861113274231}, -0.000249094601806692, 0.000231466668650876},
{-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253, {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384,
0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05, -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05,
6.09619017310129e-05, 4.23908862836885e-05}, -9.78534772816957e-06, -4.72083745991256e-05},
{-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607, {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541,
0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257, -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463,
0.000150795563555828, -0.000279246491927943}, 0.00010150571088124, -0.000367705461590854},
{7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557, {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526,
-0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05, -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05,
9.13233708460098e-05, -0.000206717750924323}, 6.97244631313601e-05, 2.50519145070895e-05},
{16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216, {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191,
-0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597, -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928,
-0.000532190902882765, 9.36018171121253e-05}, -1.78466217018177e-05, -0.00058976234038192},
{-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645, {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089,
-0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05, -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06,
1.02609175615251e-05, -9.64717658954667e-06}, -2.73407888222758e-05, 5.45131881032866e-06},
{-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358, {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675,
-0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05, 0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078,
0.000868163357631254, 0.00120099606910313}}; -0.000889196131314391, -0.000509766491897672}};
float sus6coeffBeta[9][10] = { float sus6coeffBeta[9][10] = {
{12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917, {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235,
0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05, -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05,
0.000821479971871302, -4.5330528329465e-05}, -5.52178824394723e-06, 5.73935295303589e-05},
{1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823, {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133,
0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05, 0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05,
-4.04514487800062e-05, -7.6296149087237e-05}, -7.58501977656551e-05, -8.79809730730992e-05},
{5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941, {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518,
-0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412, 0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341,
0.0011723869613426, 0.000122882034141316}, -0.00020861690864945},
{2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172, {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335,
-0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05, -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06,
-5.87601932564404e-05, -3.92033314627704e-05}, -6.00508568552101e-05, 0.000101583822589461},
{2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898, {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729,
0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264, -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05,
0.000259925974231328, 0.000222437797823852}, 0.000316835483508523, 0.000151442890664899},
{1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167, {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231,
0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05, 0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05,
-4.33518182614169e-05, 4.66993119419691e-05}, -9.83662017231755e-05, 1.87078667116619e-05},
{-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108, {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016,
0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429, -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772,
0.000401546410974577, -0.000147563886502726}, -0.000285438191474895, -5.71327627917386e-05},
{1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231, {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328,
0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05, -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05,
-5.3309466243918e-05, 6.20289310356821e-06}, -9.77555098179959e-05, 2.09624089449761e-05},
{1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461,
0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688,
-0.000186887396585446, 0.00119710704771344}}; 0.000141998709314758, 0.000101051304611037}};
float sus7coeffAlpha[9][10] = { float sus7coeffAlpha[9][10] = {
{-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523,
-0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283,
@ -749,9 +764,6 @@ class AcsParameters /*: public HasParametersIF*/ {
{116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
-0.000889232196185857, -0.00168429567131815}}; -0.000889232196185857, -0.00168429567131815}};
float filterAlpha;
float sunThresh;
} susHandlingParameters; } susHandlingParameters;
struct GyrHandlingParameters { struct GyrHandlingParameters {
@ -759,110 +771,119 @@ class AcsParameters /*: public HasParametersIF*/ {
double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
// var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
// assumed to be equal for the same class of sensors double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004};
double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359};
double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
enum PreferAdis { NO = 0, YES = 1 }; uint8_t preferAdis = true;
uint8_t preferAdis = PreferAdis::YES;
} gyrHandlingParameters; } gyrHandlingParameters;
struct RwHandlingParameters { struct RwHandlingParameters {
double rw0orientationMatrix[3][3];
double rw1orientationMatrix[3][3];
double rw2orientationMatrix[3][3];
double rw3orientationMatrix[3][3];
double inertiaWheel = 0.000028198; double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm] double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 100; // 80; // RPM
double stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006;
} rwHandlingParameters; } rwHandlingParameters;
struct RwMatrices { struct RwMatrices {
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
{0.0000, -0.9205, 0.0000, 0.9205}, {0.0000, -0.9205, 0.0000, 0.9205},
{0.3907, 0.3907, 0.3907, 0.3907}}; {0.3907, 0.3907, 0.3907, 0.3907}};
double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, double pseudoInverse[4][3] = {
{0.2136, -0.3317, 1.0123}, {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
{-0.8672, -0.1406, 0.1778}, double without0[4][3] = {
{0.6426, 0.4794, 1.3603}}; {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
double without0[4][3]; double without1[4][3] = {
double without1[4][3]; {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
double without2[4][3]; double without2[4][3] = {
double without3[4][3]; {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649}; double without3[4][3] = {
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
} rwMatrices; } rwMatrices;
struct SafeModeControllerParameters { struct SafeModeControllerParameters {
double k_rate_mekf = 0.00059437; double k_rate_mekf = 0.00059437;
double k_align_mekf = 0.000056875; double k_align_mekf = 0.000056875;
double k_rate_no_mekf; double k_rate_no_mekf = 0.00059437;
double k_align_no_mekf; double k_align_no_mekf = 0.000056875;
double sunMagAngleMin;
double sunTargetDir[3] = {1, 0, 0}; // Body frame double sunMagAngleMin = 5 * M_PI / 180;
double satRateRef[3]; // Body frame
double sunTargetDirLeop[3] = {0, .5, .5};
double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0};
} safeModeControllerParameters; } safeModeControllerParameters;
struct DetumbleCtrlParameters { struct PointingLawParameters {
double gainD = pow(10.0, -3.3);
} detumbleCtrlParameters;
// ToDo: mutiple structs for different pointing mode controllers?
struct PointingModeControllerParameters {
double updtFlag;
double A_rw[3][12];
double refDirection[3] = {1, 0, 0};
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
double zeta = 0.3; double zeta = 0.3;
double zetaLow;
double om = 0.3; double om = 0.3;
double omLow;
double omMax = 1 * M_PI / 180; double omMax = 1 * M_PI / 180;
double qiMin = 0.1; double qiMin = 0.1;
double gainNullspace = 0.01; double gainNullspace = 0.01;
double desatMomentumRef[3] = {0, 0, 0}; double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000; double deSatGainFactor = 1000;
bool desatOn = true; uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
} pointingLawParameters;
double omegaEarth = 0.000072921158553; struct TargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
// give this as an input- currently en calculation is done
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
} inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; // Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeTgt = 500; // [m]
// For one-axis control:
uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
} targetModeControllerParameters;
struct NadirModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters;
struct InertialModeControllerParameters : PointingLawParameters {
double tgtQuat[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
} inertialModeControllerParameters;
struct StrParameters { struct StrParameters {
double exclusionAngle = 20 * M_PI / 180; double exclusionAngle = 20 * M_PI / 180;
// double strOrientationMatrix[3][3]; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // in body/geometry frame
} strParameters; } strParameters;
struct GpsParameters { struct GpsParameters {
double timeDiffVelocityMax = 30; //[s]
} gpsParameters; } gpsParameters;
struct GroundStationParameters {
double latitudeGs = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeGs = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeGs = 500; // [m] Altitude
double earthRadiusEquat = 6378137; // [m]
double earthRadiusPolar = 6356752.314; // [m]
} groundStationParameters; // Stuttgart
struct SunModelParameters { struct SunModelParameters {
enum UseSunModel { NO = 0, YES = 3 };
uint8_t useSunModel;
float domega = 36000.771; float domega = 36000.771;
float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of
float m_0 = 357.5256; // coefficients for mean anomaly // perigee
float m_0 = 357.5277; // coefficients for mean anomaly
float dm = 35999.049; // coefficients for mean anomaly float dm = 35999.049; // coefficients for mean anomaly
float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis
float e1 = 0.74508 * M_PI / 180.; float e1 = 0.74508 * M_PI / 180.;
@ -872,19 +893,13 @@ class AcsParameters /*: public HasParametersIF*/ {
} sunModelParameters; } sunModelParameters;
struct KalmanFilterParameters { struct KalmanFilterParameters {
uint8_t activateKalmanFilter;
uint8_t requestResetFlag;
double maxToleratedTimeBetweenKalmanFilterExecutionSteps;
double processNoiseOmega[3];
double processNoiseQuaternion[4];
double sensorNoiseSTR = 0.1 * M_PI / 180; double sensorNoiseSTR = 0.1 * M_PI / 180;
double sensorNoiseSS = 8 * M_PI / 180; double sensorNoiseSS = 8 * M_PI / 180;
double sensorNoiseMAG = 4 * M_PI / 180; double sensorNoiseMAG = 4 * M_PI / 180;
double sensorNoiseRMU[3]; double sensorNoiseGYR = 0.1 * M_PI / 180;
double sensorNoiseArwRmu; // Angular Random Walk double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk
double sensorNoiseBsRMU; // Bias Stability double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters; } kalmanFilterParameters;
struct MagnetorquesParameter { struct MagnetorquesParameter {
@ -899,8 +914,8 @@ class AcsParameters /*: public HasParametersIF*/ {
struct DetumbleParameter { struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2; double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4; double omegaDetumbleEnd = 0.4 * M_PI / 180;
double gainD = pow(10.0, -3.3); double gainD = pow(10.0, -3.3);
} detumbleParameter; } detumbleParameter;
}; };

21
mission/controller/acs/ActuatorCmd.cpp
View File

@ -21,26 +21,27 @@ ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsPa
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
using namespace Math;
// Scaling the commanded torque to a maximum value // Scaling the commanded torque to a maximum value
double torque[4] = {0, 0, 0, 0};
double maxTrq = acsParameters.rwHandlingParameters.maxTrq; double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ? for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(torque[i]) > maxValue) { if (abs(rwTrq[i]) > maxValue) {
maxValue = abs(torque[i]); maxValue = abs(rwTrq[i]);
} }
} }
if (maxValue > maxTrq) { if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue; double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4); VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
} }
}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTorque, double *rwCmdSpeed) {
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel // Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
@ -50,7 +51,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s] // W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm; double factor = commandTime / inertiaWheel * radToRpm;
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4); VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
} }

18
mission/controller/acs/ActuatorCmd.h
View File

@ -1,10 +1,3 @@
/*
* ActuatorCmd.h
*
* Created on: 4 Aug 2022
* Author: Robin Marquardt
*/
#ifndef ACTUATORCMD_H_ #ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_ #define ACTUATORCMD_H_
@ -18,6 +11,14 @@ class ActuatorCmd {
ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
virtual ~ActuatorCmd(); virtual ~ActuatorCmd();
/*
* @brief: scalingTorqueRws() scales the torque via maximum part in case this part is
* higher then the maximum torque
* @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque
*/
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
* wheels, also will calculate the needed revolutions per minute for the RWs, which will be given * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
@ -28,8 +29,7 @@ class ActuatorCmd {
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS, const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
double *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques

486
mission/controller/acs/Guidance.cpp
View File

@ -13,6 +13,8 @@
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> #include <math.h>
#include <filesystem>
#include "string.h" #include "string.h"
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
@ -22,57 +24,53 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete
Guidance::~Guidance() {} Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) { if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
} }
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); 3 * sizeof(double));
} }
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, acsctrl::SusDataProcessed *susDataProcessed,
double targetQuat[4], double refSatRate[3]) { acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation // Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth // Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame) // fixed/centered frame)
double groundStationCart[3] = {0, 0, 0}; double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.groundStationParameters.longitudeGs, acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.groundStationParameters.altitudeGs, acsParameters.targetModeControllerParameters.longitudeTgt,
groundStationCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value, double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
sensorValues->gpsSet.longitude.value, double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE); sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame // Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3); VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame // Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::dcmEJ(now, *dcmEJ);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
// TEST SECTION !
// double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix,
// dcmTEST, dcmTEST, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame // Transformation between ECEF and Body frame
@ -111,9 +109,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
// Calculation of reference rotation rate // Calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0}; double velSatE[3] = {0, 0, 0};
velSatE[0] = 0.0; // sensorValues->gps0Velocity[0]; std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
velSatE[1] = 0.0; // sensorValues->gps0Velocity[1];
velSatE[2] = 0.0; // sensorValues->gps0Velocity[2];
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
@ -134,10 +130,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
// Calculation of reference rotation rate in case of star tracker blinding // Calculation of reference rotation rate in case of star tracker blinding
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) { if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirJ[3] = {0, 0, 0};
double sunDirB[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0};
if (susDataProcessed->sunIjkModel.isValid()) { if (susDataProcessed->sunIjkModel.isValid()) {
double sunDirJ[3] = {0, 0, 0};
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else { } else {
@ -183,14 +179,414 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
} }
} }
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { double *refSatRate) {
double quatRef[4] = {0, 0, 0, 0}; //-------------------------------------------------------------------------------------
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; // Calculation of reference rotation rate
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; //-------------------------------------------------------------------------------------
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; (timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
if (timeElapsed < timeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[2] = omegaRefNew[2];
} else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
}
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// Transform velocity into inertial frame
double velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
// orbital normal vector
double orbitalNormalJ[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-Axis completes RHS
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get Sun Vector Model in ECI
double sunJ[3];
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
VectorOperations<double>::normalize(sunJ, sunJ, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
double sunParallel[3], zAxis[3];
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// calculate y-axis
double yAxis[3];
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
double quatBJ[4] = {0, 0, 0, 0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
if (susDataProcessed->sunIjkModel.isValid()) {
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else if (susDataProcessed->susVecTot.isValid()) {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
} else {
return;
}
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// positive z-Axis of EIVE in direction of sun
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
// Assign helper vector (north pole inertial)
double helperVec[3] = {0, 0, 1};
//
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
//
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
// Transformation matrix to Sun, no further transforamtions, reference is already
// the EIVE body frame
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatSun[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatSun);
targetQuat[0] = quatSun[0];
targetQuat[1] = quatSun[1];
targetQuat[2] = quatSun[2];
targetQuat[3] = quatSun[3];
//----------------------------------------------------------------------------
// Calculation of reference rotation rate
//----------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4],
double refSatRate[3]) { // old version of Nadir Pointing
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction in the body frame
double targetDirJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// z-Axis parallel to long side of picture resolution
double zAxis[3] = {0, 0, 0}, velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
4 * sizeof(double));
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
3 * sizeof(double));
}
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]) {
double satRate[3] = {0, 0, 0}; double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3); VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
@ -210,8 +606,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
quatError[1] = quatErrorComplete[1]; quatError[1] = quatErrorComplete[1];
quatError[2] = quatErrorComplete[2]; quatError[2] = quatErrorComplete[2];
// target flag in matlab, importance, does look like it only gives // target flag in matlab, importance, does look like it only gives feedback if pointing control is
// feedback if pointing control is under 150 arcsec ?? // under 150 arcsec ??
} }
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
@ -300,7 +696,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *
// @note: This one takes the normal pseudoInverse of all four raction wheels valid. // @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?! // Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one // Thought: It does not really play a role, because in case there are more then one
// reaction wheel the pointing control is destined to fail. // reaction wheel invalid the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];

53
mission/controller/acs/Guidance.h
View File

@ -21,16 +21,49 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
// Function to get the target quaternion and refence rotation rate from gps position and position // Function to get the target quaternion and refence rotation rate from gps position and
// of the ground station // position of the ground station
void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], acsctrl::GpsDataProcessed *gpsDataProcessed,
double refSatRate[3]); acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]);
void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]);
void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground
// station
void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing
void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]);
void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial
// pointing
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired // desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]); double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]);
void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h" // reation wheel maybe can be done in "commanding.h"
@ -39,6 +72,12 @@ class Guidance {
private: private:
AcsParameters acsParameters; AcsParameters acsParameters;
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";
}; };
#endif /* ACS_GUIDANCE_H_ */ #endif /* ACS_GUIDANCE_H_ */

95
mission/controller/acs/Igrf13Model.cpp
View File

@ -1,22 +1,19 @@
/*
* Igrf13Model.cpp
*
* Created on: 10 Mar 2022
* Author: Robin Marquardt
*/
#include "Igrf13Model.h" #include "Igrf13Model.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <time.h>
#include <cmath>
#include "util/MathOperations.h" #include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {} Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {} Igrf13Model::~Igrf13Model() {}
@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
double* magFieldModelInertial) { double* magFieldModelInertial) {
double phi = longitude, theta = gcLatitude; // geocentric double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/ /* Here is the co-latitude needed*/
theta -= 90 * Math::PI / 180; theta -= 90 * PI / 180;
theta *= (-1); theta *= (-1);
double rE = 6371200.0; // radius earth [m] double rE = 6371200.0; // radius earth [m]
@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
/* Calculation of Legendre Polynoms (normalised) */ /* Calculation of Legendre Polynoms (normalised) */
if (n == m) { if (n == m) {
P2 = sin(theta) * P11; P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 - cos(theta) * P11; dP2 = sin(theta) * dP11 + cos(theta) * P11;
P11 = P2; P11 = P2;
P10 = P11; P10 = P11;
P20 = 0; P20 = 0;
@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[0] += magFieldModel[0] +=
pow(rE / (altitude + rE), (n + 2)) * (n + 1) * pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
/* gradient of scalar potential towards phi */ /* gradient of scalar potential towards theta */
magFieldModel[1] += magFieldModel[1] +=
pow(rE / (altitude + rE), (n + 2)) * pow(rE / (altitude + rE), (n + 2)) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
/* gradient of scalar potential towards theta */ /* gradient of scalar potential towards phi */
magFieldModel[2] += magFieldModel[2] +=
pow(rE / (altitude + rE), (n + 2)) * pow(rE / (altitude + rE), (n + 2)) *
((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
@ -85,31 +82,30 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[1] *= -1; magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta)); magFieldModel[2] *= (-1 / sin(theta));
/* Next step: transform into inertial KOS (IJK)*/
// Julean Centuries
double JD2000Floor = 0;
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
JD2000Floor = floor(JD2000); double UT1 = JD2000 / 36525.;
double JC2000 = JD2000Floor / 36525;
double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
0.000000026 * pow(JC2000, 3); // greenwich sidereal time
gst *= PI / 180; // convert to radians
double sec =
(JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ?
double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
gst += omega0 * sec;
double gst =
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
gst = std::fmod(gst, 360.);
gst *= PI / 180.;
double lst = gst + longitude; // local sidereal time [rad] double lst = gst + longitude; // local sidereal time [rad]
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + magFieldModelInertial[0] =
magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + magFieldModel[2] * sin(lst);
magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); magFieldModelInertial[1] =
magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) +
magFieldModel[2] * cos(lst);
magFieldModelInertial[2] =
magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
double normVecMagFieldInert[3] = {0, 0, 0}; double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3); VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
} }
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
@ -123,3 +119,34 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
} }
} }
} }
void Igrf13Model::schmidtNormalization() {
double kronDelta = 0;
schmidtFactors[0][0] = 1;
for (int n = 1; n <= igrfOrder; n++) {
if (n == 1) {
schmidtFactors[0][n - 1] = 1;
} else {
schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n;
}
for (int m = 1; m <= igrfOrder; m++) {
if (m == 1) {
kronDelta = 1;
} else {
kronDelta = 0;
}
schmidtFactors[m][n - 1] =
schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m));
}
}
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {
coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j];
coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j];
svG[i][j] = schmidtFactors[i][j] * svG[i][j];
svH[i][j] = schmidtFactors[i][j] * svH[i][j];
}
}
}

21
mission/controller/acs/Igrf13Model.h
View File

@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ {
* - timeOfMagMeasurement: time of actual measurement [s] * - timeOfMagMeasurement: time of actual measurement [s]
* *
* Outputs: * Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ * - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/
// Coefficient wary over year, could be updated sometimes. // Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3]; double magFieldModel[3];
void schmidtNormalization();
private: private:
const double coeffG[14][13] = { double coeffG[14][13] = {
{-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1}, {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
{-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9}, {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
{0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT
const double coeffH[14][13] = { double coeffH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0},
{4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9}, {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
{0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT
const double svG[14][13] = { double svG[14][13] = {
{5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
const double svH[14][13] = { double svH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -114,6 +115,16 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
double schmidtFactors[14][13] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
;
double updatedG[14][13]; double updatedG[14][13];
double updatedH[14][13]; double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation static const int igrfOrder = 13; // degree of truncation

328
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -1,10 +1,3 @@
/*
* MultiplicativeKalmanFilter.cpp
*
* Created on: 4 Feb 2022
* Author: rooob
*/
#include "MultiplicativeKalmanFilter.h" #include "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
@ -14,6 +7,8 @@
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <cmath>
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
@ -29,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
} }
void MultiplicativeKalmanFilter::reset() {} void MultiplicativeKalmanFilter::reset() {}
@ -41,14 +36,11 @@ void MultiplicativeKalmanFilter::init(
// check for valid mag/sun // check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true; validInit = true;
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
// QUEST ALGO ----------------------------------------------------------------------- // QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG; sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
sigmaGyro = 0.1 * 3.141 / 180; // acs parameters
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0}; normSunJ[3] = {0, 0, 0};
@ -136,7 +128,6 @@ void MultiplicativeKalmanFilter::init(
matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
/* vector*transpose(vector)*/
MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3); MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3); MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
@ -199,8 +190,6 @@ void MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2]; initialCovarianceMatrix[5][5] = initGyroCov[2][2];
// initialCovarianceMatrix[][] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},
//{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
} else { } else {
// no initialisation possible, no valid measurements // no initialisation possible, no valid measurements
validInit = false; validInit = false;
@ -208,14 +197,15 @@ void MultiplicativeKalmanFilter::init(
} }
// --------------- MEKF DISCRETE TIME STEP ------------------------------- // --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst( ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, const double *rateGYRs_, const bool validGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *magneticField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const bool validMagField_, const double *sunDir_,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) { const bool validSS, const double *sunDirJ,
const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime,
acsctrl::MekfData *mekfData) {
// Check for GYR Measurements // Check for GYR Measurements
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) { if (!validGYRs_) {
{ {
@ -960,10 +950,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
biasGYR[2] = updatedGyroBias[2]; biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/ /* ----------- PROPAGATION ----------*/
// double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
// double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = 3 * 3.141 / 180 / 3600;
double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -977,170 +965,135 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// Discrete Process Noise Covariance Q // Discrete Process Noise Covariance Q
double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double covQ1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < 3.141 / 10) { if (normRotEst * sampleTime < M_PI / 10) {
double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3); double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3);
MatrixOperations<double>::transpose(*covQ2, *transCovQ2, 3);
double fact3 = sampleTime * pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3);
discProcessNoiseCov[0][0] = covQ1[0][0]; double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
discProcessNoiseCov[0][1] = covQ1[0][1]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
discProcessNoiseCov[0][2] = covQ1[0][2]; std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
discProcessNoiseCov[0][3] = covQ2[0][0];
discProcessNoiseCov[0][4] = covQ2[0][1]; double fact22 = pow(sigmaU, 2) * sampleTime;
discProcessNoiseCov[0][5] = covQ2[0][2]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
discProcessNoiseCov[1][0] = covQ1[1][0];
discProcessNoiseCov[1][1] = covQ1[1][1];
discProcessNoiseCov[1][2] = covQ1[1][2];
discProcessNoiseCov[1][3] = covQ2[1][0];
discProcessNoiseCov[1][4] = covQ2[1][1];
discProcessNoiseCov[1][5] = covQ2[1][2];
discProcessNoiseCov[2][0] = covQ1[2][0];
discProcessNoiseCov[2][1] = covQ1[2][1];
discProcessNoiseCov[2][2] = covQ1[2][2];
discProcessNoiseCov[2][3] = covQ2[2][0];
discProcessNoiseCov[2][4] = covQ2[2][1];
discProcessNoiseCov[2][5] = covQ2[2][2];
discProcessNoiseCov[3][0] = transCovQ2[0][0];
discProcessNoiseCov[3][1] = transCovQ2[0][1];
discProcessNoiseCov[3][2] = transCovQ2[0][2];
discProcessNoiseCov[3][3] = covQ3[0][0];
discProcessNoiseCov[3][4] = covQ3[0][1];
discProcessNoiseCov[3][5] = covQ3[0][2];
discProcessNoiseCov[4][0] = transCovQ2[1][0];
discProcessNoiseCov[4][1] = transCovQ2[1][1];
discProcessNoiseCov[4][2] = transCovQ2[1][2];
discProcessNoiseCov[4][3] = covQ3[1][0];
discProcessNoiseCov[4][4] = covQ3[1][1];
discProcessNoiseCov[4][5] = covQ3[1][2];
discProcessNoiseCov[5][0] = transCovQ2[2][0];
discProcessNoiseCov[5][1] = transCovQ2[2][1];
discProcessNoiseCov[5][2] = transCovQ2[2][2];
discProcessNoiseCov[5][3] = covQ3[2][0];
discProcessNoiseCov[5][4] = covQ3[2][1];
discProcessNoiseCov[5][5] = covQ3[2][2];
} else { } else {
// double fact1 = sampleTime*pow(sigmaV,2); double fact22 = pow(sigmaU, 2) * sampleTime;
double covQ11[3][3], covQ12[3][3], covQ13[3][3]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
// MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) -
pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3);
double fact2 = pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3);
MatrixOperations<double>::subtract(*covQ12, *covQ11, *covQ11, 3, 3);
double fact3 = sampleTime * pow(sigmaV, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3);
MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3);
double covQ21[3][3], covQ22[3][3], covQ23[3][3]; double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact4 = double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
(0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
pow(normRotEst, 4); pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
double fact5 = 0.5 * pow(sampleTime, 2); MatrixOperations<double>::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); MatrixOperations<double>::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3);
MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3);
double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3); MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3);
MatrixOperations<double>::subtract(*covQ23, *covQ21, *covQ21, 3, 3);
double fact7 = pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
discProcessNoiseCov[0][0] = covQ1[0][0]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
discProcessNoiseCov[0][1] = covQ1[0][1]; double fact11_1 = 1. / 3. * pow(sampleTime, 3);
discProcessNoiseCov[0][2] = covQ1[0][2]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
discProcessNoiseCov[0][3] = covQ2[0][0]; double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
discProcessNoiseCov[0][4] = covQ2[0][1]; 1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
discProcessNoiseCov[0][5] = covQ2[0][2]; pow(normRotEst, 5);
discProcessNoiseCov[1][0] = covQ1[1][0]; MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
discProcessNoiseCov[1][1] = covQ1[1][1]; MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
discProcessNoiseCov[1][2] = covQ1[1][2]; MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
discProcessNoiseCov[1][3] = covQ2[1][0]; MatrixOperations<double>::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3);
discProcessNoiseCov[1][4] = covQ2[1][1]; MatrixOperations<double>::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3);
discProcessNoiseCov[1][5] = covQ2[1][2];
discProcessNoiseCov[2][0] = covQ1[2][0];
discProcessNoiseCov[2][1] = covQ1[2][1];
discProcessNoiseCov[2][2] = covQ1[2][2];
discProcessNoiseCov[2][3] = covQ2[2][0];
discProcessNoiseCov[2][4] = covQ2[2][1];
discProcessNoiseCov[2][5] = covQ2[2][2];
discProcessNoiseCov[3][0] = covQ2[0][0];
discProcessNoiseCov[3][1] = covQ2[0][1];
discProcessNoiseCov[3][2] = covQ2[0][2];
discProcessNoiseCov[3][3] = covQ3[0][0];
discProcessNoiseCov[3][4] = covQ3[0][1];
discProcessNoiseCov[3][5] = covQ3[0][2];
discProcessNoiseCov[4][0] = covQ2[1][0];
discProcessNoiseCov[4][1] = covQ2[1][1];
discProcessNoiseCov[4][2] = covQ2[1][2];
discProcessNoiseCov[4][3] = covQ3[1][0];
discProcessNoiseCov[4][4] = covQ3[1][1];
discProcessNoiseCov[4][5] = covQ3[1][2];
discProcessNoiseCov[5][0] = covQ2[2][0];
discProcessNoiseCov[5][1] = covQ2[2][1];
discProcessNoiseCov[5][2] = covQ2[2][2];
discProcessNoiseCov[5][3] = covQ3[2][0];
discProcessNoiseCov[5][4] = covQ3[2][1];
discProcessNoiseCov[5][5] = covQ3[2][2];
} }
discProcessNoiseCov[0][0] = covQ11[0][0];
discProcessNoiseCov[0][1] = covQ11[0][1];
discProcessNoiseCov[0][2] = covQ11[0][2];
discProcessNoiseCov[0][3] = covQ12[0][0];
discProcessNoiseCov[0][4] = covQ12[0][1];
discProcessNoiseCov[0][5] = covQ12[0][2];
discProcessNoiseCov[1][0] = covQ11[1][0];
discProcessNoiseCov[1][1] = covQ11[1][1];
discProcessNoiseCov[1][2] = covQ11[1][2];
discProcessNoiseCov[1][3] = covQ12[1][0];
discProcessNoiseCov[1][4] = covQ12[1][1];
discProcessNoiseCov[1][5] = covQ12[1][2];
discProcessNoiseCov[2][0] = covQ11[2][0];
discProcessNoiseCov[2][1] = covQ11[2][1];
discProcessNoiseCov[2][2] = covQ11[2][2];
discProcessNoiseCov[2][3] = covQ12[2][0];
discProcessNoiseCov[2][4] = covQ12[2][1];
discProcessNoiseCov[2][5] = covQ12[2][2];
discProcessNoiseCov[3][0] = covQ12trans[0][0];
discProcessNoiseCov[3][1] = covQ12trans[0][1];
discProcessNoiseCov[3][2] = covQ12trans[0][2];
discProcessNoiseCov[3][3] = covQ22[0][0];
discProcessNoiseCov[3][4] = covQ22[0][1];
discProcessNoiseCov[3][5] = covQ22[0][2];
discProcessNoiseCov[4][0] = covQ12trans[1][0];
discProcessNoiseCov[4][1] = covQ12trans[1][1];
discProcessNoiseCov[4][2] = covQ12trans[1][2];
discProcessNoiseCov[4][3] = covQ22[1][0];
discProcessNoiseCov[4][4] = covQ22[1][1];
discProcessNoiseCov[4][5] = covQ22[1][2];
discProcessNoiseCov[5][0] = covQ12trans[2][0];
discProcessNoiseCov[5][1] = covQ12trans[2][1];
discProcessNoiseCov[5][2] = covQ12trans[2][2];
discProcessNoiseCov[5][3] = covQ22[2][0];
discProcessNoiseCov[5][4] = covQ22[2][1];
discProcessNoiseCov[5][5] = covQ22[2][2];
// State Transition Matrix phi // State Transition Matrix phi
double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11[3][3], phi12[3][3]; double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact1 = sin(normRotEst * sampleTime); double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3); MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3); MatrixOperations<double>::add(*phi11_01, *phi11_2, *phi11, 3, 3);
double phi21[3][3], phi22[3][3]; double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3); MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 3, 3); MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
MatrixOperations<double>::subtract(*phi12_01, *phi12_2, *phi12, 3, 3);
phi[0][0] = phi1[0][0]; phi[0][0] = phi11[0][0];
phi[0][1] = phi1[0][1]; phi[0][1] = phi11[0][1];
phi[0][2] = phi1[0][2]; phi[0][2] = phi11[0][2];
phi[0][3] = phi2[0][0]; phi[0][3] = phi12[0][0];
phi[0][4] = phi2[0][1]; phi[0][4] = phi12[0][1];
phi[0][5] = phi2[0][2]; phi[0][5] = phi12[0][2];
phi[1][0] = phi1[1][0]; phi[1][0] = phi11[1][0];
phi[1][1] = phi1[1][1]; phi[1][1] = phi11[1][1];
phi[1][2] = phi1[1][2]; phi[1][2] = phi11[1][2];
phi[1][3] = phi2[1][0]; phi[1][3] = phi12[1][0];
phi[1][4] = phi2[1][1]; phi[1][4] = phi12[1][1];
phi[1][5] = phi2[1][2]; phi[1][5] = phi12[1][2];
phi[2][0] = phi1[2][0]; phi[2][0] = phi11[2][0];
phi[2][1] = phi1[2][1]; phi[2][1] = phi11[2][1];
phi[2][2] = phi1[2][2]; phi[2][2] = phi11[2][2];
phi[2][3] = phi2[2][0]; phi[2][3] = phi12[2][0];
phi[2][4] = phi2[2][1]; phi[2][4] = phi12[2][1];
phi[2][5] = phi2[2][2]; phi[2][5] = phi12[2][2];
// Propagated Quaternion // Propagated Quaternion
double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * sampleTime); double rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3); VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
@ -1148,25 +1101,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(rotSin, *skewSin); MathOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
MatrixOperations<double>::subtract(*omega1, *skewSin, *omega1, 3, 3); double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]}, MatrixOperations<double>::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3);
{omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]}, double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]},
{omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]}, {subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]},
{subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]},
{-rotSin[0], -rotSin[1], -rotSin[2], rotCos}}; {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
// Update Covariance Matrix // Update Covariance Matrix
double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6]; double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6];
MatrixOperations<double>::transpose(*phi, *transPhi, 6);
MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6);
MatrixOperations<double>::multiply(*phi, *cov0, *cov0, 6, 6, 6);
MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6);
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
MatrixOperations<double>::transpose(*phi, *transPhi, 6); MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6);
MatrixOperations<double>::multiply(*phi, *cov2, *cov2, 6, 6, 6);
MatrixOperations<double>::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6);
validMekf = true; validMekf = true;
// Discrete Time Step // Discrete Time Step

17
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -20,7 +20,7 @@
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h" #include "AcsParameters.h"
#include "config/classIds.h" #include "eive/resultClassIds.h"
class MultiplicativeKalmanFilter { class MultiplicativeKalmanFilter {
public: public:
@ -61,17 +61,13 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_, const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS, const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData); const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
// Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
// (/config/returnvalues/classIDs.h) static const Event RESET =
// MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
// resetting Mekf // resetting Mekf
static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01); static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2);
static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3);
private: private:
/*Parameters*/ /*Parameters*/
@ -79,7 +75,6 @@ class MultiplicativeKalmanFilter {
AcsParameters::KalmanFilterParameters *kalmanFilterParameters; AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4]; double quaternion_STR_SB[4];
bool validInit; bool validInit;
double sampleTime = 0.1;
/*States*/ /*States*/
double initialQuaternion[4]; /*after reset?QUEST*/ double initialQuaternion[4]; /*after reset?QUEST*/

2
mission/controller/acs/Navigation.cpp
View File

@ -39,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
mekfData); // VALIDS FOR QUAT AND RATE ?? mekfData); // VALIDS FOR QUAT AND RATE ??
} else { } else {
multiplicativeKalmanFilter.init( multiplicativeKalmanFilter.init(

272
mission/controller/acs/SensorProcessing.cpp
View File

@ -1,3 +1,10 @@
/*
* SensorProcessing.cpp
*
* Created on: 7 Mar 2022
* Author: Robin Marquardt
*/
#include "SensorProcessing.h" #include "SensorProcessing.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
@ -14,8 +21,7 @@
using namespace Math; using namespace Math;
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
: savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
SensorProcessing::~SensorProcessing() {} SensorProcessing::~SensorProcessing() {}
@ -27,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid, const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) { acsctrl::MgmDataProcessed *mgmDataProcessed) {
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
Igrf13Model igrf13;
// So the line above should not be done here. Update: Can be done here as long updated coffs
// stored in acsParameters ?
igrf13.schmidtNormalization();
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
}
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
{ {
PoolReadGuard pg(mgmDataProcessed); PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float)); float zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double)); std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float));
mgmDataProcessed->setValidity(false, true); mgmDataProcessed->setValidity(false, true);
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
} }
} }
return; return;
@ -47,71 +69,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0}, mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
mgm4ValueNoBias[3] = {0, 0, 0}; mgm4ValueNoBias[3] = {0, 0, 0};
float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
if (mgm0valid) { if (mgm0valid) {
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
3); mgm0ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
mgm0ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
mgm0ValueCalib, 3, 3, 1); mgm0ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
mgm0ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i]; sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
} }
} }
if (mgm1valid) { if (mgm1valid) {
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
3); mgm1ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
mgm1ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
mgm1ValueCalib, 3, 3, 1); mgm1ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
mgm1ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i]; sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
} }
} }
if (mgm2valid) { if (mgm2valid) {
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
3); mgm2ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
mgm2ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
mgm2ValueCalib, 3, 3, 1); mgm2ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
mgm2ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i]; sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
} }
} }
if (mgm3valid) { if (mgm3valid) {
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
3); mgm3ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
mgm3ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
mgm3ValueCalib, 3, 3, 1); mgm3ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
mgm3ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i]; sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
} }
} }
if (mgm4valid) { if (mgm4valid) {
float mgm4ValueNT[3]; float mgm4ValueUT[3];
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3); // uT to nT VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT
VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset, MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
mgm4ValueBody, 3, 3, 1);
VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
mgm4ValueNoBias, 3); mgm4ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias, MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
mgm4ValueCalib, 3, 3, 1); mgm4ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
mgm4ValueBody, 3, 3, 1);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i]; sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
} }
} }
@ -128,35 +151,23 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
savedMgmVecTot[i] = mgmVecTot[i]; savedMgmVecTot[i] = mgmVecTot[i];
mgmVecTotDerivativeValid = true;
} }
} }
timeOfSavedMagFieldEst = timeOfMgmMeasurement; timeOfSavedMagFieldEst = timeOfMgmMeasurement;
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
Igrf13Model igrf13;
// So the line above should not be done here. Update: Can be done here as long updated coffs
// stored in acsParameters ?
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
}
{ {
PoolReadGuard pg(mgmDataProcessed); PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm0vec.setValid(mgm0valid); mgmDataProcessed->mgm0vec.setValid(mgm0valid);
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm1vec.setValid(mgm1valid); mgmDataProcessed->mgm1vec.setValid(mgm1valid);
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm2vec.setValid(mgm2valid); mgmDataProcessed->mgm2vec.setValid(mgm2valid);
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm3vec.setValid(mgm3valid); mgmDataProcessed->mgm3vec.setValid(mgm3valid);
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
mgmDataProcessed->mgm4vec.setValid(mgm4valid); mgmDataProcessed->mgm4vec.setValid(mgm4valid);
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double)); std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
mgmDataProcessed->mgmVecTot.setValid(true); mgmDataProcessed->mgmVecTot.setValid(true);
@ -180,6 +191,25 @@ void SensorProcessing::processSus(
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) { acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
// Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0};
double JC2000 = JD2000 / 36525.;
double meanLongitude =
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
sunModelParameters->p2 * sin(2 * meanAnomaly);
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
if (sus0valid) { if (sus0valid) {
sus0valid = susConverter.checkSunSensorData(sus0Value); sus0valid = susConverter.checkSunSensorData(sus0Value);
} }
@ -222,22 +252,24 @@ void SensorProcessing::processSus(
{ {
PoolReadGuard pg(susDataProcessed); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); float zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double)); std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
susDataProcessed->setValidity(false, true); susDataProcessed->setValidity(false, true);
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
susDataProcessed->sunIjkModel.setValid(true);
} }
} }
return; return;
@ -256,16 +288,6 @@ void SensorProcessing::processSus(
susParameters->sus0coeffBeta), susParameters->sus0coeffBeta),
sus0VecBody, 3, 3, 1); sus0VecBody, 3, 3, 1);
} }
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
susDataProcessed->sus0vec.setValid(sus0valid);
if (!sus0valid) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
}
}
}
if (sus1valid) { if (sus1valid) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus1orientationMatrix[0], susParameters->sus1orientationMatrix[0],
@ -273,16 +295,6 @@ void SensorProcessing::processSus(
susParameters->sus1coeffBeta), susParameters->sus1coeffBeta),
sus1VecBody, 3, 3, 1); sus1VecBody, 3, 3, 1);
} }
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
susDataProcessed->sus1vec.setValid(sus1valid);
if (!sus1valid) {
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
}
}
}
if (sus2valid) { if (sus2valid) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus2orientationMatrix[0], susParameters->sus2orientationMatrix[0],
@ -387,30 +399,10 @@ void SensorProcessing::processSus(
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i]; savedSusVecTot[i] = susVecTot[i];
susVecTotDerivativeValid = true;
} }
} }
timeOfSavedSusDirEst = timeOfSusMeasurement; timeOfSavedSusDirEst = timeOfSusMeasurement;
/* -------- Sun Model Direction (IJK frame) ------- */
// if (useSunModel) eventuell
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
// Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0};
double JC2000 = JD2000 / 36525;
double meanLongitude =
(sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
sunModelParameters->p2 * sin(2 * meanAnomaly);
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
{ {
PoolReadGuard pg(susDataProcessed); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -467,6 +459,7 @@ void SensorProcessing::processGyr(
{ {
PoolReadGuard pg(gyrDataProcessed); PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
double zeroVector[3] = {0.0, 0.0, 0.0};
std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
@ -486,6 +479,8 @@ void SensorProcessing::processGyr(
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1); gyr0ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -495,6 +490,8 @@ void SensorProcessing::processGyr(
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1); gyr1ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -504,6 +501,8 @@ void SensorProcessing::processGyr(
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1); gyr2ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -513,6 +512,8 @@ void SensorProcessing::processGyr(
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1); gyr3ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -523,7 +524,7 @@ void SensorProcessing::processGyr(
// take ADIS measurements, if both avail // take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion // if just one ADIS measurement avail, perform sensor fusion
double gyrVecTot[3] = {0.0, 0.0, 0.0}; double gyrVecTot[3] = {0.0, 0.0, 0.0};
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) {
double gyr02ValuesSum[3]; double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
@ -550,30 +551,45 @@ void SensorProcessing::processGyr(
} }
} }
void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
const double gpsAltitude, const double gpsUnixSeconds,
const bool validGps, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) { acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process // name to convert not process
double gdLongitude, gcLatitude; double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
if (validGps) { if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gps0longitude * PI / 180; gdLongitude = gpsLongitude * PI / 180.;
double latitudeRad = gps0latitude * PI / 180; double latitudeRad = gpsLatitude * PI / 180.;
double eccentricityWgs84 = 0.0818195; double eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2); double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad)); gcLatitude = atan(factor * tan(latitudeRad));
// validGcLatitude = true;
// Calculation of the satellite velocity in earth fixed frame
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
if (validSavedPosSatE &&
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
}
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
} }
{ {
PoolReadGuard pg(gpsDataProcessed); PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude; gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->setValidity(validGps, validGps); std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
if (!validGps) { std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->gdLongitude.value = 0.0; gpsDataProcessed->setValidity(validGps, true);
gpsDataProcessed->gcLatitude.value = 0.0;
}
} }
} }
} }
@ -585,10 +601,12 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) { const AcsParameters *acsParameters) {
sensorValues->update(); sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, processGps(
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.isValid()), sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
gpsDataProcessed); (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()),
&acsParameters->gpsParameters, gpsDataProcessed);
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),

19
mission/controller/acs/SensorProcessing.h
View File

@ -1,5 +1,5 @@
/******************************* /*******************************
* EIVE Flight Software Framework (FSFW) * EIVE Flight Software
* (c) 2022 IRS, Uni Stuttgart * (c) 2022 IRS, Uni Stuttgart
*******************************/ *******************************/
#ifndef SENSORPROCESSING_H_ #ifndef SENSORPROCESSING_H_
@ -13,7 +13,7 @@
#include "AcsParameters.h" #include "AcsParameters.h"
#include "SensorValues.h" #include "SensorValues.h"
#include "SusConverter.h" #include "SusConverter.h"
#include "config/classIds.h" #include "eive/resultClassIds.h"
class SensorProcessing { class SensorProcessing {
public: public:
@ -65,17 +65,20 @@ class SensorProcessing {
void processStr(); void processStr();
void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
const double gpsUnixSeconds, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed); acsctrl::GpsDataProcessed *gpsDataProcessed);
double savedMgmVecTot[3]; double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst; timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3]; double savedSusVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedSusDirEst; timeval timeOfSavedSusDirEst;
bool validMagField; bool validMagField = false;
bool validGcLatitude;
const float zeroVector[3] = {0.0, 0.0, 0.0}; double savedPosSatE[3] = {0.0, 0.0, 0.0};
double timeOfSavedPosSatE = 0.0;
bool validSavedPosSatE = false;
SusConverter susConverter; SusConverter susConverter;
AcsParameters acsParameters; AcsParameters acsParameters;

2
mission/controller/acs/SensorValues.h
View File

@ -1,12 +1,12 @@
#ifndef SENSORVALUES_H_ #ifndef SENSORVALUES_H_
#define SENSORVALUES_H_ #define SENSORVALUES_H_
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"

18
mission/controller/acs/config/classIds.h
View File

@ -1,18 +0,0 @@
#ifndef ACS_CONFIG_CLASSIDS_H_
#define ACS_CONFIG_CLASSIDS_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/returnvalues/FwClassIds.h>
namespace CLASS_ID {
enum eiveclassIds : uint8_t {
EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
KALMAN,
SAFE,
PTG,
DETUMBLE,
EIVE_CLASS_ID_END // [EXPORT] : [END]
};
}
#endif /* ACS_CONFIG_CLASSIDS_H_ */

16
mission/controller/acs/control/Detumble.cpp
View File

@ -22,7 +22,7 @@ Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParamet
Detumble::~Detumble() {} Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); detumbleParameter = &(acsParameters_->detumbleParameter);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter); magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
} }
@ -31,7 +31,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
if (!magRateValid || !magFieldValid) { if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double gain = detumbleCtrlParameters->gainD; double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2); double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3); VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK; return returnvalue::OK;
@ -50,3 +50,15 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
return returnvalue::OK; return returnvalue::OK;
} }
ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
const double *magField, const bool *magFieldValid,
double *magMom) {
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
return returnvalue::OK;
}

20
mission/controller/acs/control/Detumble.h
View File

@ -1,10 +1,3 @@
/*
* Detumble.h
*
* Created on: 17 Aug 2022
* Author: Robin Marquardt
*/
#ifndef ACS_CONTROL_DETUMBLE_H_ #ifndef ACS_CONTROL_DETUMBLE_H_
#define ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_
@ -15,17 +8,17 @@
#include "../AcsParameters.h" #include "../AcsParameters.h"
#include "../SensorValues.h" #include "../SensorValues.h"
#include "../config/classIds.h" #include "eive/resultClassIds.h"
class Detumble { class Detumble {
public: public:
Detumble(AcsParameters *acsParameters_); Detumble(AcsParameters *acsParameters_);
virtual ~Detumble(); virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class /* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
@ -35,8 +28,13 @@ class Detumble {
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom);
private: private:
AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters; AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter *magnetorquesParameter; AcsParameters::MagnetorquesParameter *magnetorquesParameter;
}; };

60
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -21,21 +21,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices); rwMatrices = &(acsParameters_->rwMatrices);
} }
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate, void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *rwPseudoInv, double *torqueRws) { const double *qError, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix // Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om; double om = pointingLawParameters->om;
double zeta = pointingModeControllerParameters->zeta; double zeta = pointingLawParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin; double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax; double omMax = pointingLawParameters->omMax;
double cInt = 2 * om * zeta; double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2); double kInt = 2 * pow(om, 2);
@ -104,12 +104,14 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
double torque[3] = {0, 0, 0}; double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3); VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
} }
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) { int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
mgtDpDes[0] = 0; mgtDpDes[0] = 0;
mgtDpDes[1] = 0; mgtDpDes[1] = 0;
mgtDpDes[2] = 0; mgtDpDes[2] = 0;
@ -127,17 +129,18 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3); VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error // calculating momentum error
double deltaMomentum[3] = {0, 0, 0}; double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract( VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); deltaMomentum, 3);
// resulting magnetic dipole command // resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0}; double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField); VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0; double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; factor = (pointingLawParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
} }
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; double wheelMomentum[4] = {0, 0, 0, 0};
@ -149,7 +152,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
wheelMomentum, 4); wheelMomentum, 4);
double gainNs = pointingModeControllerParameters->gainNullspace; double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
*nullSpaceMatrix, 4); *nullSpaceMatrix, 4);
@ -157,3 +160,32 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4); VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
} }
void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
double *torqueCommand) {
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
(omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
}
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}

27
mission/controller/acs/control/PtgCtrl.h
View File

@ -20,7 +20,7 @@
#include "../AcsParameters.h" #include "../AcsParameters.h"
#include "../SensorValues.h" #include "../SensorValues.h"
#include "../config/classIds.h" #include "eive/resultClassIds.h"
class PtgCtrl { class PtgCtrl {
public: public:
@ -30,10 +30,10 @@ class PtgCtrl {
PtgCtrl(AcsParameters *acsParameters_); PtgCtrl(AcsParameters *acsParameters_);
virtual ~PtgCtrl(); virtual ~PtgCtrl();
static const uint8_t INTERFACE_ID = CLASS_ID::PTG; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class /* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
@ -41,21 +41,32 @@ class PtgCtrl {
/* @brief: Calculates the needed torque for the pointing control mechanism /* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes); double *mgtDpDes);
void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, double *rwTrqNs); const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low
* @param: rwAvailable Boolean Flag for all reaction wheels
* omegaRw current wheel speed of reaction wheels
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
private: private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices; AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0};
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #endif /* ACS_CONTROL_PTGCTRL_H_ */

15
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -87,7 +87,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
return returnvalue::OK; return returnvalue::OK;
} }
// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid, bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef, double *magRateB, bool magRateBValid, double *sunDirRef,
@ -127,20 +127,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3); VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
/* Only valid if angle between sun direction and magnetic field direction /* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */ * is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
double sinAngle = 0; if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
sinAngle = sin(acos(cos(cosAngleSunMag)));
if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
return; return;
} }
// Rate for Torque Calculation // Rate for Torque Calculation
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3); VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation // Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0; double kRateNoMekf = 0, kAlignNoMekf = 0;
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;

11
mission/controller/acs/control/SafeCtrl.h
View File

@ -1,10 +1,3 @@
/*
* safeCtrl.h
*
* Created on: 19 Apr 2022
* Author: rooob
*/
#ifndef SAFECTRL_H_ #ifndef SAFECTRL_H_
#define SAFECTRL_H_ #define SAFECTRL_H_
@ -14,14 +7,14 @@
#include "../AcsParameters.h" #include "../AcsParameters.h"
#include "../SensorValues.h" #include "../SensorValues.h"
#include "../config/classIds.h" #include "eive/resultClassIds.h"
class SafeCtrl { class SafeCtrl {
public: public:
SafeCtrl(AcsParameters *acsParameters_); SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl(); virtual ~SafeCtrl();
static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);

46
mission/controller/acs/util/MathOperations.h
View File

@ -96,8 +96,16 @@ class MathOperations {
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
T2 *cartesianOutput) { T2 *cartesianOutput) {
double radiusPolar = 6378137; /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
double radiusEqua = 6356752.314; * longitude and altitude
* @param: lat geodetic latitude [rad]
* longi longitude [rad]
* alt altitude [m]
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
* Landis Markley and John L. Crassidis*/
double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
@ -106,13 +114,13 @@ class MathOperations {
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
} }
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time * @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff * outputDotDcmEJ Derivative of transformation matrix [3][3]
* Landis Markley and John L. Crassidis*/ * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
static void dcmEJ(timeval time, T1 *outputDcmEJ) { * Landis Markley and John L. Crassidis*/
double JD2000Floor = 0; double JD2000Floor = 0;
double JD2000 = convertUnixToJD2000(time); double JD2000 = convertUnixToJD2000(time);
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0) // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
@ -143,6 +151,16 @@ class MathOperations {
outputDcmEJ[6] = 0; outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0; outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1; outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
} }
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
@ -215,7 +233,7 @@ class MathOperations {
precession[2][2] = cos(theta2); precession[2][2] = cos(theta2);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Nutation size // Calculation of Transformation from earth Nutation N
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// lunar asc node // lunar asc node
@ -230,7 +248,6 @@ class MathOperations {
// % true obliquity of the ecliptic eps p.71 (simplified) // % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
;
nutation[0][0] = cos(dp); nutation[0][0] = cos(dp);
nutation[1][0] = cos(e + de) * sin(dp); nutation[1][0] = cos(e + de) * sin(dp);
@ -260,10 +277,9 @@ class MathOperations {
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
} }
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
int i, j; int i, j;
double determinant; double determinant = 0;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
{matrix[3], matrix[4], matrix[5]}, {matrix[3], matrix[4], matrix[5]},
{matrix[6], matrix[7], matrix[8]}}; {matrix[6], matrix[7], matrix[8]}};
@ -272,8 +288,8 @@ class MathOperations {
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] - determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3])); mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
} }
// cout<<"\size\ndeterminant: "<<determinant; // cout<<"\n\ndeterminant: "<<determinant;
// cout<<"\size\nInverse of matrix is: \size"; // cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) { for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) - output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -

8
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -86,6 +86,8 @@ enum PoolIds : lp_id_t {
// GPS Processed // GPS Processed
GC_LATITUDE, GC_LATITUDE,
GD_LONGITUDE, GD_LONGITUDE,
GPS_POSITION,
GPS_VELOCITY,
// MEKF // MEKF
SAT_ROT_RATE_MEKF, SAT_ROT_RATE_MEKF,
QUAT_MEKF, QUAT_MEKF,
@ -99,13 +101,13 @@ enum PoolIds : lp_id_t {
MTQ_TARGET_DIPOLE, MTQ_TARGET_DIPOLE,
}; };
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8; static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8;
static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@ -224,6 +226,8 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this); lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this); lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
private: private:
}; };

17
mission/core/GenericFactory.cpp
View File

@ -123,18 +123,19 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR); new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib); new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
*cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50, 15, PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50, 15);
*ipcStore); *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID);
*pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore, sdcMan, PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore,
config::MAX_PUS_FUNNEL_QUEUE_DEPTH, 15, *ipcStore); config::MAX_PUS_FUNNEL_QUEUE_DEPTH, 15);
*pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan);
#if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1
(*cfdpFunnel)->addDestination(*udpBridge, 0); (*cfdpFunnel)->addDestination("UDP Server", *udpBridge, 0);
(*pusFunnel)->addDestination(*udpBridge, 0); (*pusFunnel)->addDestination("UDP Server", *udpBridge, 0);
#endif #endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1 #if OBSW_ADD_TMTC_TCP_SERVER == 1
(*cfdpFunnel)->addDestination(*tcpBridge, 0); (*cfdpFunnel)->addDestination("TCP Server", *tcpBridge, 0);
(*pusFunnel)->addDestination(*tcpBridge, 0); (*pusFunnel)->addDestination("TCP Server", *tcpBridge, 0);
#endif #endif
#endif #endif
// Every TM packet goes through this funnel // Every TM packet goes through this funnel

3
mission/devices/devicedefinitions/GPSDefinitions.h
View File

@ -13,6 +13,9 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER;
//! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix //! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix
//! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix //! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix
static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO); static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time
//! to get a fix after the GPS was switched on.
static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
static constexpr DeviceCommandId_t GPS_REPLY = 0; static constexpr DeviceCommandId_t GPS_REPLY = 0;
static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5; static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;

4
mission/system/objects/AcsSubsystem.cpp
View File

@ -58,7 +58,7 @@ void AcsSubsystem::handleEventMessages() {
case EventMessage::EVENT_MESSAGE: case EventMessage::EVENT_MESSAGE:
if (event.getEvent() == acs::SAFE_RATE_VIOLATION) { if (event.getEvent() == acs::SAFE_RATE_VIOLATION) {
CommandMessage msg; CommandMessage msg;
ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::DETUMBLE, 0); ModeMessage::setCmdModeMessage(msg, acs::AcsMode::DETUMBLE, 0);
ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
@ -66,7 +66,7 @@ void AcsSubsystem::handleEventMessages() {
} }
if (event.getEvent() == acs::SAFE_RATE_RECOVERY) { if (event.getEvent() == acs::SAFE_RATE_RECOVERY) {
CommandMessage msg; CommandMessage msg;
ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::SAFE, 0); ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl; sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl;

97
mission/system/tree/acsModeTree.cpp
View File

@ -30,69 +30,69 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper);
static const auto OFF = HasModesIF::MODE_OFF; static const auto OFF = HasModesIF::MODE_OFF;
static const auto NML = DeviceHandlerIF::MODE_NORMAL; static const auto NML = DeviceHandlerIF::MODE_NORMAL;
auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlSubmode::OFF, FixedArrayList<ModeListEntry, 3>()); auto ACS_SEQUENCE_OFF = std::make_pair(acs::AcsMode::OFF, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_OFF_TGT = auto ACS_TABLE_OFF_TGT =
std::make_pair((acs::CtrlSubmode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
auto ACS_TABLE_OFF_TRANS_0 = auto ACS_TABLE_OFF_TRANS_0 =
std::make_pair((acs::CtrlSubmode::OFF << 24) | 2, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::OFF << 24) | 2, FixedArrayList<ModeListEntry, 1>());
auto ACS_TABLE_OFF_TRANS_1 = auto ACS_TABLE_OFF_TRANS_1 =
std::make_pair((acs::CtrlSubmode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 6>());
auto ACS_SEQUENCE_DETUMBLE = auto ACS_SEQUENCE_DETUMBLE =
std::make_pair(acs::CtrlSubmode::DETUMBLE, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_DETUMBLE_TGT = auto ACS_TABLE_DETUMBLE_TGT =
std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 1, FixedArrayList<ModeListEntry, 4>()); std::make_pair((acs::AcsMode::DETUMBLE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
auto ACS_TABLE_DETUMBLE_TRANS_0 = auto ACS_TABLE_DETUMBLE_TRANS_0 =
std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 2, FixedArrayList<ModeListEntry, 5>()); std::make_pair((acs::AcsMode::DETUMBLE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto ACS_TABLE_DETUMBLE_TRANS_1 = auto ACS_TABLE_DETUMBLE_TRANS_1 =
std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_SAFE = std::make_pair(acs::CtrlSubmode::SAFE, FixedArrayList<ModeListEntry, 3>()); auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_SAFE_TGT = auto ACS_TABLE_SAFE_TGT =
std::make_pair((acs::CtrlSubmode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 4>()); std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
auto ACS_TABLE_SAFE_TRANS_0 = auto ACS_TABLE_SAFE_TRANS_0 =
std::make_pair((acs::CtrlSubmode::SAFE << 24) | 2, FixedArrayList<ModeListEntry, 5>()); std::make_pair((acs::AcsMode::SAFE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto ACS_TABLE_SAFE_TRANS_1 = auto ACS_TABLE_SAFE_TRANS_1 =
std::make_pair((acs::CtrlSubmode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_IDLE = std::make_pair(acs::CtrlSubmode::IDLE, FixedArrayList<ModeListEntry, 3>()); auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_IDLE_TGT = auto ACS_TABLE_IDLE_TGT =
std::make_pair((acs::CtrlSubmode::IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_IDLE_TRANS_0 = auto ACS_TABLE_IDLE_TRANS_0 =
std::make_pair((acs::CtrlSubmode::IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_IDLE_TRANS_1 = auto ACS_TABLE_IDLE_TRANS_1 =
std::make_pair((acs::CtrlSubmode::IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>()); std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>());
auto ACS_TABLE_PTG_TRANS_0 = auto ACS_TABLE_PTG_TRANS_0 =
std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>()); std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto ACS_SEQUENCE_PTG_TARGET = auto ACS_SEQUENCE_PTG_TARGET =
std::make_pair(acs::CtrlSubmode::PTG_TARGET, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::PTG_TARGET, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_TGT = auto ACS_TABLE_PTG_TARGET_TGT =
std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 1, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_TRANS_1 = auto ACS_TABLE_PTG_TARGET_TRANS_1 =
std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_GS = auto ACS_SEQUENCE_PTG_TARGET_GS =
std::make_pair(acs::CtrlSubmode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_GS_TGT = auto ACS_TABLE_PTG_TARGET_GS_TGT =
std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 =
std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_NADIR = auto ACS_SEQUENCE_PTG_TARGET_NADIR =
std::make_pair(acs::CtrlSubmode::PTG_TARGET_NADIR, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 1, auto ACS_TABLE_PTG_TARGET_NADIR_TGT =
FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 =
(acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = auto ACS_SEQUENCE_PTG_TARGET_INERTIAL =
std::make_pair(acs::CtrlSubmode::PTG_TARGET_INERTIAL, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT =
(acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 =
(acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());
void satsystem::acs::init() { void satsystem::acs::init() {
ModeListEntry entry; ModeListEntry entry;
@ -122,7 +122,7 @@ void satsystem::acs::init() {
buildTargetPtGsSequence(ACS_SUBSYSTEM, entry); buildTargetPtGsSequence(ACS_SUBSYSTEM, entry);
buildTargetPtNadirSequence(ACS_SUBSYSTEM, entry); buildTargetPtNadirSequence(ACS_SUBSYSTEM, entry);
buildTargetPtInertialSequence(ACS_SUBSYSTEM, entry); buildTargetPtInertialSequence(ACS_SUBSYSTEM, entry);
ACS_SUBSYSTEM.setInitialMode(::acs::CtrlSubmode::SAFE); ACS_SUBSYSTEM.setInitialMode(::acs::AcsMode::SAFE);
} }
namespace { namespace {
@ -190,7 +190,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc); check(sequence.insert(eh), ctxc);
}; };
// Build SAFE target // Build SAFE target
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TGT.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second);
@ -206,7 +206,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc); ctxc);
// Build SAFE transition 1 // Build SAFE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TRANS_1.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true), check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true),
ctxc); ctxc);
@ -239,7 +239,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc); check(sequence.insert(eh), ctxc);
}; };
// Build DETUMBLE target // Build DETUMBLE target
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
@ -257,7 +257,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc); ctxc);
// Build DETUMBLE transition 1 // Build DETUMBLE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false,
true), true),
ctxc); ctxc);
@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc); check(sequence.insert(eh), ctxc);
}; };
// Build IDLE target // Build IDLE target
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
@ -307,7 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
// Build IDLE transition 1 // Build IDLE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second);
ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true);
// Build IDLE sequence // Build IDLE sequence
@ -339,7 +339,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build TARGET PT table // Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
@ -350,8 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built // Transition 0 already built
// Build TARGET PT transition 1 // Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second);
ACS_TABLE_PTG_TARGET_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false,
true), true),
ctxc); ctxc);
@ -386,7 +385,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build TARGET PT table // Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET,
ACS_TABLE_PTG_TARGET_NADIR_TGT.second); ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
@ -399,7 +398,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built // Transition 0 already built
// Build TARGET PT transition 1 // Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_NADIR, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR,
ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)),
@ -436,7 +435,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build TARGET PT table // Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS,
ACS_TABLE_PTG_TARGET_GS_TGT.second); ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
@ -449,7 +448,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built // Transition 0 already built
// Build TARGET PT transition 1 // Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS,
ACS_TABLE_PTG_TARGET_GS_TRANS_1.second); ACS_TABLE_PTG_TARGET_GS_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)), &ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)),
@ -485,7 +484,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build TARGET PT table // Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
@ -498,7 +497,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built // Transition 0 already built
// Build TARGET PT transition 1 // Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)),

14
mission/tmtc/CfdpTmFunnel.cpp
View File

@ -4,11 +4,8 @@
#include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h" #include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
#include "fsfw/tmtcservices/TmTcMessage.h" #include "fsfw/tmtcservices/TmTcMessage.h"
CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, uint16_t cfdpInCcsdsApid)
StorageManagerIF& tmStore, uint32_t tmMsgDepth, uint32_t tcMsgDepth, : TmFunnelBase(cfg), cfdpInCcsdsApid(cfdpInCcsdsApid) {}
StorageManagerIF& ipcStore)
: TmFunnelBase(objectId, tmStore, tmMsgDepth, tcMsgDepth, ipcStore),
cfdpInCcsdsApid(cfdpInCcsdsApid) {}
const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; } const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
@ -70,7 +67,7 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
msg.setStorageId(newStoreId); msg.setStorageId(newStoreId);
store_address_t origStoreId = newStoreId; store_address_t origStoreId = newStoreId;
for (unsigned int idx = 0; idx < destinations.size(); idx++) { for (unsigned int idx = 0; idx < destinations.size(); idx++) {
const auto& destVcidPair = destinations[idx]; const auto& dest = destinations[idx];
if (destinations.size() > 1) { if (destinations.size() > 1) {
if (idx < destinations.size() - 1) { if (idx < destinations.size() - 1) {
// Create copy of data to ensure each TM recipient has its own copy. That way, we don't need // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
@ -89,10 +86,11 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
msg.setStorageId(origStoreId); msg.setStorageId(origStoreId);
} }
} }
result = tmQueue->sendMessage(destVcidPair.first, &msg); result = tmQueue->sendMessage(dest.queueId, &msg);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name
<< " failed" << std::endl;
#endif #endif
tmStore.deleteData(msg.getStorageId()); tmStore.deleteData(msg.getStorageId());
} }

3
mission/tmtc/CfdpTmFunnel.h
View File

@ -12,8 +12,7 @@
class CfdpTmFunnel : public TmFunnelBase { class CfdpTmFunnel : public TmFunnelBase {
public: public:
CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, StorageManagerIF& tmStore, CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, uint16_t cfdpInCcsdsApid);
uint32_t tmMsgDepth, uint32_t tcMsgDepth, StorageManagerIF& ipcStore);
[[nodiscard]] const char* getName() const override; [[nodiscard]] const char* getName() const override;
ReturnValue_t performOperation(uint8_t opCode); ReturnValue_t performOperation(uint8_t opCode);
ReturnValue_t initialize() override; ReturnValue_t initialize() override;

2
mission/tmtc/PersistentTmStore.cpp
View File

@ -57,7 +57,7 @@ ReturnValue_t TmStore::passPacket(PusTmReader& reader) {
return returnvalue::OK; return returnvalue::OK;
} }
void TmStore::dumpFrom(uint32_t fromUnixSeconds, TmFunnelBase &tmFunnel) { void TmStore::dumpFrom(uint32_t fromUnixSeconds, TmFunnelBase& tmFunnel) {
return dumpFromUpTo(fromUnixSeconds, currentTv.tv_sec, tmFunnel); return dumpFromUpTo(fromUnixSeconds, currentTv.tv_sec, tmFunnel);
} }

15
mission/tmtc/PusTmFunnel.cpp
View File

@ -8,10 +8,9 @@
#include "fsfw/tmstorage/TmStoreMessage.h" #include "fsfw/tmstorage/TmStoreMessage.h"
#include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h" #include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"
PusTmFunnel::PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore, PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, TimeReaderIF &timeReader,
SdCardMountedIF &sdcMan, uint32_t tmMsgDepth, uint32_t tcMsgDepth, SdCardMountedIF &sdcMan)
StorageManagerIF &ipcStore) : TmFunnelBase(cfg),
: TmFunnelBase(objectId, tmStore, tmMsgDepth, tcMsgDepth, ipcStore),
timeReader(timeReader), timeReader(timeReader),
miscStore(objects::MISC_TM_STORE, "tm", "misc", RolloverInterval::HOURLY, 2, currentTv, miscStore(objects::MISC_TM_STORE, "tm", "misc", RolloverInterval::HOURLY, 2, currentTv,
tmStore, sdcMan), tmStore, sdcMan),
@ -107,17 +106,21 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
TmTcMessage currentMessage; TmTcMessage currentMessage;
status = tmQueue->receiveMessage(&currentMessage); status = tmQueue->receiveMessage(&currentMessage);
while (status == returnvalue::OK) { while (status == returnvalue::OK) {
status = handlePacket(currentMessage); status = handleTmPacket(currentMessage);
if (status != returnvalue::OK) { if (status != returnvalue::OK) {
sif::warning << "TmFunnel packet handling failed" << std::endl; sif::warning << "TmFunnel packet handling failed" << std::endl;
break; break;
} }
status = tmQueue->receiveMessage(&currentMessage); status = tmQueue->receiveMessage(&currentMessage);
} }
if (status == MessageQueueIF::EMPTY) {
return returnvalue::OK;
}
return status; return status;
} }
ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) { ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) {
uint8_t *packetData = nullptr; uint8_t *packetData = nullptr;
size_t size = 0; size_t size = 0;
store_address_t origStoreId = message.getStorageId(); store_address_t origStoreId = message.getStorageId();

6
mission/tmtc/PusTmFunnel.h
View File

@ -22,9 +22,7 @@
*/ */
class PusTmFunnel : public TmFunnelBase { class PusTmFunnel : public TmFunnelBase {
public: public:
explicit PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore, PusTmFunnel(TmFunnelBase::FunnelCfg cfg, TimeReaderIF &timeReader, SdCardMountedIF &sdcMan);
SdCardMountedIF &sdcMan, uint32_t tmMsgDepth, uint32_t tcMsgDepth,
StorageManagerIF &ipcStore);
[[nodiscard]] const char *getName() const override; [[nodiscard]] const char *getName() const override;
~PusTmFunnel() override; ~PusTmFunnel() override;
@ -45,7 +43,7 @@ class PusTmFunnel : public TmFunnelBase {
TmStore hkStore; TmStore hkStore;
SdCardMountedIF &sdcMan; SdCardMountedIF &sdcMan;
ReturnValue_t handlePacket(TmTcMessage &message); ReturnValue_t handleTmPacket(TmTcMessage &message);
void initStoresIfPossible(bool sdCardUsable); void initStoresIfPossible(bool sdCardUsable);
ReturnValue_t initialize() override; ReturnValue_t initialize() override;
}; };

22
mission/tmtc/TmFunnelBase.cpp
View File

@ -4,24 +4,22 @@
#include "fsfw/ipc/QueueFactory.h" #include "fsfw/ipc/QueueFactory.h"
TmFunnelBase::TmFunnelBase(object_id_t objectId, StorageManagerIF &tmStore, uint32_t tmMsgDepth, TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
uint32_t tcMsgDepth, StorageManagerIF &ipcStore) : SystemObject(cfg.objectId), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) {
: SystemObject(objectId), tmStore(tmStore), ipcStore(ipcStore) { tmQueue = QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth);
tmQueue = QueueFactory::instance()->createMessageQueue(tmMsgDepth); tcQueue = QueueFactory::instance()->createMessageQueue(cfg.tcMsgDepth);
tcQueue = QueueFactory::instance()->createMessageQueue(tcMsgDepth);
} }
MessageQueueId_t TmFunnelBase::getCommandQueue() const { return tcQueue->getId(); }
TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); } TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); }
MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) const { MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) const {
return tmQueue->getId(); return tmQueue->getId();
} }
void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) { void TmFunnelBase::addDestination(const char *name, const AcceptsTelemetryIF &downlinkDestination,
uint8_t vcid) {
auto queueId = downlinkDestination.getReportReceptionQueue(vcid); auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
destinations.emplace_back(queueId, vcid); destinations.emplace_back(name, queueId, vcid);
} }
ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId, ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId,
@ -29,7 +27,7 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
const uint8_t *packetData, size_t size) { const uint8_t *packetData, size_t size) {
ReturnValue_t result; ReturnValue_t result;
for (unsigned int idx = 0; idx < destinations.size(); idx++) { for (unsigned int idx = 0; idx < destinations.size(); idx++) {
const auto &destVcidPair = destinations[idx]; const auto &dest = destinations[idx];
if (destinations.size() > 1) { if (destinations.size() > 1) {
if (idx < destinations.size() - 1) { if (idx < destinations.size() - 1) {
// Create copy of data to ensure each TM recipient has its own copy. That way, we don't need // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
@ -48,7 +46,7 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
message.setStorageId(origStoreId); message.setStorageId(origStoreId);
} }
} }
result = tmQueue->sendMessage(destVcidPair.first, &message); result = tmQueue->sendMessage(dest.queueId, &message);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
@ -58,3 +56,5 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
} }
return result; return result;
} }
MessageQueueId_t TmFunnelBase::getCommandQueue() const { return tcQueue->getId(); }

39
mission/tmtc/TmFunnelBase.h
View File

@ -13,13 +13,25 @@ class TmFunnelBase : public TmStoreFrontendSimpleIF,
public AcceptsTelemetryIF, public AcceptsTelemetryIF,
public SystemObject { public SystemObject {
public: public:
TmFunnelBase(object_id_t objectId, StorageManagerIF& tmStore, uint32_t tmMsgDepth, struct FunnelCfg {
uint32_t tcMsgDepth, StorageManagerIF& ipcStore); FunnelCfg(object_id_t objId, StorageManagerIF& tmStore, StorageManagerIF& ipcStore,
void addDestination(const AcceptsTelemetryIF& downlinkDestination, uint8_t vcid = 0); uint32_t tmMsgDepth, uint32_t tcMsgDepth)
: objectId(objId),
tmStore(tmStore),
ipcStore(ipcStore),
tmMsgDepth(tmMsgDepth),
tcMsgDepth(tcMsgDepth) {}
object_id_t objectId;
StorageManagerIF& tmStore;
StorageManagerIF& ipcStore;
uint32_t tmMsgDepth;
uint32_t tcMsgDepth;
};
TmFunnelBase(FunnelCfg cfg);
void addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid = 0);
ReturnValue_t sendPacketToDestinations(store_address_t origStoreId, TmTcMessage& message, ReturnValue_t sendPacketToDestinations(store_address_t origStoreId, TmTcMessage& message,
const uint8_t* packetData, size_t size); const uint8_t* packetData, size_t size);
[[nodiscard]] MessageQueueId_t getCommandQueue() const override;
[[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override;
virtual ~TmFunnelBase(); virtual ~TmFunnelBase();
@ -27,9 +39,22 @@ class TmFunnelBase : public TmStoreFrontendSimpleIF,
protected: protected:
StorageManagerIF& tmStore; StorageManagerIF& tmStore;
StorageManagerIF& ipcStore; StorageManagerIF& ipcStore;
std::vector<std::pair<MessageQueueId_t, uint8_t>> destinations;
MessageQueueIF* tcQueue = nullptr; struct Destination {
Destination(const char* name, MessageQueueId_t queueId, uint8_t virtualChannel)
: name(name), queueId(queueId), virtualChannel(virtualChannel) {}
const char* name;
MessageQueueId_t queueId;
uint8_t virtualChannel = 0;
};
std::vector<Destination> destinations;
MessageQueueIF* tmQueue = nullptr; MessageQueueIF* tmQueue = nullptr;
MessageQueueIF* tcQueue = nullptr;
MessageQueueId_t getCommandQueue() const override;
}; };
#endif /* MISSION_TMTC_TMFUNNELBASE_H_ */ #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */

12
release_checklist.md Normal file
View File

@ -0,0 +1,12 @@
OBSW Release Checklist
=========
# Pre-Release
1. Update version in `CMakeLists.txt`
2. Verify that the Q7S, Q7S EM and Host build are working
3. Wait for CI/CD results
# Post-Release
1. Create a new release with tag on `EGit`

2
tmtc

@ -1 +1 @@
Subproject commit 2bd6caa3c21255f2ab5a2773eb83d2fca78c2234 Subproject commit d0c8e20d4f9a6f5aee3ccfa05c4f7ab7151400b5