diff --git a/CHANGELOG.md b/CHANGELOG.md index b52806a0..bf627b3a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,13 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/). The [milestone](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/milestones) list yields a list of all related PRs for each release. +# [unreleased] + +## Added + +- First version of ACS controller + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/329 + # [v1.18.0] 01.12.2022 ## Changed diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 5cde824d..fd4c7659 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -23,7 +23,7 @@ #define OBSW_ADD_SUN_SENSORS @OBSW_ADD_SUN_SENSORS@ #define OBSW_ADD_SUS_BOARD_ASS @OBSW_ADD_SUS_BOARD_ASS@ #define OBSW_ADD_ACS_BOARD @OBSW_ADD_ACS_BOARD@ -#define OBSW_ADD_ACS_CTRL @OBSW_ADD_ACS_CTRL@ +#define OBSW_ADD_ACS_CTRL 1 #define OBSW_ADD_GPS_CTRL @OBSW_ADD_GPS_CTRL@ #define OBSW_ADD_TCS_CTRL @OBSW_ADD_TCS_CTRL@ #define OBSW_ADD_RW @OBSW_ADD_RW@ diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 5a8c3827..a866c0e2 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -165,6 +165,35 @@ void scheduling::initTasks() { scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); } #endif +#if OBSW_Q7S_EM == 1 + acsCtrlTask->addComponent(objects::MGM_0_LIS3_HANDLER); + acsCtrlTask->addComponent(objects::MGM_1_RM3100_HANDLER); + acsCtrlTask->addComponent(objects::MGM_2_LIS3_HANDLER); + acsCtrlTask->addComponent(objects::MGM_3_RM3100_HANDLER); + acsCtrlTask->addComponent(objects::IMTQ_HANDLER); + acsCtrlTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); + acsCtrlTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF); + acsCtrlTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB); + acsCtrlTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB); + acsCtrlTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB); + acsCtrlTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB); + acsCtrlTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF); + acsCtrlTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF); + acsCtrlTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); + acsCtrlTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); + acsCtrlTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); + acsCtrlTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); + acsCtrlTask->addComponent(objects::GYRO_0_ADIS_HANDLER); + acsCtrlTask->addComponent(objects::GYRO_1_L3G_HANDLER); + acsCtrlTask->addComponent(objects::GYRO_2_ADIS_HANDLER); + acsCtrlTask->addComponent(objects::GYRO_3_L3G_HANDLER); + acsCtrlTask->addComponent(objects::GPS_CONTROLLER); + acsCtrlTask->addComponent(objects::STAR_TRACKER); + acsCtrlTask->addComponent(objects::RW1); + acsCtrlTask->addComponent(objects::RW2); + acsCtrlTask->addComponent(objects::RW3); + acsCtrlTask->addComponent(objects::RW4); +#endif PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); diff --git a/dummies/CMakeLists.txt b/dummies/CMakeLists.txt index 677120fb..96ec1731 100644 --- a/dummies/CMakeLists.txt +++ b/dummies/CMakeLists.txt @@ -13,6 +13,7 @@ target_sources( PduDummy.cpp P60DockDummy.cpp SaDeploymentDummy.cpp + GpsDummy.cpp GyroAdisDummy.cpp GyroL3GD20Dummy.cpp MgmLIS3MDLDummy.cpp diff --git a/dummies/GpsDummy.cpp b/dummies/GpsDummy.cpp new file mode 100644 index 00000000..893e8464 --- /dev/null +++ b/dummies/GpsDummy.cpp @@ -0,0 +1,56 @@ +#include "GpsDummy.h" + +#include + +GpsDummy::GpsDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) + : DeviceHandlerBase(objectId, comif, comCookie) {} + +GpsDummy::~GpsDummy() {} + +void GpsDummy::doStartUp() {} + +void GpsDummy::doShutDown() {} + +ReturnValue_t GpsDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } + +ReturnValue_t GpsDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} + +ReturnValue_t GpsDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { + return returnvalue::OK; +} + +ReturnValue_t GpsDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) { + return returnvalue::OK; +} + +ReturnValue_t GpsDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + return returnvalue::OK; +} + +void GpsDummy::fillCommandAndReplyMap() {} + +uint32_t GpsDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } + +ReturnValue_t GpsDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({0.0}, 1)); + localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({0.0}, 1)); + localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry({0.0})); + localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry({7684.2})); + localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry({0})); + + return returnvalue::OK; +} diff --git a/dummies/GpsDummy.h b/dummies/GpsDummy.h new file mode 100644 index 00000000..b32b7f17 --- /dev/null +++ b/dummies/GpsDummy.h @@ -0,0 +1,33 @@ +#ifndef DUMMIES_GPSDUMMY_H_ +#define DUMMIES_GPSDUMMY_H_ + +#include + +class GpsDummy : public DeviceHandlerBase { + public: + static const DeviceCommandId_t SIMPLE_COMMAND = 1; + static const DeviceCommandId_t PERIODIC_REPLY = 2; + + static const uint8_t SIMPLE_COMMAND_DATA = 1; + static const uint8_t PERIODIC_REPLY_DATA = 2; + + GpsDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + virtual ~GpsDummy(); + + protected: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; +}; + +#endif /* DUMMIES_GPSDUMMY_H_ */ diff --git a/dummies/GyroAdisDummy.cpp b/dummies/GyroAdisDummy.cpp index 42af6412..050c838d 100644 --- a/dummies/GyroAdisDummy.cpp +++ b/dummies/GyroAdisDummy.cpp @@ -40,6 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({-0.5}, true)); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.2}, true)); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); + return returnvalue::OK; } diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index 935e32f5..20309c63 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({1.2}, true)); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.7}, true)); localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index aad1478b..fdc7b009 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -39,5 +39,9 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0, 0.0, 0.0})); + localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.12, 0.76, -0.45}, true)); + localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 7aa5fdf7..31228260 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -41,5 +41,7 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, + new PoolEntry({1.02, 0.56, -0.78}, true)); return returnvalue::OK; } diff --git a/dummies/MgmRm3100Dummy.cpp b/dummies/MgmRm3100Dummy.cpp index ad97e7be..1bd4f513 100644 --- a/dummies/MgmRm3100Dummy.cpp +++ b/dummies/MgmRm3100Dummy.cpp @@ -36,5 +36,7 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - return OK; + localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, + new PoolEntry({0.87, -0.95, 0.11}, true)); + return returnvalue::OK; } diff --git a/dummies/StarTrackerDummy.cpp b/dummies/StarTrackerDummy.cpp index 8e2dd507..71f0030c 100644 --- a/dummies/StarTrackerDummy.cpp +++ b/dummies/StarTrackerDummy.cpp @@ -41,5 +41,29 @@ uint32_t StarTrackerDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) ReturnValue_t StarTrackerDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(startracker::MCU_TEMPERATURE, new PoolEntry({0})); + + localDataPoolMap.emplace(startracker::TICKS_SOLUTION_SET, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::TIME_SOLUTION_SET, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::CALI_QW, new PoolEntry({1.0}, true)); + localDataPoolMap.emplace(startracker::CALI_QX, new PoolEntry({0.0}, true)); + localDataPoolMap.emplace(startracker::CALI_QY, new PoolEntry({0.0}, true)); + localDataPoolMap.emplace(startracker::CALI_QZ, new PoolEntry({0.0}, true)); + localDataPoolMap.emplace(startracker::TRACK_CONFIDENCE, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_QW, new PoolEntry({1.0})); + localDataPoolMap.emplace(startracker::TRACK_QX, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_QY, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_QZ, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_REMOVED, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::STARS_CENTROIDED, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::STARS_MATCHED_DATABASE, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::LISA_QW, new PoolEntry({1.0})); + localDataPoolMap.emplace(startracker::LISA_QX, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_QY, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_QZ, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_PERC_CLOSE, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_NR_CLOSE, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::TRUST_WORTHY, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::STABLE_COUNT, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::SOLUTION_STRATEGY, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 6d02ba69..c0aed6dd 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -1,79 +1,43 @@ #include "SusDummy.h" -#include +SusDummy::SusDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) + : DeviceHandlerBase(objectId, comif, comCookie), susSet(this) {} -#include -#include +SusDummy::~SusDummy() {} -SusDummy::SusDummy() : ExtendedControllerBase(objects::SUS_0_N_LOC_XFYFZM_PT_XF), susSet(this) { - ObjectManager::instance()->insert(objects::SUS_6_R_LOC_XFYBZM_PT_XF, this); - ObjectManager::instance()->insert(objects::SUS_1_N_LOC_XBYFZM_PT_XB, this); - ObjectManager::instance()->insert(objects::SUS_7_R_LOC_XBYBZM_PT_XB, this); - ObjectManager::instance()->insert(objects::SUS_2_N_LOC_XFYBZB_PT_YB, this); - ObjectManager::instance()->insert(objects::SUS_8_R_LOC_XBYBZB_PT_YB, this); - ObjectManager::instance()->insert(objects::SUS_3_N_LOC_XFYBZF_PT_YF, this); - ObjectManager::instance()->insert(objects::SUS_9_R_LOC_XBYBZB_PT_YF, this); - ObjectManager::instance()->insert(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, this); - ObjectManager::instance()->insert(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, this); - ObjectManager::instance()->insert(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, this); - ObjectManager::instance()->insert(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, this); +void SusDummy::doStartUp() {} + +void SusDummy::doShutDown() {} + +ReturnValue_t SusDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } + +ReturnValue_t SusDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; } -ReturnValue_t SusDummy::initialize() { - static bool done = false; - if (not done) { - done = true; - ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != returnvalue::OK) { - return result; - } - } - +ReturnValue_t SusDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { return returnvalue::OK; } -ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { - return returnvalue::FAILED; +ReturnValue_t SusDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) { + return returnvalue::OK; } -void SusDummy::performControlOperation() { - iteration++; - value = sin(iteration / 80. * M_PI + 10) * 10 - 10; - - susSet.read(); - susSet.temperatureCelcius = value; - if ((iteration % 100) < 20) { - susSet.setValidity(false, true); - } else { - susSet.setValidity(true, true); - } - susSet.commit(); +ReturnValue_t SusDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + return returnvalue::OK; } -ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) { +void SusDummy::fillCommandAndReplyMap() {} + +uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } + +ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, new PoolEntry({0})); + localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + new PoolEntry({0, 0, 0, 0, 0, 0}, true)); return returnvalue::OK; } - -LocalPoolDataSetBase* SusDummy::getDataSetHandle(sid_t sid) { - switch (sid.ownerSetId) { - case SUS::SUS_DATA_SET_ID: - return &susSet; - default: - return nullptr; - } -} - -ReturnValue_t SusDummy::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t* msToReachTheMode) { - if (submode != SUBMODE_NONE) { - return INVALID_SUBMODE; - } - if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { - return INVALID_MODE; - } - return returnvalue::OK; -} diff --git a/dummies/SusDummy.h b/dummies/SusDummy.h index bdabaafc..a229bd87 100644 --- a/dummies/SusDummy.h +++ b/dummies/SusDummy.h @@ -1,27 +1,36 @@ -#pragma once +#ifndef DUMMIES_SUSDUMMY_H_ +#define DUMMIES_SUSDUMMY_H_ -#include -#include +#include -class SusDummy : public ExtendedControllerBase { +#include "mission/devices/devicedefinitions/SusDefinitions.h" + +class SusDummy : public DeviceHandlerBase { public: - SusDummy(); + static const DeviceCommandId_t SIMPLE_COMMAND = 1; + static const DeviceCommandId_t PERIODIC_REPLY = 2; - ReturnValue_t initialize() override; + static const uint8_t SIMPLE_COMMAND_DATA = 1; + static const uint8_t PERIODIC_REPLY_DATA = 2; + + SusDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + virtual ~SusDummy(); protected: - virtual ReturnValue_t handleCommandMessage(CommandMessage* message) override; - virtual void performControlOperation() override; - virtual ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) override; - virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; - - // Mode abstract functions - virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t* msToReachTheMode) override; - - private: - int iteration = 0; - float value = 0; SUS::SusDataset susSet; -}; \ No newline at end of file + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; +}; + +#endif /* DUMMIES_SUSDUMMY_H_ */ diff --git a/dummies/helpers.cpp b/dummies/helpers.cpp index c489d3a0..0ffb724f 100644 --- a/dummies/helpers.cpp +++ b/dummies/helpers.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -59,10 +60,22 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch) { new MgmLIS3MDLDummy(objects::MGM_2_LIS3_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new MgmRm3100Dummy(objects::MGM_1_RM3100_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new MgmRm3100Dummy(objects::MGM_3_RM3100_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + new GpsDummy(objects::GPS_CONTROLLER, objects::DUMMY_COM_IF, comCookieDummy); } if (cfg.addSusDummies) { - new SusDummy(); + new SusDummy(objects::SUS_0_N_LOC_XFYFZM_PT_XF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_1_N_LOC_XBYFZM_PT_XB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_2_N_LOC_XFYBZB_PT_YB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_3_N_LOC_XFYBZF_PT_YF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_6_R_LOC_XFYBZM_PT_XF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_7_R_LOC_XBYBZM_PT_XB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_8_R_LOC_XBYBZB_PT_YB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_9_R_LOC_XBYBZB_PT_YF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, objects::DUMMY_COM_IF, comCookieDummy); } if (cfg.addTempSensorDummies) { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 788ee1c4..f9ea5926 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -5,7 +5,25 @@ #include "mission/devices/torquer.h" AcsController::AcsController(object_id_t objectId) - : ExtendedControllerBase(objectId), mgmData(this) {} + : ExtendedControllerBase(objectId), + sensorProcessing(&acsParameters), + navigation(&acsParameters), + actuatorCmd(&acsParameters), + guidance(&acsParameters), + safeCtrl(&acsParameters), + detumble(&acsParameters), + ptgCtrl(&acsParameters), + detumbleCounter{0}, + mgmDataRaw(this), + mgmDataProcessed(this), + susDataRaw(this), + susDataProcessed(this), + gyrDataRaw(this), + gyrDataProcessed(this), + gpsDataProcessed(this), + mekfData(this), + ctrlValData(this), + actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -25,6 +43,21 @@ void AcsController::performControlOperation() { return; } case InternalState::READY: { + if (mode != MODE_OFF) { + switch (submode) { + case SUBMODE_SAFE: + performSafe(); + break; + case SUBMODE_DETUMBLE: + performDetumble(); + break; + case SUBMODE_PTG_TARGET: + case SUBMODE_PTG_NADIR: + case SUBMODE_PTG_INERTIAL: + performPointingCtrl(); + break; + } + } break; } default: @@ -32,76 +65,569 @@ void AcsController::performControlOperation() { } { - // TODO: Calculate actuator output - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // TODO: Insert correct values here - // dipoleSet.setDipoles(500, 500, 500, 150); - } - - { - PoolReadGuard pg(&mgmData); + PoolReadGuard pg(&mgmDataRaw); if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); } } + { + PoolReadGuard pg(&susDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copySusData(); + } + } + { + PoolReadGuard pg(&gyrDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copyGyrData(); + } + } +} + +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; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + // Give desired satellite rate and sun direction to align + double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); + // IF MEKF is working + double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; + bool magMomMtqValid = false; + if (validMekf == returnvalue::OK) { + safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), + mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), + susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), + mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + } else { + safeCtrl.safeNoMekf( + now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), + susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + } + + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + double zeroQuat[4] = {0, 0, 0, 0}; + std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); + } + } + + // Detumble check and switch + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_DETUMBLE; + detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + int32_t zeroVec[4] = {0, 0, 0, 0}; + std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration); + // } +} + +void AcsController::performDetumble() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double magMomMtq[3] = {0, 0, 0}; + detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, + mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_SAFE; + detumbleCounter = 0; + } + + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); + } + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + int32_t zeroVec[4] = {0, 0, 0, 0}; + std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } +} + +void AcsController::performPointingCtrl() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; + guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + 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}}; + guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; + ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + 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 + ptgCtrl.ptgDesaturation(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); + actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); + + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); + } + int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; + for (int i = 0; i < 4; ++i) { + cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); + // MGM Raw + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); - poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); + poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); + // MGM Processed + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); + poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); + // SUS Raw + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); + poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); + // SUS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); + poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); + // GYR Raw + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); + poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); + // GYR Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); + poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); + // GPS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); + // MEKF + localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); + localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); + poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); + // Ctrl Values + localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); + poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); + // Actuator CMD + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); + localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); + poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); return returnvalue::OK; } LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { - if (sid == mgmData.getSid()) { - return &mgmData; + switch (sid.ownerSetId) { + case acsctrl::MGM_SENSOR_DATA: + return &mgmDataRaw; + case acsctrl::MGM_PROCESSED_DATA: + return &mgmDataProcessed; + case acsctrl::SUS_SENSOR_DATA: + return &susDataRaw; + case acsctrl::SUS_PROCESSED_DATA: + return &susDataProcessed; + case acsctrl::GYR_SENSOR_DATA: + return &gyrDataRaw; + case acsctrl::GYR_PROCESSED_DATA: + return &gyrDataProcessed; + case acsctrl::GPS_PROCESSED_DATA: + return &gpsDataProcessed; + case acsctrl::MEKF_DATA: + return &mekfData; + case acsctrl::CTRL_VAL_DATA: + return &ctrlValData; + case acsctrl::ACTUATOR_CMD_DATA: + return &actuatorCmdData; + default: + return nullptr; } return nullptr; } ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { - return returnvalue::OK; + if (mode == MODE_OFF) { + if (submode == SUBMODE_NONE) { + return returnvalue::OK; + } else { + return INVALID_SUBMODE; + } + } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { + if ((submode > 6) || (submode < 2)) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; + } + } + return INVALID_MODE; } +void AcsController::modeChanged(Mode_t mode, Submode_t submode) {} + +void AcsController::announceMode(bool recursive) {} + void AcsController::copyMgmData() { + ACS::SensorValues sensorValues; { - PoolReadGuard pg(&mgm0Lis3Set); + PoolReadGuard pg(&sensorValues.mgm0Lis3Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&mgm1Rm3100Set); + PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&mgm2Lis3Set); + PoolReadGuard pg(&sensorValues.mgm2Lis3Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&mgm3Rm3100Set); + PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, + 3 + sizeof(float)); + mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&imtqMgmSet); + PoolReadGuard pg(&sensorValues.imtqMgmSet); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); - mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; + std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, + 3 * sizeof(float)); + mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); + mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; + mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid()); + } + } +} + +void AcsController::copySusData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.susSets[0]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[1]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[2]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[3]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[4]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[5]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[6]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[7]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[8]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[9]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[10]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[11]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); + } + } +} + +void AcsController::copyGyrData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.gyr0AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; + gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; + gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; + gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && + sensorValues.gyr0AdisSet.angVelocY.isValid() && + sensorValues.gyr0AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr1L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; + gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; + gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; + gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && + sensorValues.gyr1L3gSet.angVelocY.isValid() && + sensorValues.gyr1L3gSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr2AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; + gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; + gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; + gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && + sensorValues.gyr2AdisSet.angVelocY.isValid() && + sensorValues.gyr2AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr3L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; + gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; + gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; + gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && + sensorValues.gyr3L3gSet.angVelocY.isValid() && + sensorValues.gyr3L3gSet.angVelocZ.isValid()); } } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 0cf38bc7..1c4996b3 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -2,7 +2,15 @@ #define MISSION_CONTROLLER_ACSCONTROLLER_H_ #include +#include +#include "acs/ActuatorCmd.h" +#include "acs/Guidance.h" +#include "acs/Navigation.h" +#include "acs/SensorProcessing.h" +#include "acs/control/Detumble.h" +#include "acs/control/PtgCtrl.h" +#include "acs/control/SafeCtrl.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" @@ -16,10 +24,38 @@ class AcsController : public ExtendedControllerBase { AcsController(object_id_t objectId); + static const Submode_t SUBMODE_SAFE = 2; + static const Submode_t SUBMODE_DETUMBLE = 3; + static const Submode_t SUBMODE_PTG_TARGET = 4; + static const Submode_t SUBMODE_PTG_NADIR = 5; + static const Submode_t SUBMODE_PTG_INERTIAL = 6; + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; + static const Event SAFE_RATE_VIOLATION = + MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated. + + protected: + void performSafe(); + void performDetumble(); + void performPointingCtrl(); + private: + AcsParameters acsParameters; + SensorProcessing sensorProcessing; + Navigation navigation; + ActuatorCmd actuatorCmd; + Guidance guidance; + + SafeCtrl safeCtrl; + Detumble detumble; + PtgCtrl ptgCtrl; + + uint8_t detumbleCounter; + enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; + ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; @@ -30,45 +66,100 @@ class AcsController : public ExtendedControllerBase { // Mode abstract functions ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t* msToReachTheMode) override; + void modeChanged(Mode_t mode, Submode_t submode); + void announceMode(bool recursive); - // MGMs - acsctrl::MgmDataRaw mgmData; - - MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); - MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); - IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); + /* ACS Datasets */ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); - - PoolEntry mgm0PoolVec = PoolEntry(3); - PoolEntry mgm1PoolVec = PoolEntry(3); - PoolEntry mgm2PoolVec = PoolEntry(3); - PoolEntry mgm3PoolVec = PoolEntry(3); - PoolEntry imtqMgmPoolVec = PoolEntry(3); + // MGMs + acsctrl::MgmDataRaw mgmDataRaw; + PoolEntry mgm0VecRaw = PoolEntry(3); + PoolEntry mgm1VecRaw = PoolEntry(3); + PoolEntry mgm2VecRaw = PoolEntry(3); + PoolEntry mgm3VecRaw = PoolEntry(3); + PoolEntry imtqMgmVecRaw = PoolEntry(3); PoolEntry imtqCalActStatus = PoolEntry(); - void copyMgmData(); - // Sun Sensors - std::array susSets{ - SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), - SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), - SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), - SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), - SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), - SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), - SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), - SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), - SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), - SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), - SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), - SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), - }; + acsctrl::MgmDataProcessed mgmDataProcessed; + PoolEntry mgm0VecProc = PoolEntry(3); + PoolEntry mgm1VecProc = PoolEntry(3); + PoolEntry mgm2VecProc = PoolEntry(3); + PoolEntry mgm3VecProc = PoolEntry(3); + PoolEntry mgm4VecProc = PoolEntry(3); + PoolEntry mgmVecTot = PoolEntry(3); + PoolEntry mgmVecTotDer = PoolEntry(3); + PoolEntry magIgrf = PoolEntry(3); + + // SUSs + acsctrl::SusDataRaw susDataRaw; + PoolEntry sus0ValRaw = PoolEntry(6); + PoolEntry sus1ValRaw = PoolEntry(6); + PoolEntry sus2ValRaw = PoolEntry(6); + PoolEntry sus3ValRaw = PoolEntry(6); + PoolEntry sus4ValRaw = PoolEntry(6); + PoolEntry sus5ValRaw = PoolEntry(6); + PoolEntry sus6ValRaw = PoolEntry(6); + PoolEntry sus7ValRaw = PoolEntry(6); + PoolEntry sus8ValRaw = PoolEntry(6); + PoolEntry sus9ValRaw = PoolEntry(6); + PoolEntry sus10ValRaw = PoolEntry(6); + PoolEntry sus11ValRaw = PoolEntry(6); + void copySusData(); + + acsctrl::SusDataProcessed susDataProcessed; + PoolEntry sus0VecProc = PoolEntry(3); + PoolEntry sus1VecProc = PoolEntry(3); + PoolEntry sus2VecProc = PoolEntry(3); + PoolEntry sus3VecProc = PoolEntry(3); + PoolEntry sus4VecProc = PoolEntry(3); + PoolEntry sus5VecProc = PoolEntry(3); + PoolEntry sus6VecProc = PoolEntry(3); + PoolEntry sus7VecProc = PoolEntry(3); + PoolEntry sus8VecProc = PoolEntry(3); + PoolEntry sus9VecProc = PoolEntry(3); + PoolEntry sus10VecProc = PoolEntry(3); + PoolEntry sus11VecProc = PoolEntry(3); + PoolEntry susVecTot = PoolEntry(3); + PoolEntry susVecTotDer = PoolEntry(3); + PoolEntry sunIjk = PoolEntry(3); + + // GYRs + acsctrl::GyrDataRaw gyrDataRaw; + PoolEntry gyr0VecRaw = PoolEntry(3); + PoolEntry gyr1VecRaw = PoolEntry(3); + PoolEntry gyr2VecRaw = PoolEntry(3); + PoolEntry gyr3VecRaw = PoolEntry(3); + void copyGyrData(); + + acsctrl::GyrDataProcessed gyrDataProcessed; + PoolEntry gyr0VecProc = PoolEntry(3); + PoolEntry gyr1VecProc = PoolEntry(3); + PoolEntry gyr2VecProc = PoolEntry(3); + PoolEntry gyr3VecProc = PoolEntry(3); + PoolEntry gyrVecTot = PoolEntry(3); + + // GPS + acsctrl::GpsDataProcessed gpsDataProcessed; + PoolEntry gcLatitude = PoolEntry(); + PoolEntry gdLongitude = PoolEntry(); + + // MEKF + acsctrl::MekfData mekfData; + PoolEntry quatMekf = PoolEntry(4); + PoolEntry satRotRateMekf = PoolEntry(3); + + // Ctrl Values + acsctrl::CtrlValData ctrlValData; + PoolEntry tgtQuat = PoolEntry(4); + PoolEntry errQuat = PoolEntry(4); + PoolEntry errAng = PoolEntry(); + + // Actuator CMD + acsctrl::ActuatorCmdData actuatorCmdData; + PoolEntry rwTargetTorque = PoolEntry(4); + PoolEntry rwTargetSpeed = PoolEntry(4); + PoolEntry mtqTargetDipole = PoolEntry(3); // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); diff --git a/mission/controller/CMakeLists.txt b/mission/controller/CMakeLists.txt index 84ff7f9e..6177a14b 100644 --- a/mission/controller/CMakeLists.txt +++ b/mission/controller/CMakeLists.txt @@ -2,3 +2,5 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "") target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp AcsController.cpp) endif() + +add_subdirectory(acs) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp new file mode 100644 index 00000000..cc9959ab --- /dev/null +++ b/mission/controller/acs/AcsParameters.cpp @@ -0,0 +1,544 @@ +#include "AcsParameters.h" + +#include +#include + +AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) : + // parameterModuleId(parameterModuleId) {} + +AcsParameters::~AcsParameters() {} + +/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, + uint16_t startAtIndex) { + if (domainId == parameterModuleId) { + switch (parameterId >> 8) { + case 0x0: // direct members + switch (parameterId & 0xFF) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x1: // OnBoardParams + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(onBoardParams.sampleTime); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x2: // InertiaEIVE + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(inertiaEIVE.inertiaMatrix); + break; + case 0x1: + parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x3: // MgmHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + break; + case 0x6: + parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + break; + case 0x7: + parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + break; + case 0x8: + parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + break; + case 0x9: + parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + break; + case 0xA: + parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + break; + case 0xB: + parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + break; + case 0xC: + parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + break; + case 0xD: + parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + break; + case 0xE: + parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x4: // SusHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + break; + case 0x6: + parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + break; + case 0x7: + parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + break; + case 0x8: + parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + break; + case 0x9: + parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + break; + case 0xA: + parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + break; + case 0xB: + parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + break; + case 0xC: + parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + break; + case 0xD: + parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + break; + case 0xE: + parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + break; + case 0xF: + parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + break; + case 0x10: + parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + break; + case 0x11: + parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + break; + case 0x12: + parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + break; + case 0x13: + parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + break; + case 0x14: + parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + break; + case 0x15: + parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + break; + case 0x16: + parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + break; + case 0x17: + parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + break; + case 0x18: + parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + break; + case 0x19: + parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + break; + case 0x1A: + parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + break; + case 0x1B: + parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + break; + case 0x1C: + parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + break; + case 0x1D: + parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + break; + case 0x1E: + parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + break; + case 0x1F: + parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + break; + case 0x20: + parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + break; + case 0x21: + parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + break; + case 0x22: + parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + break; + case 0x23: + parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + break; + case 0x24: + parameterWrapper->set(susHandlingParameters.filterAlpha); + break; + case 0x25: + parameterWrapper->set(susHandlingParameters.sunThresh); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x5): // GyrHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x6): // RwHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(rwHandlingParameters.inertiaWheel); + break; + case 0x5: + parameterWrapper->set(rwHandlingParameters.maxTrq); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x7): // RwMatrices + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(rwMatrices.alignmentMatrix); + break; + case 0x1: + parameterWrapper->set(rwMatrices.pseudoInverse); + break; + case 0x2: + parameterWrapper->set(rwMatrices.without0); + break; + case 0x3: + parameterWrapper->set(rwMatrices.without1); + break; + case 0x4: + parameterWrapper->set(rwMatrices.without2); + break; + case 0x5: + parameterWrapper->set(rwMatrices.without3); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x8): // SafeModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); + break; + case 0x1: + parameterWrapper->set(safeModeControllerParameters.k_align_mekf); + break; + case 0x2: + parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); + break; + case 0x3: + parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); + break; + case 0x4: + parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); + break; + case 0x5: + parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + break; + case 0x6: + parameterWrapper->set(safeModeControllerParameters.satRateRef); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x9): // DetumbleCtrlParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(detumbleCtrlParameters.gainD); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // PointingModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.updtFlag); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.A_rw); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.blindRotRate); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.zetaLow); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.omLow); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x10: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x11: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x12: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x13: + parameterWrapper->set(targetModeControllerParameters.omegaEarth); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xB): // StrParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(strParameters.exclusionAngle); + break; + case 0x1: + parameterWrapper->set(strParameters.boresightAxis); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // GpsParameters + switch (parameterId & 0xFF) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xD): // GroundStationParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(groundStationParameters.latitudeGs); + break; + case 0x1: + parameterWrapper->set(groundStationParameters.longitudeGs); + break; + case 0x2: + parameterWrapper->set(groundStationParameters.altitudeGs); + break; + case 0x3: + parameterWrapper->set(groundStationParameters.earthRadiusEquat); + break; + case 0x4: + parameterWrapper->set(groundStationParameters.earthRadiusPolar); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xE): // SunModelParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(sunModelParameters.useSunModel); + break; + case 0x1: + parameterWrapper->set(sunModelParameters.domega); + break; + case 0x2: + parameterWrapper->set(sunModelParameters.omega_0); + break; + case 0x3: + parameterWrapper->set(sunModelParameters.m_0); + break; + case 0x4: + parameterWrapper->set(sunModelParameters.dm); + break; + case 0x5: + parameterWrapper->set(sunModelParameters.e); + break; + case 0x6: + parameterWrapper->set(sunModelParameters.e1); + break; + case 0x7: + parameterWrapper->set(sunModelParameters.p1); + break; + case 0x8: + parameterWrapper->set(sunModelParameters.p2); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xF): // KalmanFilterParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter); + break; + case 0x1: + parameterWrapper->set(kalmanFilterParameters.requestResetFlag); + break; + case 0x2: + parameterWrapper->set( + kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps); + break; + case 0x3: + parameterWrapper->set(kalmanFilterParameters.processNoiseOmega); + break; + case 0x4: + parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion); + break; + case 0x5: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + break; + case 0x6: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + break; + case 0x7: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + break; + case 0x8: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + break; + case 0x9: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + break; + case 0xA: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x10): // MagnetorquesParameter + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + break; + case 0x4: + parameterWrapper->set(magnetorquesParameter.inverseAlignment); + break; + case 0x5: + parameterWrapper->set(magnetorquesParameter.DipolMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x11): // DetumbleParameter + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(detumbleParameter.detumblecounter); + break; + case 0x1: + parameterWrapper->set(detumbleParameter.omegaDetumbleStart); + break; + case 0x2: + parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + default: + return INVALID_IDENTIFIER_ID; + } + return returnvalue::OK; + } else { + return INVALID_DOMAIN_ID; + } +}*/ diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h new file mode 100644 index 00000000..828e4495 --- /dev/null +++ b/mission/controller/acs/AcsParameters.h @@ -0,0 +1,905 @@ +/******************************* + * EIVE Flight Software Framework (FSFW) + * (c) 2022 IRS, Uni Stuttgart + *******************************/ + +#ifndef ACSPARAMETERS_H_ +#define ACSPARAMETERS_H_ + +#include + +#include + +typedef unsigned char uint8_t; + +class AcsParameters /*: public HasParametersIF*/ { + public: + AcsParameters(); + virtual ~AcsParameters(); + /* + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex); + */ + struct OnBoardParams { + double sampleTime = 0.1; // [s] + } onBoardParams; + + 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 inertiaMatrixInverse[3][3]; + } inertiaEIVE; + + struct MgmHandlingParameters { + float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; + float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; + float mgm2orientationMatrix[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 mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508}; + float mgm1hardIronOffset[3] = {10.95500, -8.053403, -33.36383}; + float mgm2hardIronOffset[3] = {15.72181, -26.87090, -62.19010}; + float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0}; + float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0}; + + float mgm0softIronInverse[3][3] = {{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] = {{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] = {{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 mgm13variance[3] = {1, 1, 1}; + float mgm4variance[3] = {1, 1, 1}; + + } mgmHandlingParameters; + + struct SusHandlingParameters { + float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 + 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 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 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 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 sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11 + float sus10orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM09 + float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08 + + float sus0coeffAlpha[9][10] = { + {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, + -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, + -0.000124096561459262, 0.00040790566176981}, + {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417, + 0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415, + 1.63114413539632e-05, -2.0187452317724e-05}, + {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334, + 0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995, + -0.000249094601806692, 0.000231466668650876}, + {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384, + -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05, + -9.78534772816957e-06, -4.72083745991256e-05}, + {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541, + -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463, + 0.00010150571088124, -0.000367705461590854}, + {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526, + -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05, + 6.97244631313601e-05, 2.50519145070895e-05}, + {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191, + -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928, + -1.78466217018177e-05, -0.00058976234038192}, + {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089, + -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06, + -2.73407888222758e-05, 5.45131881032866e-06}, + {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675, + 0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078, + -0.000889196131314391, -0.000509766491897672}}; + float sus0coeffBeta[9][10] = { + {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235, + -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05, + -5.52178824394723e-06, 5.73935295303589e-05}, + {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133, + 0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05, + -7.58501977656551e-05, -8.79809730730992e-05}, + {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518, + 0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341, + -0.00020861690864945}, + {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335, + -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06, + -6.00508568552101e-05, 0.000101583822589461}, + {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729, + -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05, + 0.000316835483508523, 0.000151442890664899}, + {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231, + 0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05, + -9.83662017231755e-05, 1.87078667116619e-05}, + {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016, + -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772, + -0.000285438191474895, -5.71327627917386e-05}, + {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328, + -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05, + -9.77555098179959e-05, 2.09624089449761e-05}, + {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, + -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, + 0.000141998709314758, 0.000101051304611037}}; + float sus1coeffAlpha[9][10] = { + {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, + -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, + -0.000178087038629721, 4.09208968678946e-05}, + {2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385, + 0.0117765927439368, -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956, + -9.10792250542345e-06, -2.03870119873411e-05}, + {26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656, + 0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331, + -0.00063621309529853, -0.00158363678978767}, + {-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127, + 0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05, + 6.42331081725944e-05, 7.11656918299053e-05}, + {1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834, + 0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409, + 8.52016306311741e-05, -0.000291797625433646}, + {0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958, + -0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05, + 2.84958344955681e-05, 0.000128583400693359}, + {6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284, + -0.0158258335207969, 0.0328249756354635, 0.00027113042931131, -0.000133980867173428, + -0.000357964552318811, 0.000224235061786191}, + {2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401, + -0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05, + 3.28466749016414e-05, -6.63837547601294e-06}, + {42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603, + 0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977, + -0.00058814458116749}}; + float sus1coeffBeta[9][10] = { + {-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722, + -0.04872758086642, -0.0114263851027856, 0.000141061196404012, -0.000675469545483099, + -0.000138249928781575, -0.000138871036200597}, + {10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535, + 0.00151674939001532, -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05, + -4.15760162347772e-05, -7.60797902457032e-05}, + {-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328, + 0.00379001947505376, -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05, + -5.58651971370719e-05, 0.00173185063955313}, + {12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331, + -0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05, + -5.67352645830913e-05, 0.000232270832022029}, + {4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577, + 0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05, + 0.000275311897725414, 7.58157740577916e-05}, + {3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268, + 0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05, + -2.61224817848938e-05, 6.00381132540332e-05}, + {29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232, + -0.0474780142430845, 0.0486755575785462, 0.000126295091963757, 0.000526632230895258, + -0.000259305985126003, 0.000412751148048724}, + {2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753, + -0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271, + -8.47348746872376e-05, -1.92509604512729e-06}, + {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, + 0.0887545371234514, -0.193862330062381, 0.000216532998121618, -0.00207707610520973, + 0.000552928905346826, 0.00190182163597828}}; + float sus2coeffAlpha[9][10] = { + {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, + 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, + 9.74494676304114e-05, 0.000334122888261002}, + {0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208, + 0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167, + 1.30750132315838e-05, -2.43580795300515e-05}, + {-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258, + 0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709, + -0.000586102213707195, 0.000658133691098817}, + {1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053, + 0.00202677073171519, 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05, + 3.93641210098335e-05, 4.06398431916703e-05}, + {3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784, + -0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589, + 0.000137876961532787, -0.000326798596746712}, + {6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201, + 0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06, + 7.25318569569788e-06, -2.5930447720704e-05}, + {191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846, + -0.344226468125615, 0.126267158197535, -0.00186612281541009, 0.00304415728817747, + -0.00304958575196089, 0.000457236034569107}, + {5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849, + -0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05, + 5.81988007269583e-05, 1.3723604109425e-05}, + {98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659, + 0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616, + -0.00254305206375073, -0.000466731538082995}}; + float sus2coeffBeta[9][10] = { + {41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495, + 0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05, + 0.000259197384126413}, + {4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156, + 0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05, + -5.49439160235527e-05, -8.30708110469922e-05}, + {64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773, + -0.077990648565002, 0.0613989204238974, 0.00055512113297293, -0.000347045533958329, + 0.00104059576098392, -0.000348638726253297}, + {3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723, + -0.0067958260462381, 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05, + -5.55828531601728e-05, 0.000109833374761172}, + {0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939, + 0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678, + 0.000292373398965513, 0.000183599033441813}, + {2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05, + 0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06, + -3.56732787246631e-05, -7.04534663356379e-05}, + {-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348, + 0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05, + 0.000941378089367713, 0.000353137737023192}, + {-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975, + 0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05, + -7.47076172176468e-05, -1.94516917836346e-05}, + {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, + -0.0380997421813177, -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, + 0.000168067749764069, 0.000563224178849256}}; + float sus3coeffAlpha[9][10] = { + {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, + -0.242523652239734, -0.202086838965846, -0.00138648793335223, -0.00225430176012882, + -0.00198887215340364, -0.00160678535160774}, + {6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017, + 0.00894059238779664, -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05, + -9.4312261303588e-06, -1.76647897892869e-05}, + {-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327, + 0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969, + -0.000436055839773919, -9.5869891049503e-05}, + {-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184, + 0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05, + 7.4423443938376e-05, -0.000107927900087035}, + {0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378, + -0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882, + 0.000137187356620739, -0.000320202731145004}, + {8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295, + -0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05, + 7.80079707003333e-05, -8.95904360920744e-05}, + {-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278, + 0.130143669167547, 0.0284326533865768, 0.00149831261351137, -0.0011583692969717, + 0.0010560778729661, 0.000635404380970666}, + {3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041, + -0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05, + -3.54608413548779e-05, 8.54042677832451e-06}, + {78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499, + 0.146629920899062, 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713, + -0.00133177694921411, -0.000796422644338886}}; + float sus3coeffBeta[9][10] = { + {-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957, + -0.11257100034746, 0.102120576206517, -0.000796645106694696, -0.00192211266325167, + -4.99981232866237e-05, 0.00104036677004523}, + {-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119, + 0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05, + -6.54571602919161e-05, -0.000104146832644606}, + {24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466, + -0.0526971951753399, -0.0477053831942802, 0.000613806533422364, -0.000631628059238499, + 0.00026217621127941, 0.000555307997961608}, + {0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142, + -0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05, + -7.1643235604579e-06, 0.000147876621100347}, + {-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467, + -0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935, + 0.000286018723737538, 0.000192248693306256}, + {-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05, + 0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06, + -0.00014623717850039, 8.12219846932013e-06}, + {110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034, + -0.142611297893517, -0.0141810186612052, -0.00275127095595919, 0.00160686698368569, + -0.000872029428758877, -0.000410522437887563}, + {-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852, + 0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05, + -5.87922606348196e-05, 2.56146268588382e-05}, + {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, + 0.108087933176612, -0.116711715153385, -0.00194260120960441, -0.0015752762498594, + -0.000331129410732722, 0.00125896996438418}}; + float sus4coeffAlpha[9][10] = { + {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, + -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, + -9.79031907635314e-05, -1.49299384451257e-05}, + {8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676, + 0.00879426079919981, -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05, + -3.56478452552367e-05, -1.65680920554434e-05}, + {16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303, + 0.0183175817756131, -0.008545409848878, 0.000422696533006382, -0.000268245978898508, + -0.000663188021815416, -7.51144017137827e-05}, + {0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128, + 0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05, + 5.03074365340535e-05, 8.40279146176271e-05}, + {2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513, + 0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598, + 0.000201613303652666, -0.000277796442847579}, + {6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891, + -0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05, + 0.000104552066440564, -2.78115121929346e-05}, + {-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978, + 0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844, + 0.000716490441845967, -0.000136132038635483}, + {6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045, + -0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187, + -3.3503088289589e-05, -1.03874407813931e-05}, + {102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996, + 0.216574192561683, 0.0750031215784663, -0.000147358932612646, -0.0023710703422108, + -0.00143310719642393, -0.000431914403446768}}; + float sus4coeffBeta[9][10] = { + {-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659, + -0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431, + -0.00030514398458781, -0.000211986731019738}, + {9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427, + 0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05, + -3.62215715492783e-05, -5.24384190165239e-05}, + {-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729, + 0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05, + 0.000113172463974702, 0.000271939918507175}, + {19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458, + -0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05, + -1.22808643520768e-05, 0.000440555709696048}, + {2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271, + 0.00148059212230411, 0.000557885068990542, 0.000288633931072557, 0.000172775380291659, + 0.000269738457990237, 0.000254577019084984}, + {2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743, + 0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05, + -0.000156735218010879, 3.81458400945988e-05}, + {27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388, + -0.0341141456915131, 0.0281959785297513, -0.000252996164135396, 0.000163365679366542, + -0.000380129463154642, 0.000159350154429114}, + {-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817, + -0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05, + -6.07188867796123e-05, 7.72199861279611e-06}, + {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, + -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, + 0.000698140692952438, 0.00123017528239406}}; + float sus5coeffAlpha[9][10] = { + {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, + -0.0309683799946315, -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, + -0.000183899715035788, -0.000600349486293698}, + {4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035, + 0.0108410181586673, -0.000272215427359511, 0.000124164068046579, 0.000125586897851351, + -1.24082224214974e-05, -1.63339067540159e-05}, + {63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374, + -0.0652454854214506, -0.03906716801285, 0.00166924078925478, -0.000749939315526625, + 0.000320696101132374, 0.000499934751180042}, + {-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845, + 0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05, + 8.6123353128647e-05, 2.85447259858337e-05}, + {0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534, + 0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525, + 8.66683396011167e-05, -0.000385839566009832}, + {-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706, + -0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05, + 5.73853071731283e-05, 8.56628414466758e-05}, + {22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265, + -0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216, + 0.000191004410219065, 0.000102775124022018}, + {4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749, + -0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06, + -2.78503202185463e-05, 3.99983788680736e-06}, + {-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189, + -0.0514805639475938, 0.0503660452068572, 0.00072776817295273, 0.00120288537038655, + -0.000301602375634166, -0.000477098479809266}}; + float sus5coeffBeta[9][10] = { + {16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669, + 0.0426379914736747, 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05, + 0.000645497238623369, -0.000234155227840799}, + {9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363, + 0.00581086655972212, -0.00365006277801141, 0.000180048140973223, 0.000102002650672644, + -4.10833110241736e-05, -8.7810396165556e-05}, + {-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292, + 0.0288441527062856, -0.00322823115861497, -0.00148509910480755, 0.000284265179004289, + -0.000175299737313045, -7.04175618676909e-05}, + {3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265, + -0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05, + -8.33769024439277e-05, 6.90991113575066e-05}, + {0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729, + 4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05, + 0.000318092703362044, 0.000107158633760141}, + {5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436, + 0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05, + -5.20221896473413e-05, -0.000143840492906166}, + {-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314, + 0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122, + 0.000407657552700476, -0.000458411000693613}, + {0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805, + -0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05, + -7.89362487264572e-05, -1.0951496495443e-05}, + {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, + -0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, + -0.000169538153750085, -0.000336529204350355}}; + float sus6coeffAlpha[9][10] = { + {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, + 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, + 0.00091428505258307, 0.000259857066722932}, + {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859, + 0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05, + -1.06065583714065e-05, -1.43899892666699e-05}, + {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577, + 0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412, + -4.88501228194031e-06, -0.000434861113274231}, + {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253, + 0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05, + 6.09619017310129e-05, 4.23908862836885e-05}, + {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607, + 0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257, + 0.000150795563555828, -0.000279246491927943}, + {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557, + -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05, + 9.13233708460098e-05, -0.000206717750924323}, + {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216, + -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597, + -0.000532190902882765, 9.36018171121253e-05}, + {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645, + -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05, + 1.02609175615251e-05, -9.64717658954667e-06}, + {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358, + -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05, + 0.000868163357631254, 0.00120099606910313}}; + float sus6coeffBeta[9][10] = { + {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917, + 0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05, + 0.000821479971871302, -4.5330528329465e-05}, + {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823, + 0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05, + -4.04514487800062e-05, -7.6296149087237e-05}, + {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941, + -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412, + 0.0011723869613426, 0.000122882034141316}, + {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172, + -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05, + -5.87601932564404e-05, -3.92033314627704e-05}, + {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898, + 0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264, + 0.000259925974231328, 0.000222437797823852}, + {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167, + 0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05, + -4.33518182614169e-05, 4.66993119419691e-05}, + {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108, + 0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429, + 0.000401546410974577, -0.000147563886502726}, + {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231, + 0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05, + -5.3309466243918e-05, 6.20289310356821e-06}, + {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, + 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, + -0.000186887396585446, 0.00119710704771344}}; + float sus7coeffAlpha[9][10] = { + {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, + -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, + -0.00119021101127241, -0.000460110780350978}, + {1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946, + 0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563, + -1.57820750462019e-05, -2.13840141586661e-05}, + {-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462, + 0.284142453949027, -0.198625061659164, -0.000836450164210354, 0.00174062771509636, + -0.00323746390757859, 0.00124721932086258}, + {3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927, + 0.00154601909793475, 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05, + 2.97537427220847e-05, 0.000104735911514185}, + {3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434, + -8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837, + 0.000103360882478383, -0.000296739688930329}, + {-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511, + 0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05, + -1.00328708454487e-05, 0.000138142092498215}, + {9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505, + -0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584, + 7.96466345174136e-06, 0.000638822537725177}, + {7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627, + -0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06, + -2.62114930414747e-05, -2.88713611443788e-06}, + {135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067, + 0.207680233512614, 0.12880101618361, -0.00169832076532024, -0.00192216719797732, + -0.00188763612041332, -0.00103101801961442}}; + float sus7coeffBeta[9][10] = { + {-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951, + -0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458, + -0.000555136649469199, -0.000198260004582737}, + {-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845, + -0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06, + 7.21038415209318e-06, -6.54881435118179e-05}, + {98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745, + -0.172774236139236, 0.107243391488741, 0.000421832640471043, -0.00140450884710288, + 0.00158019019392239, -0.00078512547169536}, + {4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304, + -0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05, + -3.25505031810898e-05, 5.79970895921158e-06}, + {3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324, + 0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05, + 0.000258447032558814, 9.79142983264352e-05}, + {8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996, + 0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05, + -6.887009887486e-05, -0.000266455617735054}, + {-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251, + 0.110873671161269, -0.0475247245070445, 0.000194652401328063, -0.000697307928990137, + 0.00124021816001, -0.000194213899980878}, + {2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444, + 0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05, + -6.30148122529749e-05, 2.28114298989664e-05}, + {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, + 0.331529493933124, 0.149867703984027, -0.00222279201444128, -0.00284724570619954, + -0.00298774060233964, -0.000988903783752156}}; + float sus8coeffAlpha[9][10] = { + {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, + -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, + 0.000198938007250408, 0.000102026690279006}, + {8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972, + 0.00973925295182384, -0.00116357269193765, 0.000185865842232419, 0.000103311614912702, + -2.46539447920969e-05, -2.06292928734686e-05}, + {-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634, + 0.0725172355124129, -0.13947591535243, 0.000412577580637848, 0.000434545096994917, + -0.000840043932292312, 0.00126857487044307}, + {1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056, + -0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05, + -2.98129544974679e-06, 0.000108913239345604}, + {2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518, + 0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595, + 0.000165966620658577, -0.000268394081675921}, + {15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466, + -0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05, + 8.55484605384438e-05, -0.000379241348638065}, + {-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856, + 0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145, + 0.000324089709129097, -9.33010240878062e-05}, + {0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575, + -0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05, + 1.70664456940787e-05, 6.75517901233086e-06}, + {2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389, + 0.102855060371092, -0.1203441505925, 0.000187004964420911, -0.00141720441050986, + -0.000336251285258365, 0.00145175125888695}}; + float sus8coeffBeta[9][10] = { + {28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683, + 0.0598759344275936, -0.014461432284373, 0.000128415617799076, 0.000664419128546701, + 0.000312923304130995, -0.000269026446641855}, + {7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521, + -0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05, + -3.95396925228538e-05, -7.01948519410633e-05}, + {-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532, + 0.115260678424016, -0.0109681510065038, -0.000438011443691466, 0.00097230136258486, + -0.000930875177732769, -0.000203144239955507}, + {12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169, + -0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06, + -4.75196303016323e-05, 0.000221160482364556}, + {3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587, + 0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883, + 0.000256686898599516, 0.000198982412957039}, + {9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864, + 0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05, + -3.63917701783264e-05, -0.000101376940843402}, + {9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277, + -0.0280426434406976, 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05, + -0.000516444906323815, 0.000253844418223843}, + {4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056, + 0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05, + -5.3803493814502e-05, 6.80587059728718e-06}, + {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, + 0.0898786950946473, 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, + -0.00146684876653306, -0.00102226799570239}}; + float sus9coeffAlpha[9][10] = { + {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, + 0.119985456538482, 0.142746712551924, -0.000465882328687976, 0.000606525132125852, + 0.00141667074621881, 0.00109715845894006}, + {5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426, + 0.00973291465247784, -0.00155675297510773, 0.000147099297988423, 0.000115708967219349, + -4.1462310493722e-05, -9.80097031103588e-06}, + {138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519, + -0.143071750033303, 0.237868300719915, 0.000626693630444932, -0.000579788170871402, + 0.00181740650944343, -0.00207086879728281}, + {-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038, + 0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05, + 6.3416992450033e-05, -0.000190177262510221}, + {0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05, + 0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716, + 7.71348293209208e-05, -0.000393885990364776}, + {4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428, + 0.0030051503726095, 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05, + -4.43545804544095e-05, -4.83524454851519e-05}, + {99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933, + -0.15701634159692, 0.107834711298836, -0.000885326636237814, 0.000960753844480462, + -0.00179894024848343, 0.000583066757644971}, + {2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662, + -0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05, + -4.15108111710131e-06, 1.33520085213482e-05}, + {36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879, + 0.060573568610239, 0.0182582125221054, -0.000244373383911157, -0.000271385147292484, + -0.000723799969427732, 6.76324880239196e-05}}; + float sus9coeffBeta[9][10] = { + {128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636, + 0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017, + 0.000556768406475719}, + {-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226, + -0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05, + -4.87265282482212e-05, -7.87490350444332e-05}, + {41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423, + -0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365, + 0.000938960439977633, 0.000599987111822885}, + {2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689, + -0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05, + -8.48879419798771e-05, 3.84043821333798e-05}, + {0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05, + 0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05, + 0.000327042170278218, 5.93011568264671e-05}, + {4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306, + 0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06, + -0.000131398914898614, -0.000145568992865512}, + {-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572, + 0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401, + 0.00043261858797068, -0.00021172728254561}, + {-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792, + 0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05, + -0.000107500091550635, -5.1379722947632e-07}, + {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, + -0.0625549615377418, 0.019178365782485, 0.000664877496455304, 0.000942971403881222, + 0.000190754698755098, -0.000372226659190439}}; + float sus10coeffAlpha[9][10] = { + {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, + 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, + -0.000279407497782961, 0.000726904943739837}, + {-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316, + 0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05, + -3.13338277344246e-05, 5.32977236959767e-06}, + {30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413, + -0.0553588742704929, 0.0299275582316466, 0.000167915322354466, -0.00050925078118232, + 0.000463662961330962, -0.000232919143454163}, + {2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675, + 0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05, + 6.4669137025142e-05, 0.000209727581169138}, + {-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889, + 0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244, + 0.000159157597180407, -0.00032235099420715}, + {7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199, + -0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05, + 8.85844309936609e-05, -0.000143217870916994}, + {-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416, + 0.0385056413989437, -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05, + 0.000514049116643205, 0.000112015427600697}, + {-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885, + -0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05, + -7.55982776243849e-06, 2.78417756661088e-06}, + {-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327, + -0.0193152048730234, -0.0600902798379509, 0.00031786916010672, 0.000162178988605602, + 0.000224550786416246, 0.000614337977361927}}; + float sus10coeffBeta[9][10] = { + {12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043, + 0.0220810531548995, -0.0220926906772, 0.000310052324529521, 0.000658151808869523, + -0.000288026365111098, -0.000214619731807045}, + {-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075, + 0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05, + -5.4550919751855e-05, -8.6314530565085e-05}, + {-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321, + 0.090326735447661, -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509, + -0.000956171370931993, 0.000491554402311223}, + {-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089, + -0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05, + -4.85061736834262e-05, -1.8387092782907e-06}, + {-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089, + -0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744, + 0.000278914324565192, 0.000218512838469476}, + {-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669, + 0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06, + -5.41141037812903e-05, 8.50767021818388e-06}, + {58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612, + -0.112195722921667, 0.0890150265857542, -0.000100675657768708, 0.000493488022135339, + -0.00137672908303878, 0.000518683157694955}, + {3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344, + 0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05, + -6.07871064300252e-05, 1.91822310311955e-05}, + {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, + -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, + 0.000551249824375055, 0.00062236627391337}}; + float sus11coeffAlpha[9][10] = { + {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, + -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, + 0.000166230212359982, 0.00103455037278067}, + {-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314, + 0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439, + 1.89380065823205e-05, 1.83481122973969e-07}, + {-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062, + 0.227153492753605, 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792, + -0.00247865821192621, -0.00166333309739387}, + {-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732, + 0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05, + 0.000127251739461239, 3.26943341606363e-05}, + {-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061, + 0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835, + -1.59510520000704e-05, -0.000448565104826966}, + {-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109, + -0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05, + 0.000146570207542509, 0.000201596912834554}, + {-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635, + 0.047897938410221, -0.0355687158405231, 0.000374738707508583, -0.000448453494537518, + 0.000377784972619365, -0.000276573228333836}, + {-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855, + -0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05, + 7.75581514100296e-05, -9.25934285039627e-06}, + {183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303, + 0.314006821405967, 0.200264755034566, -0.0011895153717447, -0.00253812476819896, + -0.00291324393641628, -0.00140062522117514}}; + float sus11coeffBeta[9][10] = { + {34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433, + 0.0562461093661426, 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05, + 0.00102633196865105, 0.000552974013759876}, + {7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871, + 0.00163731860604376, -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05, + -4.27874746147066e-05, -8.77418673597557e-05}, + {130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681, + -0.188837138116058, 0.346177462085361, -0.000183276946352264, -0.000702183496893294, + 0.00293145272693857, -0.00318194442670715}, + {-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05, + -0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06, + -0.000156596507890443, -0.000123254220377897}, + {3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506, + 0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07, + 0.000368585523744765, -6.3420715525635e-05}, + {14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293, + 0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05, + -2.05720000720608e-05, -0.000338584671430337}, + {-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202, + 0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561, + 0.00047849204592989, 0.000158509018296766}, + {1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305, + -0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05, + -6.74778593434431e-05, -2.81973589469059e-05}, + {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, + 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, + -0.000889232196185857, -0.00168429567131815}}; + + float filterAlpha; + float sunThresh; + } susHandlingParameters; + + struct GyrHandlingParameters { + double gyr0orientationMatrix[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 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 + 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(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)}; + enum PreferAdis { NO = 0, YES = 1 }; + uint8_t preferAdis = PreferAdis::YES; + } gyrHandlingParameters; + + struct RwHandlingParameters { + double rw0orientationMatrix[3][3]; + double rw1orientationMatrix[3][3]; + double rw2orientationMatrix[3][3]; + double rw3orientationMatrix[3][3]; + double inertiaWheel = 0.000028198; + double maxTrq = 0.0032; // 3.2 [mNm] + } rwHandlingParameters; + + struct RwMatrices { + double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, + {0.0000, -0.9205, 0.0000, 0.9205}, + {0.3907, 0.3907, 0.3907, 0.3907}}; + double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, + {0.2136, -0.3317, 1.0123}, + {-0.8672, -0.1406, 0.1778}, + {0.6426, 0.4794, 1.3603}}; + double without0[4][3]; + double without1[4][3]; + double without2[4][3]; + double without3[4][3]; + double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649}; + } rwMatrices; + + struct SafeModeControllerParameters { + double k_rate_mekf = 0.00059437; + double k_align_mekf = 0.000056875; + + double k_rate_no_mekf; + double k_align_no_mekf; + double sunMagAngleMin; + + double sunTargetDir[3] = {1, 0, 0}; // Body frame + double satRateRef[3]; // Body frame + + } safeModeControllerParameters; + + struct DetumbleCtrlParameters { + 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 zetaLow; + double om = 0.3; + double omLow; + double omMax = 1 * M_PI / 180; + double qiMin = 0.1; + double gainNullspace = 0.01; + + double desatMomentumRef[3] = {0, 0, 0}; + double deSatGainFactor = 1000; + bool desatOn = true; + + double omegaEarth = 0.000072921158553; + + } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; + + struct StrParameters { + double exclusionAngle = 20 * M_PI / 180; + // double strOrientationMatrix[3][3]; + double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // in body/geometry frame + } strParameters; + + struct 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 { + enum UseSunModel { NO = 0, YES = 3 }; + uint8_t useSunModel; + float domega = 36000.771; + float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee + float m_0 = 357.5256; // 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 e1 = 0.74508 * M_PI / 180.; + + float p1 = 6892. / 3600. * M_PI / 180.; // some parameter + float p2 = 72. / 3600. * M_PI / 180.; // some parameter + } sunModelParameters; + + struct KalmanFilterParameters { + uint8_t activateKalmanFilter; + uint8_t requestResetFlag; + double maxToleratedTimeBetweenKalmanFilterExecutionSteps; + double processNoiseOmega[3]; + double processNoiseQuaternion[4]; + + double sensorNoiseSTR = 0.1 * M_PI / 180; + double sensorNoiseSS = 8 * M_PI / 180; + double sensorNoiseMAG = 4 * M_PI / 180; + double sensorNoiseRMU[3]; + + double sensorNoiseArwRmu; // Angular Random Walk + double sensorNoiseBsRMU; // Bias Stability + } kalmanFilterParameters; + + struct MagnetorquesParameter { + double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; + double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; + double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; + double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; + double DipolMax = 0.2; // [Am^2] + + } magnetorquesParameter; + + struct DetumbleParameter { + uint8_t detumblecounter = 75; // 30 s + double omegaDetumbleStart = 2 * M_PI / 180; + double omegaDetumbleEnd = 0.4 * M_PI / 180; + double gainD = pow(10.0, -3.3); + } detumbleParameter; +}; + +#endif /* ACSPARAMETERS_H_ */ diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp new file mode 100644 index 00000000..2a0a9425 --- /dev/null +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -0,0 +1,74 @@ +/* + * ActuatorCmd.cpp + * + * Created on: 4 Aug 2022 + * Author: Robin Marquardt + */ + +#include "ActuatorCmd.h" + +#include +#include +#include +#include + +#include + +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" + +ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } + +ActuatorCmd::~ActuatorCmd() {} + +void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, + 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 + double torque[4] = {0, 0, 0, 0}; + double maxTrq = acsParameters.rwHandlingParameters.maxTrq; + VectorOperations::add(rwTrqIn, rwTrqNS, torque, 4); + + double maxValue = 0; + for (int i = 0; i < 4; i++) { // size of torque, always 4 ? + if (abs(torque[i]) > maxValue) { + maxValue = abs(torque[i]); + } + } + + if (maxValue > maxTrq) { + double scalingFactor = maxTrq / maxValue; + VectorOperations::mulScalar(torque, scalingFactor, torque, 4); + } + + // Calculating the commanded speed in RPM for every reaction wheel + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + double deltaSpeed[4] = {0, 0, 0, 0}; + double commandTime = acsParameters.onBoardParams.sampleTime, + inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; + double radToRpm = 60 / (2 * PI); // factor for conversion to RPM + // W_RW = Torque_RW / I_RW * delta t [rad/s] + double factor = commandTime / inertiaWheel * radToRpm; + VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); +} + +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) { + // Convert to Unit frame + MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, + dipolMoment, dipolMomentUnits, 3, 3, 1); + // Scaling along largest element if dipol exceeds maximum + double maxDipol = acsParameters.magnetorquesParameter.DipolMax; + double maxValue = 0; + for (int i = 0; i < 3; i++) { + if (abs(dipolMomentUnits[i]) > maxDipol) { + maxValue = abs(dipolMomentUnits[i]); + } + } + + if (maxValue > maxDipol) { + double scalingFactor = maxDipol / maxValue; + VectorOperations::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3); + } +} diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h new file mode 100644 index 00000000..5cb3ff00 --- /dev/null +++ b/mission/controller/acs/ActuatorCmd.h @@ -0,0 +1,47 @@ +/* + * ActuatorCmd.h + * + * Created on: 4 Aug 2022 + * Author: Robin Marquardt + */ + +#ifndef ACTUATORCMD_H_ +#define ACTUATORCMD_H_ + +#include "AcsParameters.h" +#include "MultiplicativeKalmanFilter.h" +#include "SensorProcessing.h" +#include "SensorValues.h" + +class ActuatorCmd { + public: + ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + virtual ~ActuatorCmd(); + + /* + * @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 + * as Input to the RWs + * @param: rwTrqIn given torque from pointing controller + * rwTrqNS Nullspace torque + * rwCmdSpeed output revolutions per minute for every + * reaction wheel + */ + void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS, + double *rwCmdSpeed); + + /* + * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * + * @param: dipolMoment given dipol moment in spacecraft frame + * dipolMomentUnits resulting dipol moment for every unit + */ + void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); + + protected: + private: + AcsParameters acsParameters; +}; + +#endif /* ACTUATORCMD_H_ */ diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt new file mode 100644 index 00000000..3c4a3475 --- /dev/null +++ b/mission/controller/acs/CMakeLists.txt @@ -0,0 +1,13 @@ +target_sources( + ${LIB_EIVE_MISSION} + PRIVATE AcsParameters.cpp + ActuatorCmd.cpp + Guidance.cpp + Igrf13Model.cpp + MultiplicativeKalmanFilter.cpp + Navigation.cpp + SensorProcessing.cpp + SensorValues.cpp + SusConverter.cpp) + +add_subdirectory(control) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp new file mode 100644 index 00000000..35a7295a --- /dev/null +++ b/mission/controller/acs/Guidance.cpp @@ -0,0 +1,317 @@ +/* + * Guidance.cpp + * + * Created on: 6 Jun 2022 + * Author: Robin Marquardt + */ + +#include "Guidance.h" + +#include +#include +#include +#include +#include + +#include "string.h" +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" + +Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } + +Guidance::~Guidance() {} + +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { + for (int i = 0; i < 3; i++) { + sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; + satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; + } + + // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); +} + +void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion to groundstation + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth + // fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; + + MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, + acsParameters.groundStationParameters.longitudeGs, + acsParameters.groundStationParameters.altitudeGs, + groundStationCart); + + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + MathOperations::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value, + sensorValues->gpsSet.longitude.value, + sensorValues->gpsSet.altitude.value, posSatE); + + // Target direction in the ECEF frame + double targetDirE[3] = {0, 0, 0}; + VectorOperations::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}}; + MathOperations::dcmEJ(now, *dcmEJ); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + double dcmEJDot[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::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, + // dcmTEST, dcmTEST, 3, 3, 3); + + MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); + MathOperations::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::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); + + // Target Direction in the body frame + double targetDirB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); + + // rotation quaternion from two vectors + double refDir[3] = {0, 0, 0}; + refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + double noramlizedTargetDirB[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); + VectorOperations::normalize(refDir, refDir, 3); + double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); + double normRefDir = VectorOperations::norm(refDir, 3); + double crossDir[3] = {0, 0, 0}; + double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); + VectorOperations::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::normalize(targetQuat, targetQuat, 4); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double velSatE[3] = {0, 0, 0}; + velSatE[0] = 0.0; // sensorValues->gps0Velocity[0]; + 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}; + // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E + MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); + double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); + MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); + VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); + + double normVelSatB = VectorOperations::norm(velSatB, 3); + double normRefSatRate = normVelSatB / normTargetDirB; + + double satRateDir[3] = {0, 0, 0}; + VectorOperations::cross(velSatB, targetDirB, satRateDir); + VectorOperations::normalize(satRateDir, satRateDir, 3); + VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate in case of star tracker blinding + //------------------------------------------------------------------------------------- + if (acsParameters.targetModeControllerParameters.avoidBlindStr) { + double sunDirJ[3] = {0, 0, 0}; + double sunDirB[3] = {0, 0, 0}; + + if (susDataProcessed->sunIjkModel.isValid()) { + std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); + MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); + } else { + std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); + } + + double exclAngle = acsParameters.strParameters.exclusionAngle, + blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + double sightAngleSun = + VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); + + if (!(strBlindAvoidFlag)) { + double critSightAngle = blindStart * exclAngle; + + if (sightAngleSun < critSightAngle) { + strBlindAvoidFlag = true; + } + + } + + else { + if (sightAngleSun < blindEnd * exclAngle) { + double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; + double blindRefRate[3] = {0, 0, 0}; + + if (sunDirB[1] < 0) { + blindRefRate[0] = normBlindRefRate; + blindRefRate[1] = 0; + blindRefRate[2] = 0; + } else { + blindRefRate[0] = -normBlindRefRate; + blindRefRate[1] = 0; + blindRefRate[2] = 0; + } + + VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); + + } else { + strBlindAvoidFlag = false; + } + } + } +} + +void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], + double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { + double quatRef[4] = {0, 0, 0, 0}; + quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; + quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; + quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; + quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; + + double satRate[3] = {0, 0, 0}; + std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); + VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); + // valid checks ? + double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]}, + {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]}, + {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, + {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}}; + + MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); + + if (quatErrorComplete[3] < 0) { + quatErrorComplete[3] *= -1; + } + + quatError[0] = quatErrorComplete[0]; + quatError[1] = quatErrorComplete[1]; + quatError[2] = quatErrorComplete[2]; + + // target flag in matlab, importance, does look like it only gives + // feedback if pointing control is under 150 arcsec ?? +} + +void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { + if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && + sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + + } + + else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() && + sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; + } + + else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) && + sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2]; + } + + else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && + !(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; + } + + else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && + (sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) { + rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2]; + } + + else { + // @note: This one takes the normal pseudoInverse of all four raction wheels valid. + // 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 + // reaction wheel the pointing control is destined to fail. + rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + } +} diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h new file mode 100644 index 00000000..4c699561 --- /dev/null +++ b/mission/controller/acs/Guidance.h @@ -0,0 +1,44 @@ +/* + * Guidance.h + * + * Created on: 6 Jun 2022 + * Author: Robin Marquardt + */ + +#ifndef GUIDANCE_H_ +#define GUIDANCE_H_ + +#include + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "SensorValues.h" + +class Guidance { + public: + Guidance(AcsParameters *acsParameters_); + virtual ~Guidance(); + + void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); + + // Function to get the target quaternion and refence rotation rate from gps position and position + // of the ground station + void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], + double refSatRate[3]); + + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and + // desired + void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], + double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + + // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid + // reation wheel maybe can be done in "commanding.h" + void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); + + private: + AcsParameters acsParameters; + bool strBlindAvoidFlag = false; +}; + +#endif /* ACS_GUIDANCE_H_ */ diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp new file mode 100644 index 00000000..fcd95b68 --- /dev/null +++ b/mission/controller/acs/Igrf13Model.cpp @@ -0,0 +1,125 @@ +/* + * Igrf13Model.cpp + * + * Created on: 10 Mar 2022 + * Author: Robin Marquardt + */ + +#include "Igrf13Model.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "util/MathOperations.h" + +Igrf13Model::Igrf13Model() {} +Igrf13Model::~Igrf13Model() {} + +void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, + const double altitude, timeval timeOfMagMeasurement, + double* magFieldModelInertial) { + double phi = longitude, theta = gcLatitude; // geocentric + /* Here is the co-latitude needed*/ + theta -= 90 * Math::PI / 180; + theta *= (-1); + + double rE = 6371200.0; // radius earth [m] + /* Predefine recursive associated Legendre polynomials */ + double P11 = 1; + double P10 = P11; // P10 = P(n-1,m-0) + double dP11 = 0; // derivative + double dP10 = dP11; // derivative + + double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0; + + for (int m = 0; m <= igrfOrder; m++) { + for (int n = 1; n <= igrfOrder; n++) { + if (m <= n) { + /* Calculation of Legendre Polynoms (normalised) */ + if (n == m) { + P2 = sin(theta) * P11; + dP2 = sin(theta) * dP11 - cos(theta) * P11; + P11 = P2; + P10 = P11; + P20 = 0; + dP11 = dP2; + dP10 = dP11; + dP20 = 0; + } else if (n == 1) { + P2 = cos(theta) * P10; + dP2 = cos(theta) * dP10 - sin(theta) * P10; + P20 = P10; + P10 = P2; + dP20 = dP10; + dP10 = dP2; + } else { + K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3)); + P2 = cos(theta) * P10 - K * P20; + dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20; + P20 = P10; + P10 = P2; + dP20 = dP10; + dP10 = dP2; + } + /* gradient of scalar potential towards radius */ + magFieldModel[0] += + pow(rE / (altitude + rE), (n + 2)) * (n + 1) * + ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); + /* gradient of scalar potential towards phi */ + magFieldModel[1] += + pow(rE / (altitude + rE), (n + 2)) * + ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); + /* gradient of scalar potential towards theta */ + magFieldModel[2] += + pow(rE / (altitude + rE), (n + 2)) * + ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); + } + } + } + + magFieldModel[1] *= -1; + magFieldModel[2] *= (-1 / sin(theta)); + + /* Next step: transform into inertial KOS (IJK)*/ + // Julean Centuries + double JD2000Floor = 0; + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + JD2000Floor = floor(JD2000); + 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 lst = gst + longitude; // local sidereal time [rad] + + magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + + magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); + magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + + magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); + magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); + + double normVecMagFieldInert[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); +} + +void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { + double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double days = ceil(JD2000 - JD2000Igrf); + for (int i = 0; i <= igrfOrder; i++) { + for (int j = 0; j <= (igrfOrder - 1); j++) { + updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365); + updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365); + } + } +} diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h new file mode 100644 index 00000000..aa7e8b73 --- /dev/null +++ b/mission/controller/acs/Igrf13Model.h @@ -0,0 +1,122 @@ +/* + * Igrf13Model.h + * + * Created on: 10 Mar 2022 + * Author: Robin Marquardt + * Description: Calculates the magnetic field vector of earth with the IGRF Model. + * Sources: https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html + * https://doi.org/10.1186/s40623-020-01288-x + * J. Davis, Mathematical Modeling of Earth's Magnetic Field, TN, 2004 + * + * [Conversion of ENU (geocentric) to IJK: Skript Bahnmechanik für Raumfahrzeuge, + * Prof. Dr.-Ing. Stefanos Fasoulas / Dr.-Ing. Frank Zimmermann] + * + */ + +#ifndef IGRF13MODEL_H_ +#define IGRF13MODEL_H_ + +#include +#include +#include +#include + +#include + +// Output should be transformed to [T] instead of [nT] +// Updating Coefficients has to be implemented yet. Question, updating everyday ? +class Igrf13Model /*:public HasParametersIF*/ { + public: + Igrf13Model(); + virtual ~Igrf13Model(); + + // Main Function + void magFieldComp(const double longitude, const double gcLatitude, const double altitude, + timeval timeOfMagMeasurement, double* magFieldModelInertial); + // Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is + // oriented from the satellite to earth COM Difference up to 25 km, which is 5 % of the total + // flight altitude + /* Inputs: + * - longitude: geocentric longitude [rad] + * - latitude: geocentric latitude [rad] + * - altitude: [m] + * - timeOfMagMeasurement: time of actual measurement [s] + * + * Outputs: + * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ + + // Coefficient wary over year, could be updated sometimes. + void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) + double magFieldModel[3]; + + private: + const 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}, + {-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, 0.0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, + {0.0, 0.0, 0.0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3}, + {0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8}, + {0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -11.9, -2.4, -0.6, -0.5, 0.4}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.8, 0.2, 0.1, 0.1}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, -1.1, 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 + + const 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}, + {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, 0.0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4}, + {0.0, 0.0, 0.0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4}, + {0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3}, + {0.0, 0.0, 0.0, 0.0, 0.0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6, -0.1, -3.0, 0.2, 0.5}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.8, -2.0, -0.9, 0.5}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.6, 0.0, -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 + + const 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}, + {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, 0.0, -12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, -5.1, 1.3, -1.4, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.9, 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.9, -0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, -0.1, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 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] = { + {0.0, 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}, + {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, 0.0, 0.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, -5.0, 3.0, 0.8, -0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.3, 0.0, -1.1, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 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 updatedG[14][13]; + double updatedH[14][13]; + static const int igrfOrder = 13; // degree of truncation +}; + +#endif /* IGRF13MODEL_H_ */ diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp new file mode 100644 index 00000000..199440d5 --- /dev/null +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -0,0 +1,1186 @@ +/* + * MultiplicativeKalmanFilter.cpp + * + * Created on: 4 Feb 2022 + * Author: rooob + */ + +#include "MultiplicativeKalmanFilter.h" + +#include +#include +#include +#include +#include +#include + +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" + +/*Initialisation of values for parameters in constructor*/ +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) + : initialQuaternion{0.5, 0.5, 0.5, 0.5}, + 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}} { + loadAcsParameters(acsParameters_); +} + +MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} + +void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { + inertiaEIVE = &(acsParameters_->inertiaEIVE); + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ +} + +void MultiplicativeKalmanFilter::reset() {} + +void MultiplicativeKalmanFilter::init( + const double *magneticField_, const bool validMagField_, const double *sunDir_, + const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, + const bool validMagModel) { // valids for "model measurements"? + // check for valid mag/sun + if (validMagField_ && validSS && validSSModel && validMagModel) { + validInit = true; + // AcsParameters mekfEstParams; + // loadAcsParameters(&mekfEstParams); + // QUEST ALGO ----------------------------------------------------------------------- + double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; + sigmaSun = kalmanFilterParameters->sensorNoiseSS; + sigmaMag = kalmanFilterParameters->sensorNoiseMAG; + + sigmaGyro = 0.1 * 3.141 / 180; // acs parameters + + double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, + normSunJ[3] = {0, 0, 0}; + VectorOperations::normalize(magneticField_, normMagB, 3); // b2 + VectorOperations::normalize(sunDir_, normSunB, 3); // b1 + VectorOperations::normalize(magFieldJ, normMagJ, 3); // r2 + VectorOperations::normalize(sunDirJ, normSunJ, 3); // r1 + + double thirdAxis_B[3] = {0, 0, 0}, thirdAxis_J[3] = {0, 0, 0}; + VectorOperations::cross(normSunJ, normMagJ, thirdAxis_J); + VectorOperations::cross(normSunB, normMagB, thirdAxis_B); + VectorOperations::normalize(thirdAxis_J, thirdAxis_J, 3); // rx + VectorOperations::normalize(thirdAxis_B, thirdAxis_B, 3); // bx + + double weigthSun = 1 / (sigmaSun * sigmaSun); // a1 + double weigthMag = 1 / (sigmaMag * sigmaMag); // a2 + + double a[3] = {0, 0, 0}, b[3] = {0, 0, 0}; + VectorOperations::mulScalar(normSunB, weigthSun, a, 3); // a + VectorOperations::mulScalar(normMagB, weigthMag, b, 3); // b + + double thirdAxisCross[3] = {0, 0, 0}, weightACross[3] = {0, 0, 0}, weightBCross[3] = {0, 0, 0}, + weigthCrossSum[3] = {0, 0, 0}; + VectorOperations::cross(thirdAxis_B, thirdAxis_J, + thirdAxisCross); // cross(bx,rx) + VectorOperations::cross(a, normSunJ, weightACross); + VectorOperations::cross(b, normMagJ, weightBCross); + VectorOperations::add(weightACross, weightBCross, weigthCrossSum); + + double thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}; + VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); + VectorOperations::add(weightACross, weightBCross, sum2); + + double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0; + alpha = (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)) * + (VectorOperations::dot(a, normSunJ) + + VectorOperations::dot(b, normMagJ)) + + VectorOperations::dot(thirdAxisCross, weigthCrossSum); + beta = VectorOperations::dot(thirdAxisSum, sum2); + gamma = sqrt(alpha * alpha + beta * beta); + constPlus = 1 / (2 * sqrt(gamma * (gamma + alpha) * + (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)))); + constMin = 1 / (2 * sqrt(gamma * (gamma - alpha) * + (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)))); + + /*Quaternion Computation*/ + double quat[3] = {0, 0, 0}, var1[3] = {0, 0, 0}, var2[3] = {0, 0, 0}; + if (alpha >= 0) { + VectorOperations::mulScalar(thirdAxisCross, gamma + alpha, var1, 3); + VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); + VectorOperations::mulScalar(var2, beta, var2, 3); + VectorOperations::add(var1, var2, quat); + + for (int i = 0; i < 3; i++) { + initialQuaternion[i] = quat[i]; + } + initialQuaternion[3] = + (gamma + alpha) * (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)); + VectorOperations::mulScalar(initialQuaternion, constPlus, initialQuaternion, 4); + } else { + double diffGammaAlpha = gamma - alpha; + VectorOperations::mulScalar(thirdAxisCross, beta, var1, 3); + VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); + VectorOperations::mulScalar(var2, diffGammaAlpha, var2, 3); + VectorOperations::add(var1, var2, quat); + for (int i = 0; i < 3; i++) { + initialQuaternion[i] = quat[i]; + } + initialQuaternion[3] = beta * (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)); + VectorOperations::mulScalar(initialQuaternion, constMin, initialQuaternion, 4); + } + propagatedQuaternion[0] = initialQuaternion[0]; + propagatedQuaternion[1] = initialQuaternion[1]; + propagatedQuaternion[2] = initialQuaternion[2]; + propagatedQuaternion[3] = initialQuaternion[3]; + + /*Initial Covariance Matrix------------------------------------ */ + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0}, + magEstB[3] = {0, 0, 0}; + QuaternionOperations::toDcm(initialQuaternion, dcmBJ); + MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1); + + double matrixSun[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}}, + matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + /* vector*transpose(vector)*/ + MatrixOperations::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3); + MatrixOperations::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); + MatrixOperations::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3); + MatrixOperations::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3); + + double partA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, partB = 0; + MatrixOperations::multiplyScalar(*matrixSun, weigthSun * weigthSun, *matrixSun, 3, 3); + MatrixOperations::multiplyScalar(*matrixMag, weigthMag * weigthMag, *matrixMag, 3, 3); + MatrixOperations::add(*matrixSunMag, *matrixMagSun, *partA, 3, 3); + double factor = weigthMag * weigthSun * VectorOperations::dot(sunEstB, magEstB); + MatrixOperations::multiplyScalar(*partA, factor, *partA, 3, 3); + MatrixOperations::add(*partA, *matrixSun, *partA, 3, 3); + MatrixOperations::add(*partA, *matrixMag, *partA, 3, 3); + + double crossSunMag[3] = {0, 0, 0}; + double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + VectorOperations::cross(sunEstB, magEstB, crossSunMag); + partB = 1 / (weigthMag * weigthSun * pow(VectorOperations::norm(crossSunMag, 3), 2)); + MatrixOperations::multiplyScalar(*partA, partB, *partA, 3, 3); + MatrixOperations::add(*matrixSunMag, *identityMatrix3, *partA, 3, 3); + factor = 1 / (weigthMag + weigthMag); + + double questCovMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiplyScalar(*partA, factor, *questCovMatrix, 3, 3); + double initGyroCov[3][3] = { + {pow(sigmaGyro, 2), 0, 0}, {0, pow(sigmaGyro, 2), 0}, {0, 0, pow(sigmaGyro, 2)}}; + initialCovarianceMatrix[0][0] = questCovMatrix[0][0]; + initialCovarianceMatrix[0][1] = questCovMatrix[0][1]; + initialCovarianceMatrix[0][2] = questCovMatrix[0][2]; + initialCovarianceMatrix[0][3] = 0; + initialCovarianceMatrix[0][4] = 0; + initialCovarianceMatrix[0][5] = 0; + initialCovarianceMatrix[1][0] = questCovMatrix[1][0]; + initialCovarianceMatrix[1][1] = questCovMatrix[1][1]; + initialCovarianceMatrix[1][2] = questCovMatrix[1][2]; + initialCovarianceMatrix[1][3] = 0; + initialCovarianceMatrix[1][4] = 0; + initialCovarianceMatrix[1][5] = 0; + initialCovarianceMatrix[2][0] = questCovMatrix[2][0]; + initialCovarianceMatrix[2][1] = questCovMatrix[2][1]; + initialCovarianceMatrix[2][2] = questCovMatrix[2][2]; + initialCovarianceMatrix[2][3] = 0; + initialCovarianceMatrix[2][4] = 0; + initialCovarianceMatrix[2][5] = 0; + initialCovarianceMatrix[3][0] = 0; + initialCovarianceMatrix[3][1] = 0; + initialCovarianceMatrix[3][2] = 0; + initialCovarianceMatrix[3][3] = initGyroCov[0][0]; + initialCovarianceMatrix[3][4] = initGyroCov[0][1]; + initialCovarianceMatrix[3][5] = initGyroCov[0][2]; + initialCovarianceMatrix[4][0] = 0; + initialCovarianceMatrix[4][1] = 0; + initialCovarianceMatrix[4][2] = 0; + initialCovarianceMatrix[4][3] = initGyroCov[1][0]; + initialCovarianceMatrix[4][4] = initGyroCov[1][1]; + initialCovarianceMatrix[4][5] = initGyroCov[1][2]; + initialCovarianceMatrix[5][0] = 0; + initialCovarianceMatrix[5][1] = 0; + initialCovarianceMatrix[5][2] = 0; + initialCovarianceMatrix[5][3] = initGyroCov[2][0]; + initialCovarianceMatrix[5][4] = initGyroCov[2][1]; + 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 { + // no initialisation possible, no valid measurements + validInit = false; + } +} + +// --------------- MEKF DISCRETE TIME STEP ------------------------------- +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, + const bool validGYRs_, const double *magneticField_, const bool validMagField_, + const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) { + // Check for GYR Measurements + // AcsParameters mekfEstParams; + // loadAcsParameters(&mekfEstParams); + int MDF = 0; // Matrix Dimension Factor + if (!validGYRs_) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } + validMekf = false; + return KALMAN_NO_GYR_MEAS; + } + // Check for Model Calculations + else if (!validSSModel || !validMagModel) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } + validMekf = false; + return KALMAN_NO_MODEL; + } + // Check Measurements available from SS, MAG, STR + if (validSS && validMagField_ && validSTR_) { + sensorsAvail = 1; + MDF = 9; + } else if (validSS && validMagField_ && !validSTR_) { + sensorsAvail = 2; + MDF = 6; + } else if (validSS && !validMagField_ && validSTR_) { + sensorsAvail = 3; + MDF = 6; + } else if (!validSS && validMagField_ && validSTR_) { + sensorsAvail = 4; + MDF = 6; + } else if (validSS && !validMagField_ && !(validSTR_)) { + sensorsAvail = 5; + MDF = 3; + } else if (!validSS && validMagField_ && !validSTR_) { + sensorsAvail = 6; + MDF = 3; + } else if (!validSS && !validMagField_ && validSTR_) { + sensorsAvail = 7; + MDF = 3; + } else { + sensorsAvail = 8; // no measurements + validMekf = false; + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } + return returnvalue::FAILED; + } + + // If we are here, MEKF will perform + double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; + sigmaSun = kalmanFilterParameters->sensorNoiseSS; + sigmaMag = kalmanFilterParameters->sensorNoiseMAG; + sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + + double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, + normSunJ[3] = {0, 0, 0}; + VectorOperations::normalize(magneticField_, normMagB, 3); // b2 + VectorOperations::normalize(sunDir_, normSunB, 3); // b1 + VectorOperations::normalize(magFieldJ, normMagJ, 3); // r2 + VectorOperations::normalize(sunDirJ, normSunJ, 3); // r1 + + /*-------GAIN - UPDATE - STEP------*/ + double covMatrix11[3][3] = { + {pow(sigmaSun, 2), 0, 0}, {0, pow(sigmaSun, 2), 0}, {0, 0, pow(sigmaSun, 2)}}; + double covMatrix22[3][3] = { + {pow(sigmaMag, 2), 0, 0}, {0, pow(sigmaMag, 2), 0}, {0, 0, pow(sigmaMag, 2)}}; + double covMatrix33[3][3] = { + {pow(sigmaStr, 2), 0, 0}, {0, pow(sigmaStr, 2), 0}, {0, 0, pow(sigmaStr, 2)}}; + + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0}, + magEstB[3] = {0, 0, 0}; + QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ); + MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1); + double quatEstErr[3] = {0, 0, 0}; + + // Measurement Sensitivity Matrix + double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss + double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag + double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str + MathOperations::skewMatrix(sunEstB, *measSensMatrix11); + MathOperations::skewMatrix(magEstB, *measSensMatrix22); + + double measVecQuat[3] = {0, 0, 0}; + if (validSTR_) { + double quatError[4] = {0, 0, 0, 0}; + double invPropagatedQuat[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); + QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError); + for (int i = 0; i < 3; i++) { + measVecQuat[i] = 2 * quatError[i] / quatError[3]; + } + } + // Adjust matrices based on valid measurements + double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], measEstVec[MDF]; + if (sensorsAvail == 1) { // All measurements valid + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix22[0][0]; + measSensMatrix[3][1] = measSensMatrix22[0][1]; + measSensMatrix[3][2] = measSensMatrix22[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix22[1][0]; + measSensMatrix[4][1] = measSensMatrix22[1][1]; + measSensMatrix[4][2] = measSensMatrix22[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix22[2][0]; + measSensMatrix[5][1] = measSensMatrix22[2][1]; + measSensMatrix[5][2] = measSensMatrix22[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + measSensMatrix[6][0] = measSensMatrix33[0][0]; + measSensMatrix[6][1] = measSensMatrix33[0][1]; + measSensMatrix[6][2] = measSensMatrix33[0][2]; + measSensMatrix[6][3] = 0; + measSensMatrix[6][4] = 0; + measSensMatrix[6][5] = 0; + measSensMatrix[7][0] = measSensMatrix33[1][0]; + measSensMatrix[7][1] = measSensMatrix33[1][1]; + measSensMatrix[7][2] = measSensMatrix33[1][2]; + measSensMatrix[7][3] = 0; + measSensMatrix[7][4] = 0; + measSensMatrix[7][5] = 0; + measSensMatrix[8][0] = measSensMatrix33[2][0]; + measSensMatrix[8][1] = measSensMatrix33[2][1]; + measSensMatrix[8][2] = measSensMatrix33[2][2]; + measSensMatrix[8][3] = 0; + measSensMatrix[8][4] = 0; + measSensMatrix[8][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[0][6] = 0; + measCovMatrix[0][7] = 0; + measCovMatrix[0][8] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[1][6] = 0; + measCovMatrix[1][7] = 0; + measCovMatrix[1][8] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[2][6] = 0; + measCovMatrix[2][7] = 0; + measCovMatrix[2][8] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix22[0][0]; + measCovMatrix[3][4] = covMatrix22[0][1]; + measCovMatrix[3][5] = covMatrix22[0][2]; + measCovMatrix[3][6] = 0; + measCovMatrix[3][7] = 0; + measCovMatrix[3][8] = 0; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix22[1][0]; + measCovMatrix[4][4] = covMatrix22[1][1]; + measCovMatrix[4][5] = covMatrix22[1][2]; + measCovMatrix[4][6] = 0; + measCovMatrix[4][7] = 0; + measCovMatrix[4][8] = 0; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix22[2][0]; + measCovMatrix[5][4] = covMatrix22[2][1]; + measCovMatrix[5][5] = covMatrix22[2][2]; + measCovMatrix[5][6] = 0; + measCovMatrix[5][7] = 0; + measCovMatrix[5][8] = 0; + measCovMatrix[6][0] = 0; + measCovMatrix[6][1] = 0; + measCovMatrix[6][2] = 0; + measCovMatrix[6][3] = 0; + measCovMatrix[6][4] = 0; + measCovMatrix[6][5] = 0; + measCovMatrix[6][6] = covMatrix33[0][0]; + measCovMatrix[6][7] = covMatrix33[0][1]; + measCovMatrix[6][8] = covMatrix33[0][2]; + measCovMatrix[7][0] = 0; + measCovMatrix[7][1] = 0; + measCovMatrix[7][2] = 0; + measCovMatrix[7][3] = 0; + measCovMatrix[7][4] = 0; + measCovMatrix[7][5] = 0; + measCovMatrix[7][6] = covMatrix33[1][0]; + measCovMatrix[7][7] = covMatrix33[1][1]; + measCovMatrix[7][8] = covMatrix33[1][2]; + measCovMatrix[8][0] = 0; + measCovMatrix[8][1] = 0; + measCovMatrix[8][2] = 0; + measCovMatrix[8][3] = 0; + measCovMatrix[8][4] = 0; + measCovMatrix[8][5] = 0; + measCovMatrix[8][6] = covMatrix33[2][0]; + measCovMatrix[8][7] = covMatrix33[2][1]; + measCovMatrix[8][8] = covMatrix33[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = normMagB[0]; + measVec[4] = normMagB[1]; + measVec[5] = normMagB[2]; + measVec[6] = measVecQuat[0]; + measVec[7] = measVecQuat[1]; + measVec[8] = measVecQuat[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = magEstB[0]; + measEstVec[4] = magEstB[1]; + measEstVec[5] = magEstB[2]; + measEstVec[6] = quatEstErr[0]; + measEstVec[7] = quatEstErr[1]; + measEstVec[8] = quatEstErr[2]; + + } else if (sensorsAvail == 2) { // ss / mag valid + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix22[0][0]; + measSensMatrix[3][1] = measSensMatrix22[0][1]; + measSensMatrix[3][2] = measSensMatrix22[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix22[1][0]; + measSensMatrix[4][1] = measSensMatrix22[1][1]; + measSensMatrix[4][2] = measSensMatrix22[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix22[2][0]; + measSensMatrix[5][1] = measSensMatrix22[2][1]; + measSensMatrix[5][2] = measSensMatrix22[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix22[0][0]; + measCovMatrix[3][4] = covMatrix22[0][1]; + measCovMatrix[3][5] = covMatrix22[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix22[1][0]; + measCovMatrix[4][4] = covMatrix22[1][1]; + measCovMatrix[4][5] = covMatrix22[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix22[2][0]; + measCovMatrix[5][4] = covMatrix22[2][1]; + measCovMatrix[5][5] = covMatrix22[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = normMagB[0]; + measVec[4] = normMagB[1]; + measVec[5] = normMagB[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = magEstB[0]; + measEstVec[4] = magEstB[1]; + measEstVec[5] = magEstB[2]; + + } else if (sensorsAvail == 3) { // ss / str valid + + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix33[0][0]; + measSensMatrix[3][1] = measSensMatrix33[0][1]; + measSensMatrix[3][2] = measSensMatrix33[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix33[1][0]; + measSensMatrix[4][1] = measSensMatrix33[1][1]; + measSensMatrix[4][2] = measSensMatrix33[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix33[2][0]; + measSensMatrix[5][1] = measSensMatrix33[2][1]; + measSensMatrix[5][2] = measSensMatrix33[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix33[0][0]; + measCovMatrix[3][4] = covMatrix33[0][1]; + measCovMatrix[3][5] = covMatrix33[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix33[1][0]; + measCovMatrix[4][4] = covMatrix33[1][1]; + measCovMatrix[4][5] = covMatrix33[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix33[2][0]; + measCovMatrix[5][4] = covMatrix33[2][1]; + measCovMatrix[5][5] = covMatrix33[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = measVecQuat[0]; + measVec[4] = measVecQuat[1]; + measVec[5] = measVecQuat[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = quatEstErr[0]; + measEstVec[4] = quatEstErr[1]; + measEstVec[5] = quatEstErr[2]; + + } else if (sensorsAvail == 4) { // mag / str avail + + measSensMatrix[0][0] = measSensMatrix22[0][0]; + measSensMatrix[0][1] = measSensMatrix22[0][1]; + measSensMatrix[0][2] = measSensMatrix22[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix22[1][0]; + measSensMatrix[1][1] = measSensMatrix22[1][1]; + measSensMatrix[1][2] = measSensMatrix22[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix22[2][0]; + measSensMatrix[2][1] = measSensMatrix22[2][1]; + measSensMatrix[2][2] = measSensMatrix22[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix33[0][0]; + measSensMatrix[3][1] = measSensMatrix33[0][1]; + measSensMatrix[3][2] = measSensMatrix33[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix33[1][0]; + measSensMatrix[4][1] = measSensMatrix33[1][1]; + measSensMatrix[4][2] = measSensMatrix33[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix33[2][0]; + measSensMatrix[5][1] = measSensMatrix33[2][1]; + measSensMatrix[5][2] = measSensMatrix33[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix22[0][0]; + measCovMatrix[0][1] = covMatrix22[0][1]; + measCovMatrix[0][2] = covMatrix22[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix22[1][0]; + measCovMatrix[1][1] = covMatrix22[1][1]; + measCovMatrix[1][2] = covMatrix22[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix22[2][0]; + measCovMatrix[2][1] = covMatrix22[2][1]; + measCovMatrix[2][2] = covMatrix22[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix33[0][0]; + measCovMatrix[3][4] = covMatrix33[0][1]; + measCovMatrix[3][5] = covMatrix33[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix33[1][0]; + measCovMatrix[4][4] = covMatrix33[1][1]; + measCovMatrix[4][5] = covMatrix33[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix33[2][0]; + measCovMatrix[5][4] = covMatrix33[2][1]; + measCovMatrix[5][5] = covMatrix33[2][2]; + + measVec[0] = normMagB[0]; + measVec[1] = normMagB[1]; + measVec[2] = normMagB[2]; + measVec[3] = measVecQuat[0]; + measVec[4] = measVecQuat[1]; + measVec[5] = measVecQuat[2]; + + measEstVec[0] = magEstB[0]; + measEstVec[1] = magEstB[1]; + measEstVec[2] = magEstB[2]; + measEstVec[3] = quatEstErr[0]; + measEstVec[4] = quatEstErr[1]; + measEstVec[5] = quatEstErr[2]; + + } else if (sensorsAvail == 5) { // only ss + + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + + } else if (sensorsAvail == 6) { // only mag + + measSensMatrix[0][0] = measSensMatrix22[0][0]; + measSensMatrix[0][1] = measSensMatrix22[0][1]; + measSensMatrix[0][2] = measSensMatrix22[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix22[1][0]; + measSensMatrix[1][1] = measSensMatrix22[1][1]; + measSensMatrix[1][2] = measSensMatrix22[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix22[2][0]; + measSensMatrix[2][1] = measSensMatrix22[2][1]; + measSensMatrix[2][2] = measSensMatrix22[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix22[0][0]; + measCovMatrix[0][1] = covMatrix22[0][1]; + measCovMatrix[0][2] = covMatrix22[0][2]; + measCovMatrix[1][0] = covMatrix22[1][0]; + measCovMatrix[1][1] = covMatrix22[1][1]; + measCovMatrix[1][2] = covMatrix22[1][2]; + measCovMatrix[2][0] = covMatrix22[2][0]; + measCovMatrix[2][1] = covMatrix22[2][1]; + measCovMatrix[2][2] = covMatrix22[2][2]; + + measVec[0] = normMagB[0]; + measVec[1] = normMagB[1]; + measVec[2] = normMagB[2]; + + measEstVec[0] = magEstB[0]; + measEstVec[1] = magEstB[1]; + measEstVec[2] = magEstB[2]; + + } else if (sensorsAvail == 7) { // only str + + measSensMatrix[0][0] = measSensMatrix33[0][0]; + measSensMatrix[0][1] = measSensMatrix33[0][1]; + measSensMatrix[0][2] = measSensMatrix33[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix33[1][0]; + measSensMatrix[1][1] = measSensMatrix33[1][1]; + measSensMatrix[1][2] = measSensMatrix33[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix33[2][0]; + measSensMatrix[2][1] = measSensMatrix33[2][1]; + measSensMatrix[2][2] = measSensMatrix33[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix33[0][0]; + measCovMatrix[0][1] = covMatrix33[0][1]; + measCovMatrix[0][2] = covMatrix33[0][2]; + measCovMatrix[1][0] = covMatrix33[1][0]; + measCovMatrix[1][1] = covMatrix33[1][1]; + measCovMatrix[1][2] = covMatrix33[1][2]; + measCovMatrix[2][0] = covMatrix33[2][0]; + measCovMatrix[2][1] = covMatrix33[2][1]; + measCovMatrix[2][2] = covMatrix33[2][2]; + + measVec[0] = measVecQuat[0]; + measVec[1] = measVecQuat[1]; + measVec[2] = measVecQuat[2]; + + measEstVec[0] = quatEstErr[0]; + measEstVec[1] = quatEstErr[1]; + measEstVec[2] = quatEstErr[2]; + } + // Kalman Gain: [K = P * H' / (H * P * H' + R)] + double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}}; + + double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], residualCov1[6][MDF]; + // H * P * H' + MatrixOperations::transpose(*measSensMatrix, *measSensMatrixTrans, 6, MDF); + MatrixOperations::multiply(*initialCovarianceMatrix, *measSensMatrixTrans, *residualCov1, + 6, 6, MDF); + MatrixOperations::multiply(*measSensMatrix, *residualCov1, *residualCov, MDF, 6, MDF); + // negative values, restrictions ? + // (H * P * H' + R) + MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); + // <> + double invResidualCov[MDF][MDF] = {{0}}; + int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); + if (inversionFailed) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } + validMekf = false; + return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + } + + // [K = P * H' / (H * P * H' + R)] + MatrixOperations::multiply(*measSensMatrixTrans, *invResidualCov, *kalmanGain1, 6, MDF, + MDF); + MatrixOperations::multiply(*initialCovarianceMatrix, *kalmanGain1, *kalmanGain, 6, 6, + MDF); + + /* ------- UPDATE -STEP ---------*/ + + // Update Covariance Matrix: P_plus = (I-K*H)*P_min + double covMatPlus[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}}; + double identityMatrix[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}}; + double updateCov1[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}}; + MatrixOperations::multiply(*kalmanGain, *measSensMatrix, *updateCov1, 6, MDF, MDF); + MatrixOperations::subtract(*identityMatrix, *updateCov1, *updateCov1, 6, 6); + MatrixOperations::multiply(*updateCov1, *initialCovarianceMatrix, *covMatPlus, 6, 6, 6); + + // Error State Vector + double errStateVec[6] = {0, 0, 0, 0, 0, 0}; + double errStateVec1[MDF] = {0}; + VectorOperations::subtract(measVec, measEstVec, errStateVec1, MDF); + MatrixOperations::multiply(*kalmanGain, errStateVec1, errStateVec, 6, MDF, 1); + + double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; + double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; + + // State Vector Elements + double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::skewMatrix(propagatedQuaternion, *xi2); + double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); + MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); + double xi[4][3] = { + {xi1[0][0], xi1[0][1], xi1[0][2]}, + {xi1[1][0], xi1[1][1], xi1[1][2]}, + {xi1[2][0], xi1[2][1], xi1[2][2]}, + {-propagatedQuaternion[0], -propagatedQuaternion[1], -propagatedQuaternion[2]}}; + + double errQuatTerm[4] = {0, 0, 0, 0}; + MatrixOperations::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1); + VectorOperations::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4); + VectorOperations::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); + VectorOperations::normalize(quatBJ, quatBJ, 4); + + double updatedGyroBias[3] = {0, 0, 0}; + VectorOperations::add(biasGYR, errStateGyroBias, updatedGyroBias, 3); + // Bias GYR State + biasGYR[0] = updatedGyroBias[0]; + biasGYR[1] = updatedGyroBias[1]; + biasGYR[2] = updatedGyroBias[2]; + + /* ----------- PROPAGATION ----------*/ + // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; + // 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}, + {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; + double rotRateEst[3] = {0, 0, 0}; + VectorOperations::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3); + double normRotEst = VectorOperations::norm(rotRateEst, 3); + double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]}, + {rotRateEst[2], 0, -rotRateEst[0]}, + {-rotRateEst[1], rotRateEst[0], 0}}; + + // 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}, + {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}}, + covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + if (normRotEst * sampleTime < 3.141 / 10) { + double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3); + MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); + double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2)); + MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3); + MatrixOperations::transpose(*covQ2, *transCovQ2, 3); + double fact3 = sampleTime * pow(sigmaU, 2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3); + + discProcessNoiseCov[0][0] = covQ1[0][0]; + discProcessNoiseCov[0][1] = covQ1[0][1]; + discProcessNoiseCov[0][2] = covQ1[0][2]; + discProcessNoiseCov[0][3] = covQ2[0][0]; + discProcessNoiseCov[0][4] = covQ2[0][1]; + discProcessNoiseCov[0][5] = covQ2[0][2]; + 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 { + // double fact1 = sampleTime*pow(sigmaV,2); + double covQ11[3][3], covQ12[3][3], covQ13[3][3]; + // MatrixOperations::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::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3); + double fact2 = pow(sampleTime, 3); + MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3); + MatrixOperations::subtract(*covQ12, *covQ11, *covQ11, 3, 3); + double fact3 = sampleTime * pow(sigmaV, 2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3); + MatrixOperations::add(*covQ13, *covQ11, *covQ1, 3, 3); + + double covQ21[3][3], covQ22[3][3], covQ23[3][3]; + double fact4 = + (0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / + pow(normRotEst, 4); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); + double fact5 = 0.5 * pow(sampleTime, 2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); + MatrixOperations::add(*covQ22, *covQ21, *covQ21, 3, 3); + double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3); + MatrixOperations::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3); + MatrixOperations::subtract(*covQ23, *covQ21, *covQ21, 3, 3); + double fact7 = pow(sigmaU, 2); + MatrixOperations::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3); + + MatrixOperations::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); + + discProcessNoiseCov[0][0] = covQ1[0][0]; + discProcessNoiseCov[0][1] = covQ1[0][1]; + discProcessNoiseCov[0][2] = covQ1[0][2]; + discProcessNoiseCov[0][3] = covQ2[0][0]; + discProcessNoiseCov[0][4] = covQ2[0][1]; + discProcessNoiseCov[0][5] = covQ2[0][2]; + 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] = 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]; + } + + // State Transition Matrix phi + double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi2[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}, + {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 fact1 = sin(normRotEst * sampleTime); + MatrixOperations::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); + double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); + MatrixOperations::multiplyScalar(*phi12, fact2, *phi12, 3, 3); + MatrixOperations::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); + MatrixOperations::add(*phi11, *phi12, *phi1, 3, 3); + + double phi21[3][3], phi22[3][3]; + MatrixOperations::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); + MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); + MatrixOperations::subtract(*phi21, *phi22, *phi21, 3, 3); + double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); + MatrixOperations::multiplyScalar(*phi22, fact3, *phi22, 3, 3); + MatrixOperations::subtract(*phi21, *phi22, *phi2, 3, 3); + + phi[0][0] = phi1[0][0]; + phi[0][1] = phi1[0][1]; + phi[0][2] = phi1[0][2]; + phi[0][3] = phi2[0][0]; + phi[0][4] = phi2[0][1]; + phi[0][5] = phi2[0][2]; + phi[1][0] = phi1[1][0]; + phi[1][1] = phi1[1][1]; + phi[1][2] = phi1[1][2]; + phi[1][3] = phi2[1][0]; + phi[1][4] = phi2[1][1]; + phi[1][5] = phi2[1][2]; + phi[2][0] = phi1[2][0]; + phi[2][1] = phi1[2][1]; + phi[2][2] = phi1[2][2]; + phi[2][3] = phi2[2][0]; + phi[2][4] = phi2[2][1]; + phi[2][5] = phi2[2][2]; + + // Propagated Quaternion + double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double rotCos = cos(0.5 * normRotEst * sampleTime); + double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; + VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); + + double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::skewMatrix(rotSin, *skewSin); + + MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); + MatrixOperations::subtract(*omega1, *skewSin, *omega1, 3, 3); + double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]}, + {omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]}, + {omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]}, + {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}}; + MatrixOperations::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); + + // Update Covariance Matrix + double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6]; + MatrixOperations::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); + MatrixOperations::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); + MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); + + MatrixOperations::transpose(*phi, *transPhi, 6); + MatrixOperations::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6); + MatrixOperations::multiply(*phi, *cov2, *cov2, 6, 6, 6); + + MatrixOperations::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6); + validMekf = true; + + // Discrete Time Step + + // Check for new data in measurement -> SensorProcessing ? + + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double)); + mekfData->setValidity(true, true); + } + } + + return returnvalue::OK; +} diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h new file mode 100644 index 00000000..5eb47418 --- /dev/null +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -0,0 +1,100 @@ +/* + * MultiplicativeKalmanFilter.h + * + * Created on: 4 Feb 2022 + * Author: Robin Marquardt + * + * @brief: This class handles the calculation of an estimated quaternion and the gyro bias by + * means of the spacecraft attitude sensors + * + * @note: A description of the used algorithms can be found in the bachelor thesis of Robin + * Marquardt + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 + */ + +#ifndef MULTIPLICATIVEKALMANFILTER_H_ +#define MULTIPLICATIVEKALMANFILTER_H_ + +#include //uint8_t +#include /*purpose, timeval ?*/ + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "config/classIds.h" + +class MultiplicativeKalmanFilter { + public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + virtual ~MultiplicativeKalmanFilter(); + + void reset(); // NOT YET DEFINED - should only reset all mekf variables + + /* @brief: init() - This function initializes the Kalman Filter and will provide the first + * quaternion through the QUEST algorithm + * @param: magneticField_ magnetic field vector in the body frame + * sunDir_ sun direction vector in the body frame + * sunDirJ sun direction vector in the ECI frame + * magFieldJ magnetic field vector in the ECI frame + */ + void init(const double *magneticField_, const bool validMagField_, const double *sunDir_, + const bool validSS, const double *sunDirJ, const bool validSSModel, + const double *magFieldJ, const bool validMagModel); + + /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter + * for the current step after the initalization + * @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame + * rateGYRs_ Estimated satellite rotation rate from the + * Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_ + * sun direction vector in the body frame sunDirJ sun direction vector in the ECI + * frame magFieldJ magnetic field vector in the ECI frame + * outputQuat Stores the calculated quaternion + * outputSatRate Stores the adjusted satellite rate + * @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from + * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model + * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, + * RETURN_OK else + */ + ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, + const bool validGYRs_, const double *magneticField_, + const bool validMagField_, const double *sunDir_, const bool validSS, + const double *sunDirJ, const bool validSSModel, const double *magFieldJ, + const bool validMagModel, 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 + static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; + static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); + + private: + /*Parameters*/ + AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters::KalmanFilterParameters *kalmanFilterParameters; + double quaternion_STR_SB[4]; + bool validInit; + double sampleTime = 0.1; + + /*States*/ + double initialQuaternion[4]; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ + double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ + bool validMekf; + uint8_t sensorsAvail; + + /*Outputs*/ + double quatBJ[4]; /* Output Quaternion */ + double biasGYR[3]; /*Between measured and estimated sat Rate*/ + /*Parameter INIT*/ + // double alpha, gamma, beta; + /*Functions*/ + void loadAcsParameters(AcsParameters *acsParameters_); +}; + +#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp new file mode 100644 index 00000000..921ae604 --- /dev/null +++ b/mission/controller/acs/Navigation.cpp @@ -0,0 +1,57 @@ +/* + * Navigation.cpp + * + * Created on: 23 May 2022 + * Author: Robin Marquardt + */ + +#include "Navigation.h" + +#include +#include +#include +#include + +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" + +Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) { + acsParameters = *acsParameters_; +} + +Navigation::~Navigation() {} + +void Navigation::useMekf(ACS::SensorValues *sensorValues, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + ReturnValue_t *mekfValid) { + double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + bool quatJBValid = sensorValues->strSet.caliQx.isValid() && + sensorValues->strSet.caliQy.isValid() && + sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); + + if (kalmanInit) { + *mekfValid = multiplicativeKalmanFilter.mekfEst( + quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value, + gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, + mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, + susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, + susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, + mgmDataProcessed->magIgrfModel.isValid(), + mekfData); // VALIDS FOR QUAT AND RATE ?? + } else { + multiplicativeKalmanFilter.init( + mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), + susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), + susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid()); + kalmanInit = true; + *mekfValid = returnvalue::OK; + + // Maybe we need feedback from kalmanfilter to identify if there was a problem with the + // init of kalman filter where does this class know from that kalman filter was not + // initialized ? + } +} diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h new file mode 100644 index 00000000..2474cb67 --- /dev/null +++ b/mission/controller/acs/Navigation.h @@ -0,0 +1,35 @@ +/* + * Navigation.h + * + * Created on: 19 Apr 2022 + * Author: Robin Marquardt + */ + +#ifndef NAVIGATION_H_ +#define NAVIGATION_H_ + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "MultiplicativeKalmanFilter.h" +#include "SensorProcessing.h" +#include "SensorValues.h" + +class Navigation { + public: + Navigation(AcsParameters *acsParameters_); // Input mode ? + virtual ~Navigation(); + + void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + ReturnValue_t *mekfValid); + void processSensorData(); + + protected: + private: + MultiplicativeKalmanFilter multiplicativeKalmanFilter; + AcsParameters acsParameters; + bool kalmanInit = false; +}; + +#endif /* ACS_NAVIGATION_H_ */ diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp new file mode 100644 index 00000000..4635e673 --- /dev/null +++ b/mission/controller/acs/SensorProcessing.cpp @@ -0,0 +1,637 @@ +#include "SensorProcessing.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "Igrf13Model.h" +#include "util/MathOperations.h" + +using namespace Math; + +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) + : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {} + +SensorProcessing::~SensorProcessing() {} + +void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, + bool mgm1valid, const float *mgm2Value, bool mgm2valid, + const float *mgm3Value, bool mgm3valid, const float *mgm4Value, + bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + acsctrl::GpsDataProcessed *gpsDataProcessed, + const double gpsAltitude, bool gpsValid, + acsctrl::MgmDataProcessed *mgmDataProcessed) { + if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { + { + PoolReadGuard pg(mgmDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double)); + mgmDataProcessed->setValidity(false, true); + } + } + return; + } + float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, + mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[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}, + mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; + float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; + + if (mgm0valid) { + VectorOperations::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias, + mgm0ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, + mgm0ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; + } + } + if (mgm1valid) { + VectorOperations::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias, + mgm1ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, + mgm1ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; + } + } + if (mgm2valid) { + VectorOperations::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias, + mgm2ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, + mgm2ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; + } + } + if (mgm3valid) { + VectorOperations::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias, + mgm3ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, + mgm3ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; + } + } + if (mgm4valid) { + float mgm4ValueNT[3]; + VectorOperations::mulScalar(mgm4Value, 1e3, mgm4ValueNT, 3); // uT to nT + VectorOperations::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset, + mgm4ValueNoBias, 3); + MatrixOperations::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias, + mgm4ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, + mgm4ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; + } + } + double mgmVecTot[3] = {0.0, 0.0, 0.0}; + for (uint8_t i = 0; i < 3; i++) { + mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; + } + + //-----------------------Mgm Rate Computation --------------------------------------------------- + double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; + bool mgmVecTotDerivativeValid = false; + double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); + if (timeOfSavedMagFieldEst.tv_sec != 0) { + for (uint8_t i = 0; i < 3; i++) { + mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; + savedMgmVecTot[i] = mgmVecTot[i]; + } + } + 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); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm0vec.setValid(mgm0valid); + std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm1vec.setValid(mgm1valid); + std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm2vec.setValid(mgm2valid); + std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm3vec.setValid(mgm3valid); + std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm4vec.setValid(mgm4valid); + std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double)); + mgmDataProcessed->mgmVecTot.setValid(true); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative, + 3 * sizeof(double)); + mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid); + std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); + mgmDataProcessed->magIgrfModel.setValid(gpsValid); + mgmDataProcessed->setValidity(true, false); + } + } +} + +void SensorProcessing::processSus( + const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, + const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid, + const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid, + const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid, + const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, + const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, + acsctrl::SusDataProcessed *susDataProcessed) { + if (sus0valid) { + sus0valid = susConverter.checkSunSensorData(sus0Value); + } + if (sus1valid) { + sus1valid = susConverter.checkSunSensorData(sus1Value); + } + if (sus2valid) { + sus2valid = susConverter.checkSunSensorData(sus2Value); + } + if (sus3valid) { + sus3valid = susConverter.checkSunSensorData(sus3Value); + } + if (sus4valid) { + sus4valid = susConverter.checkSunSensorData(sus4Value); + } + if (sus5valid) { + sus5valid = susConverter.checkSunSensorData(sus5Value); + } + if (sus6valid) { + sus6valid = susConverter.checkSunSensorData(sus6Value); + } + if (sus7valid) { + sus7valid = susConverter.checkSunSensorData(sus7Value); + } + if (sus8valid) { + sus8valid = susConverter.checkSunSensorData(sus8Value); + } + if (sus9valid) { + sus9valid = susConverter.checkSunSensorData(sus9Value); + } + if (sus10valid) { + sus10valid = susConverter.checkSunSensorData(sus10Value); + } + if (sus11valid) { + sus11valid = susConverter.checkSunSensorData(sus11Value); + } + + if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && + !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { + { + PoolReadGuard pg(susDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double)); + susDataProcessed->setValidity(false, true); + } + } + return; + } + // WARNING: NOT TRANSFORMED IN BODY FRAME YET + // Transformation into Geomtry Frame + float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, + sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, + sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, + sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; + + if (sus0valid) { + MatrixOperations::multiply( + susParameters->sus0orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, + susParameters->sus0coeffBeta), + 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) { + MatrixOperations::multiply( + susParameters->sus1orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, + susParameters->sus1coeffBeta), + 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) { + MatrixOperations::multiply( + susParameters->sus2orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, + susParameters->sus2coeffBeta), + sus2VecBody, 3, 3, 1); + } + if (sus3valid) { + MatrixOperations::multiply( + susParameters->sus3orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, + susParameters->sus3coeffBeta), + sus3VecBody, 3, 3, 1); + } + if (sus4valid) { + MatrixOperations::multiply( + susParameters->sus4orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, + susParameters->sus4coeffBeta), + sus4VecBody, 3, 3, 1); + } + if (sus5valid) { + MatrixOperations::multiply( + susParameters->sus5orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, + susParameters->sus5coeffBeta), + sus5VecBody, 3, 3, 1); + } + if (sus6valid) { + MatrixOperations::multiply( + susParameters->sus6orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, + susParameters->sus6coeffBeta), + sus6VecBody, 3, 3, 1); + } + if (sus7valid) { + MatrixOperations::multiply( + susParameters->sus7orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, + susParameters->sus7coeffBeta), + sus7VecBody, 3, 3, 1); + } + if (sus8valid) { + MatrixOperations::multiply( + susParameters->sus8orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, + susParameters->sus8coeffBeta), + sus8VecBody, 3, 3, 1); + } + if (sus9valid) { + MatrixOperations::multiply( + susParameters->sus9orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, + susParameters->sus9coeffBeta), + sus9VecBody, 3, 3, 1); + } + if (sus10valid) { + MatrixOperations::multiply( + susParameters->sus10orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, + susParameters->sus10coeffBeta), + sus10VecBody, 3, 3, 1); + } + if (sus11valid) { + MatrixOperations::multiply( + susParameters->sus11orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, + susParameters->sus11coeffBeta), + sus11VecBody, 3, 3, 1); + } + + /* ------ Mean Value: susDirEst ------ */ + bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; + float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], + sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], + sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, + {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], + sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], + sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, + {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], + sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], + sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; + + double susMeanValue[3] = {0, 0, 0}; + for (uint8_t i = 0; i < 12; i++) { + if (validIds[i]) { + susMeanValue[0] += susVecBody[0][i]; + susMeanValue[1] += susVecBody[1][i]; + susMeanValue[2] += susVecBody[2][i]; + } + } + double susVecTot[3] = {0.0, 0.0, 0.0}; + VectorOperations::normalize(susMeanValue, susVecTot, 3); + + /* -------- Sun Derivatiative --------------------- */ + + double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; + bool susVecTotDerivativeValid = false; + double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); + if (timeOfSavedSusDirEst.tv_sec != 0) { + for (uint8_t i = 0; i < 3; i++) { + susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; + savedSusVecTot[i] = susVecTot[i]; + } + } + timeOfSavedSusDirEst = timeOfSusMeasurement; + + /* -------- Sun Model Direction (IJK frame) ------- */ + // if (useSunModel) eventuell + double JD2000 = MathOperations::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); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); + susDataProcessed->sus0vec.setValid(sus0valid); + std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); + susDataProcessed->sus1vec.setValid(sus1valid); + std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float)); + susDataProcessed->sus2vec.setValid(sus2valid); + std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float)); + susDataProcessed->sus3vec.setValid(sus3valid); + std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float)); + susDataProcessed->sus4vec.setValid(sus4valid); + std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float)); + susDataProcessed->sus5vec.setValid(sus5valid); + std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float)); + susDataProcessed->sus6vec.setValid(sus6valid); + std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float)); + susDataProcessed->sus7vec.setValid(sus7valid); + std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float)); + susDataProcessed->sus8vec.setValid(sus8valid); + std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float)); + susDataProcessed->sus9vec.setValid(sus9valid); + std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float)); + susDataProcessed->sus10vec.setValid(sus10valid); + std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); + susDataProcessed->sus11vec.setValid(sus11valid); + std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double)); + susDataProcessed->susVecTot.setValid(true); + std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative, + 3 * sizeof(double)); + susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid); + std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); + susDataProcessed->sunIjkModel.setValid(true); + susDataProcessed->setValidity(true, false); + } + } +} + +void SensorProcessing::processGyr( + const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid, + const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid, + const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, + const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid, + const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, + const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, + timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, + acsctrl::GyrDataProcessed *gyrDataProcessed) { + bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid); + bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid); + bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid); + bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid); + if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) { + { + PoolReadGuard pg(gyrDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(gyrDataProcessed->gyr0vec.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->gyr3vec.value, zeroVector, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); + gyrDataProcessed->setValidity(false, true); + } + } + return; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, + gyr3ValueBody[3] = {0, 0, 0}; + float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; + + if (gyr0valid) { + const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, + gyr0ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; + } + } + if (gyr1valid) { + const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, + gyr1ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; + } + } + if (gyr2valid) { + const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, + gyr2ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; + } + } + if (gyr3valid) { + const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, + gyr3ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; + } + } + + /* -------- SatRateEst: Middle Value ------- */ + // take ADIS measurements, if both avail + // if just one ADIS measurement avail, perform sensor fusion + double gyrVecTot[3] = {0.0, 0.0, 0.0}; + if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { + double gyr02ValuesSum[3]; + VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); + VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); + } else { + for (uint8_t i = 0; i < 3; i++) { + gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; + } + } + { + PoolReadGuard pg(gyrDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr0vec.setValid(gyr0valid); + std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr1vec.setValid(gyr1valid); + std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr2vec.setValid(gyr2valid); + std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr3vec.setValid(gyr3valid); + std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double)); + gyrDataProcessed->gyrVecTot.setValid(true); + gyrDataProcessed->setValidity(true, false); + } + } +} + +void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, + const bool validGps, + acsctrl::GpsDataProcessed *gpsDataProcessed) { + // name to convert not process + double gdLongitude, gcLatitude; + if (validGps) { + // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic + gdLongitude = gps0longitude * PI / 180; + double latitudeRad = gps0latitude * PI / 180; + double eccentricityWgs84 = 0.0818195; + double factor = 1 - pow(eccentricityWgs84, 2); + gcLatitude = atan(factor * tan(latitudeRad)); + // validGcLatitude = true; + } + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + gpsDataProcessed->gdLongitude.value = gdLongitude; + gpsDataProcessed->gcLatitude.value = gcLatitude; + gpsDataProcessed->setValidity(validGps, validGps); + if (!validGps) { + gpsDataProcessed->gdLongitude.value = 0.0; + gpsDataProcessed->gcLatitude.value = 0.0; + } + } + } +} + +void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, + const AcsParameters *acsParameters) { + sensorValues->update(); + processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, + (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && + sensorValues->gpsSet.altitude.isValid()), + gpsDataProcessed); + + processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, + sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm1Rm3100Set.fieldStrengths.value, + sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(), + sensorValues->mgm2Lis3Set.fieldStrengths.value, + sensorValues->mgm2Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm3Rm3100Set.fieldStrengths.value, + sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), + sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), + now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, + sensorValues->gpsSet.altitude.value, + (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && + sensorValues->gpsSet.altitude.isValid()), + mgmDataProcessed); + + processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), + sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), + sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(), + sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(), + sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(), + sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(), + sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(), + sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(), + sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(), + sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(), + sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), + sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), + now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, + susDataProcessed); + + processGyr( + sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), + sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(), + sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(), + sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(), + sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(), + sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(), + sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(), + sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(), + sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(), + sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), + sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), + sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, + &acsParameters->gyrHandlingParameters, gyrDataProcessed); +} diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h new file mode 100644 index 00000000..61bd7a96 --- /dev/null +++ b/mission/controller/acs/SensorProcessing.h @@ -0,0 +1,84 @@ +/******************************* + * EIVE Flight Software Framework (FSFW) + * (c) 2022 IRS, Uni Stuttgart + *******************************/ +#ifndef SENSORPROCESSING_H_ +#define SENSORPROCESSING_H_ + +#include +#include //uint8_t +#include /*purpose, timeval ?*/ + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "SensorValues.h" +#include "SusConverter.h" +#include "config/classIds.h" + +class SensorProcessing { + public: + void reset(); + + SensorProcessing(AcsParameters *acsParameters_); + virtual ~SensorProcessing(); + + void process(timeval now, ACS::SensorValues *sensorValues, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, + const AcsParameters *acsParameters); // Will call protected functions + private: + protected: + // short description needed for every function + void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, + const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, + const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, + bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed); + + void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, + bool sus1valid, const uint16_t *sus2Value, bool sus2valid, + const uint16_t *sus3Value, bool sus3valid, const uint16_t *sus4Value, + bool sus4valid, const uint16_t *sus5Value, bool sus5valid, + const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, + bool sus7valid, const uint16_t *sus8Value, bool sus8valid, + const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, + bool sus10valid, const uint16_t *sus11Value, bool sus11valid, + timeval timeOfSusMeasurement, + const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, + acsctrl::SusDataProcessed *susDataProcessed); + + void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, + bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid, + const double gyr1axXvalue, bool gyr1axXvalid, const double gyr1axYvalue, + bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, + const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, + bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, + const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, + bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, + timeval timeOfGyrMeasurement, + const AcsParameters::GyrHandlingParameters *gyrParameters, + acsctrl::GyrDataProcessed *gyrDataProcessed); + + void processStr(); + + void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, + acsctrl::GpsDataProcessed *gpsDataProcessed); + + double savedMgmVecTot[3]; + timeval timeOfSavedMagFieldEst; + double savedSusVecTot[3]; + timeval timeOfSavedSusDirEst; + bool validMagField; + bool validGcLatitude; + + const float zeroVector[3] = {0.0, 0.0, 0.0}; + + SusConverter susConverter; + AcsParameters acsParameters; +}; + +#endif /*SENSORPROCESSING_H_*/ diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp new file mode 100644 index 00000000..568a9e1c --- /dev/null +++ b/mission/controller/acs/SensorValues.cpp @@ -0,0 +1,95 @@ +/* + * SensorValues.cpp + * + * Created on: 30 Mar 2022 + * Author: rooob + */ +#include "SensorValues.h" + +#include +#include +#include +#include + +#include + +namespace ACS { + +SensorValues::SensorValues() {} + +SensorValues::~SensorValues() {} + +ReturnValue_t SensorValues::updateMgm() { + ReturnValue_t result; + PoolReadGuard pgMgm0(&mgm0Lis3Set), pgMgm1(&mgm1Rm3100Set), pgMgm2(&mgm2Lis3Set), + pgMgm3(&mgm3Rm3100Set), pgImtq(&imtqMgmSet); + + result = (pgMgm0.getReadResult() || pgMgm1.getReadResult() || pgMgm2.getReadResult() || + pgMgm3.getReadResult() || pgImtq.getReadResult()); + return result; +} + +ReturnValue_t SensorValues::updateSus() { + ReturnValue_t result; + PoolReadGuard pgSus0(&susSets[0]), pgSus1(&susSets[1]), pgSus2(&susSets[2]), pgSus3(&susSets[3]), + pgSus4(&susSets[4]), pgSus5(&susSets[5]), pgSus6(&susSets[6]), pgSus7(&susSets[7]), + pgSus8(&susSets[8]), pgSus9(&susSets[9]), pgSus10(&susSets[10]), pgSus11(&susSets[11]); + + result = (pgSus0.getReadResult() || pgSus1.getReadResult() || pgSus2.getReadResult() || + pgSus3.getReadResult() || pgSus4.getReadResult() || pgSus5.getReadResult() || + pgSus6.getReadResult() || pgSus7.getReadResult() || pgSus8.getReadResult() || + pgSus9.getReadResult() || pgSus10.getReadResult() || pgSus11.getReadResult()); + return result; +} + +ReturnValue_t SensorValues::updateGyr() { + ReturnValue_t result; + PoolReadGuard pgGyr0(&gyr0AdisSet), pgGyr1(&gyr1L3gSet), pgGyr2(&gyr2AdisSet), + pgGyr3(&gyr3L3gSet); + + result = (pgGyr0.getReadResult() || pgGyr1.getReadResult() || pgGyr2.getReadResult() || + pgGyr3.getReadResult()); + return result; +} + +ReturnValue_t SensorValues::updateStr() { + ReturnValue_t result; + PoolReadGuard pgStr(&strSet); + + result = pgStr.getReadResult(); + return result; +} + +ReturnValue_t SensorValues::updateGps() { + ReturnValue_t result; + PoolReadGuard pgGps(&gpsSet); + + result = pgGps.getReadResult(); + return result; +} + +ReturnValue_t SensorValues::updateRw() { + ReturnValue_t result; + PoolReadGuard pgRw1(&rw1Set), pgRw2(&rw2Set), pgRw3(&rw3Set), pgRw4(&rw4Set); + + result = (pgRw1.getReadResult() || pgRw2.getReadResult() || pgRw3.getReadResult() || + pgRw4.getReadResult()); + return result; +} + +ReturnValue_t SensorValues::update() { + ReturnValue_t mgmUpdate = updateMgm(); + ReturnValue_t susUpdate = updateSus(); + ReturnValue_t gyrUpdate = updateGyr(); + ReturnValue_t strUpdate = updateStr(); + ReturnValue_t gpsUpdate = updateGps(); + ReturnValue_t rwUpdate = updateRw(); + + if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate && gpsUpdate && rwUpdate) == + returnvalue::FAILED) { + return returnvalue::FAILED; + }; + return returnvalue::OK; +} + +} // namespace ACS diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h new file mode 100644 index 00000000..ef897207 --- /dev/null +++ b/mission/controller/acs/SensorValues.h @@ -0,0 +1,72 @@ +#ifndef SENSORVALUES_H_ +#define SENSORVALUES_H_ + +#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" +#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" +#include "mission/devices/devicedefinitions/GPSDefinitions.h" +#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h" +#include "mission/devices/devicedefinitions/RwDefinitions.h" +#include "mission/devices/devicedefinitions/SusDefinitions.h" +#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" + +namespace ACS { + +class SensorValues { + public: + SensorValues(); + virtual ~SensorValues(); + + ReturnValue_t update(); + ReturnValue_t updateMgm(); + ReturnValue_t updateSus(); + ReturnValue_t updateGyr(); + ReturnValue_t updateGps(); + ReturnValue_t updateStr(); + ReturnValue_t updateRw(); + + MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = + MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); + RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = + RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); + MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = + MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); + RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = + RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); + IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); + + std::array susSets{ + SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), + SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), + SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), + SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), + SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), + SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), + SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), + SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), + SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), + SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), + SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), + SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), + }; + + AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER); + GyroPrimaryDataset gyr1L3gSet = GyroPrimaryDataset(objects::GYRO_1_L3G_HANDLER); + AdisGyroPrimaryDataset gyr2AdisSet = AdisGyroPrimaryDataset(objects::GYRO_2_ADIS_HANDLER); + GyroPrimaryDataset gyr3L3gSet = GyroPrimaryDataset(objects::GYRO_3_L3G_HANDLER); + + startracker::SolutionSet strSet = startracker::SolutionSet(objects::STAR_TRACKER); + + GpsPrimaryDataset gpsSet = GpsPrimaryDataset(objects::GPS_CONTROLLER); + + // bool mgt0valid; + + RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1); + RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2); + RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3); + RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4); +}; +} /* namespace ACS */ + +#endif /*ENSORVALUES_H_*/ diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp new file mode 100644 index 00000000..7020742a --- /dev/null +++ b/mission/controller/acs/SusConverter.cpp @@ -0,0 +1,129 @@ +/* + * SusConverter.cpp + * + * Created on: 17.01.2022 + * Author: Timon Schwarz + */ + +#include "SusConverter.h" + +#include +#include +#include +#include //for atan2 + +#include + +bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { + if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || + susChannel[0] > susChannel[GNDREF]) { + return false; + } + if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || + susChannel[1] > susChannel[GNDREF]) { + return false; + }; + if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || + susChannel[2] > susChannel[GNDREF]) { + return false; + }; + if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || + susChannel[3] > susChannel[GNDREF]) { + return false; + }; + + susChannelValueSum = + 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); + if ((susChannelValueSum < susChannelValueSumHigh) && + (susChannelValueSum > susChannelValueSumLow)) { + return false; + }; + return true; +} + +void SusConverter::calcAngle(const uint16_t susChannel[6]) { + float xout, yout; + float s = 0.03; // s=[mm] gap between diodes + uint8_t d = 5; // d=[mm] edge length of the quadratic aperture + uint8_t h = 1; // h=[mm] distance between diodes and aperture + int ch0, ch1, ch2, ch3; + // Substract measurement values from GNDREF zero current threshold + ch0 = susChannel[GNDREF] - susChannel[0]; + ch1 = susChannel[GNDREF] - susChannel[1]; + ch2 = susChannel[GNDREF] - susChannel[2]; + ch3 = susChannel[GNDREF] - susChannel[3]; + + // Calculation of x and y + xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + + // Calculation of the angles + alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] + alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] +} + +void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { + uint8_t index; + float k, l; + + // while loop iterates above all calibration cells to use the different calibration functions in + // each cell + k = 0; + while (k < 3) { + k++; + l = 0; + while (l < 3) { + l++; + // if-condition to check in which cell the data point has to be + if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && + alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) && + (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && + alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) { + index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell + alphaBetaCalibrated[0] = + coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + + coeffAlpha[index][2] * alphaBetaRaw[1] + + coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] + alphaBetaCalibrated[1] = + coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] + + coeffBeta[index][2] * alphaBetaRaw[1] + + coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] + } + } + } +} + +float* SusConverter::calculateSunVector() { + // Calculate the normalized Sun Vector + sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / + (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / + (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + sunVectorSensorFrame[2] = + -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + + return sunVectorSensorFrame; +} + +float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], + const float coeffAlpha[9][10], + const float coeffBeta[9][10]) { + calcAngle(susChannel); + calibration(coeffAlpha, coeffBeta); + return calculateSunVector(); +} diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h new file mode 100644 index 00000000..b3829827 --- /dev/null +++ b/mission/controller/acs/SusConverter.h @@ -0,0 +1,59 @@ +/* + * SusConverter.h + * + * Created on: Sep 22, 2022 + * Author: marius + */ + +#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ +#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ + +#include +#include + +#include "AcsParameters.h" + +class SusConverter { + public: + SusConverter() {} + + bool checkSunSensorData(const uint16_t susChannel[6]); + + void calcAngle(const uint16_t susChannel[6]); + void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); + float* calculateSunVector(); + + float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], + const float coeffBeta[9][10]); + + private: + float alphaBetaRaw[2]; //[°] + // float coeffAlpha[9][10]; + // float coeffBeta[9][10]; + float alphaBetaCalibrated[2]; //[°] + float sunVectorSensorFrame[3]; //[-] + + bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, + returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, + returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; + + static const uint8_t GNDREF = 4; + uint16_t susChannelValueCheckHigh = + 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check + uint8_t susChannelValueCheckLow = + 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check + uint16_t susChannelValueSumHigh = + 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by + // the reflection of sunlight from the moon/earth + uint8_t susChannelValueSumLow = + 0; //[Bit]low borderline for check if the sun sensor is illuminated + // by the sun or by the reflection of sunlight from the moon/earth + uint8_t completeCellWidth = 140, + halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in + // which cell a data point should be + uint16_t susChannelValueSum = 0; + + AcsParameters acsParameters; +}; + +#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */ diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h new file mode 100644 index 00000000..ccf6b616 --- /dev/null +++ b/mission/controller/acs/config/classIds.h @@ -0,0 +1,18 @@ +#ifndef ACS_CONFIG_CLASSIDS_H_ +#define ACS_CONFIG_CLASSIDS_H_ + +#include +#include + +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_ */ diff --git a/mission/controller/acs/control/CMakeLists.txt b/mission/controller/acs/control/CMakeLists.txt new file mode 100644 index 00000000..2d40ecc5 --- /dev/null +++ b/mission/controller/acs/control/CMakeLists.txt @@ -0,0 +1,2 @@ +target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp + SafeCtrl.cpp) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp new file mode 100644 index 00000000..4c8217dc --- /dev/null +++ b/mission/controller/acs/control/Detumble.cpp @@ -0,0 +1,52 @@ + +/* + * Detumble.cpp + * + * Created on: 17 Aug 2022 + * Author: Robin Marquardt + */ + +#include "Detumble.h" + +#include +#include +#include +#include +#include +#include + +#include "../util/MathOperations.h" + +Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } + +Detumble::~Detumble() {} + +void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { + detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); + magnetorquesParameter = &(acsParameters_->magnetorquesParameter); +} + +ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, + const double *magField, const bool magFieldValid, double *magMom) { + if (!magRateValid || !magFieldValid) { + return DETUMBLE_NO_SENSORDATA; + } + double gain = detumbleCtrlParameters->gainD; + double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); + VectorOperations::mulScalar(magRate, factor, magMom, 3); + return returnvalue::OK; +} + +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, + double *magMom) { + if (!magRateValid) { + return DETUMBLE_NO_SENSORDATA; + } + + double dipolMax = magnetorquesParameter->DipolMax; + for (int i = 0; i < 3; i++) { + magMom[i] = -dipolMax * sign(magRate[i]); + } + + return returnvalue::OK; +} diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h new file mode 100644 index 00000000..bea9b1bf --- /dev/null +++ b/mission/controller/acs/control/Detumble.h @@ -0,0 +1,43 @@ +/* + * Detumble.h + * + * Created on: 17 Aug 2022 + * Author: Robin Marquardt + */ + +#ifndef ACS_CONTROL_DETUMBLE_H_ +#define ACS_CONTROL_DETUMBLE_H_ + +#include +#include +#include +#include + +#include "../AcsParameters.h" +#include "../SensorValues.h" +#include "../config/classIds.h" + +class Detumble { + public: + Detumble(AcsParameters *acsParameters_); + virtual ~Detumble(); + + static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE; + static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); + + /* @brief: Load AcsParameters für this class + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); + + ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, + const bool magFieldValid, double *magMom); + + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); + + private: + AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters; + AcsParameters::MagnetorquesParameter *magnetorquesParameter; +}; + +#endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp new file mode 100644 index 00000000..07b595ec --- /dev/null +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -0,0 +1,159 @@ +/* + * PtgCtrl.cpp + * + * Created on: 17 Jul 2022 + * Author: Robin Marquardt + */ + +#include "PtgCtrl.h" + +#include +#include +#include +#include +#include +#include + +#include "../util/MathOperations.h" + +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } + +PtgCtrl::~PtgCtrl() {} + +void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { + pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); + rwHandlingParameters = &(acsParameters_->rwHandlingParameters); + rwMatrices = &(acsParameters_->rwMatrices); +} + +void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate, + const double *rwPseudoInv, double *torqueRws) { + //------------------------------------------------------------------------------------------------ + // Compute gain matrix K and P matrix + //------------------------------------------------------------------------------------------------ + double om = pointingModeControllerParameters->om; + double zeta = pointingModeControllerParameters->zeta; + double qErrorMin = pointingModeControllerParameters->qiMin; + double omMax = pointingModeControllerParameters->omMax; + + double cInt = 2 * om * zeta; + double kInt = 2 * pow(om, 2); + + double qErrorLaw[3] = {0, 0, 0}; + + for (int i = 0; i < 3; i++) { + if (abs(qError[i]) < qErrorMin) { + qErrorLaw[i] = qErrorMin; + } else { + qErrorLaw[i] = abs(qError[i]); + } + } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); + + double gain1 = cInt * omMax / qErrorLawNorm; + double gainVector[3] = {0, 0, 0}; + VectorOperations::mulScalar(qErrorLaw, gain1, gainVector, 3); + + double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixDiagonal[0][0] = gainVector[0]; + gainMatrixDiagonal[1][1] = gainVector[1]; + gainMatrixDiagonal[2][2] = gainVector[2]; + MatrixOperations::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), + *gainMatrix, 3, 3, 3); + + // Inverse of gainMatrix + double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; + gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; + + double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, + 3, 3); + MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); + + //------------------------------------------------------------------------------------------------ + // Torque Calculations for the reaction wheels + //------------------------------------------------------------------------------------------------ + + double pError[3] = {0, 0, 0}; + MatrixOperations::multiply(*pMatrix, qError, pError, 3, 3, 1); + double pErrorSign[3] = {0, 0, 0}; + + for (int i = 0; i < 3; i++) { + if (abs(pError[i]) > 1) { + pErrorSign[i] = sign(pError[i]); + } else { + pErrorSign[i] = pError[i]; + } + } + // Torque for quaternion error + double torqueQuat[3] = {0, 0, 0}; + MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); + VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); + + // Torque for rate error + double torqueRate[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); + VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); + + // Final commanded Torque for every reaction wheel + double torque[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueQuat, torque, 3); + MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); +} + +void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, + int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, + int32_t *speedRw3, double *mgtDpDes) { + if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { + mgtDpDes[0] = 0; + mgtDpDes[1] = 0; + mgtDpDes[2] = 0; + return; + } + + // calculating momentum of satellite and momentum of reaction wheels + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; + VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); + MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, + 1); + double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); + // calculating momentum error + double deltaMomentum[3] = {0, 0, 0}; + VectorOperations::subtract( + momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); + // resulting magnetic dipole command + double crossMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); + double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; + factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; + VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); +} + +void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, + const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + double wheelMomentum[4] = {0, 0, 0, 0}; + double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; + // Conversion to [rad/s] for further calculations + VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); + VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); + double diffRwSpeed[4] = {0, 0, 0, 0}; + VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, + wheelMomentum, 4); + double gainNs = pointingModeControllerParameters->gainNullspace; + double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, + *nullSpaceMatrix, 4); + MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); + VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); +} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h new file mode 100644 index 00000000..64b4110c --- /dev/null +++ b/mission/controller/acs/control/PtgCtrl.h @@ -0,0 +1,61 @@ +/* + * PtgCtrl.h + * + * Created on: 17 Jul 2022 + * Author: Robin Marquardt + * + * @brief: This class handles the pointing control mechanism. Calculation of an commanded + * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * + * @note: A description of the used algorithms can be found in + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 + */ + +#ifndef PTGCTRL_H_ +#define PTGCTRL_H_ + +#include +#include +#include + +#include "../AcsParameters.h" +#include "../SensorValues.h" +#include "../config/classIds.h" + +class PtgCtrl { + public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + PtgCtrl(AcsParameters *acsParameters_); + virtual ~PtgCtrl(); + + static const uint8_t INTERFACE_ID = CLASS_ID::PTG; + static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + + /* @brief: Load AcsParameters für this class + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); + + /* @brief: Calculates the needed torque for the pointing control mechanism + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, + const double *rwPseudoInv, double *torqueRws); + + void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, + int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, + double *mgtDpDes); + + void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + const int32_t *speedRw3, double *rwTrqNs); + + private: + AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; + AcsParameters::RwHandlingParameters *rwHandlingParameters; + AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters::RwMatrices *rwMatrices; +}; + +#endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp new file mode 100644 index 00000000..54ae27ee --- /dev/null +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -0,0 +1,178 @@ +/* + * SafeCtrl.cpp + * + * Created on: 19 Apr 2022 + * Author: Robin Marquardt + */ + +#include "SafeCtrl.h" + +#include +#include +#include +#include +#include + +#include "../util/MathOperations.h" + +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { + loadAcsParameters(acsParameters_); + MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, + 3); +} + +SafeCtrl::~SafeCtrl() {} + +void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { + safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); +} + +ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, + double *magFieldModel, bool magFieldModelValid, + double *sunDirModel, bool sunDirModelValid, double *satRateMekf, + bool rateMekfValid, double *sunDirRef, double *satRatRef, + double *outputAngle, double *outputMagMomB, bool *outputValid) { + if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) { + *outputValid = false; + return SAFECTRL_MEKF_INPUT_INVALID; + } + + double kRate = 0, kAlign = 0; + kRate = safeModeControllerParameters->k_rate_mekf; + kAlign = safeModeControllerParameters->k_align_mekf; + + // Calc sunDirB ,magFieldB with mekf output and model + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::dcmFromQuat(quatBJ, *dcmBJ); + double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); + + double crossSun[3] = {0, 0, 0}; + + VectorOperations::cross(sunDirRef, sunDirB, crossSun); + double normCrossSun = VectorOperations::norm(crossSun, 3); + + // calc angle alpha between sunDirRef and sunDIr + double alpha = 0, dotSun = 0; + dotSun = VectorOperations::dot(sunDirRef, sunDirB); + alpha = acos(dotSun); + + // Law Torque calculations + double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0}, + torqueAll[3] = {0, 0, 0}; + + double scalarFac = 0; + scalarFac = kAlign * alpha / normCrossSun; + VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); + + double rateSafeMode[3] = {0, 0, 0}; + VectorOperations::subtract(satRateMekf, satRatRef, rateSafeMode, 3); + VectorOperations::mulScalar(rateSafeMode, -kRate, torqueRate, 3); + + VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); + // Adding factor of inertia for axes + MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); + + // MagMom B (orthogonal torque) + double torqueMgt[3] = {0, 0, 0}; + VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); + double normMag = VectorOperations::norm(magFieldB, 3); + VectorOperations::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); + + *outputAngle = alpha; + *outputValid = true; + + return returnvalue::OK; +} + +// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) +void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, + bool sunRateBValid, double *magFieldB, bool magFieldBValid, + double *magRateB, bool magRateBValid, double *sunDirRef, + double *satRateRef, double *outputAngle, double *outputMagMomB, + bool *outputValid) { + // Check for invalid Inputs + if (!susDirBValid || !magFieldBValid || !magRateBValid) { + *outputValid = false; + return; + } + + // normalize sunDir and magDir + double magDirB[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(susDirB, susDirB, 3); + + // Cosinus angle between sunDir and magDir + double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); + + // Rate parallel to sun direction and magnetic field direction + double rateParaSun = 0, rateParaMag = 0; + double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0; + dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); + dotmagRateSun = VectorOperations::dot(magRateB, susDirB); + rateFactor = 1 - pow(cosAngleSunMag, 2); + rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; + rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; + + // Full rate or estimate + double estSatRate[3] = {0, 0, 0}; + double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; + VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); + VectorOperations::add(sunRateB, estSatRateSun, estSatRateSun, 3); + VectorOperations::mulScalar(magDirB, rateParaMag, estSatRateMag, 3); + VectorOperations::add(magRateB, estSatRateMag, estSatRateMag, 3); + VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); + VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); + + /* Only valid if angle between sun direction and magnetic field direction + is sufficiently large */ + + double sinAngle = 0; + sinAngle = sin(acos(cos(cosAngleSunMag))); + + if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) { + return; + } + + // Rate for Torque Calculation + double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ + VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); + + // Torque Align calculation + double kRateNoMekf = 0, kAlignNoMekf = 0; + kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; + kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + + double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); + double crossSusSunRef[3] = {0, 0, 0}; + VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); + double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); + + double torqueAlign[3] = {0, 0, 0}; + double angleAlignErr = acos(cosAngleAlignErr); + double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; + VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); + + // Torque Rate Calculations + double torqueRate[3] = {0, 0, 0}; + VectorOperations::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3); + + // Final torque + double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, + 1); + + // Magnetic moment + double magMomB[3] = {0, 0, 0}; + double crossMagFieldTorque[3] = {0, 0, 0}; + VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); + double magMomFactor = pow(VectorOperations::norm(magFieldB, 3), 2); + VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); + + std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); + *outputAngle = angleAlignErr; + *outputValid = true; +} diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h new file mode 100644 index 00000000..72e45f08 --- /dev/null +++ b/mission/controller/acs/control/SafeCtrl.h @@ -0,0 +1,52 @@ +/* + * safeCtrl.h + * + * Created on: 19 Apr 2022 + * Author: rooob + */ + +#ifndef SAFECTRL_H_ +#define SAFECTRL_H_ + +#include +#include +#include + +#include "../AcsParameters.h" +#include "../SensorValues.h" +#include "../config/classIds.h" + +class SafeCtrl { + public: + SafeCtrl(AcsParameters *acsParameters_); + virtual ~SafeCtrl(); + + static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; + static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + + void loadAcsParameters(AcsParameters *acsParameters_); + + ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, + bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, + double *satRateMekf, bool rateMekfValid, double *sunDirRef, + double *satRatRef, // From Guidance (!) + double *outputAngle, double *outputMagMomB, bool *outputValid); + + void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, + bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, + bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, + double *outputMagMomB, bool *outputValid); + + void idleSunPointing(); // with reaction wheels + + protected: + private: + AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; + AcsParameters::InertiaEIVE *inertiaEIVE; + double gainMatrixInertia[3][3]; + + double magFieldBState[3]; + timeval magFieldBStateTime; +}; + +#endif /* ACS_CONTROL_SAFECTRL_H_ */ diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h new file mode 100644 index 00000000..667f9f63 --- /dev/null +++ b/mission/controller/acs/util/CholeskyDecomposition.h @@ -0,0 +1,98 @@ +/* + * TinyEKF: Extended Kalman Filter for embedded processors + * + * Copyright (C) 2015 Simon D. Levy + * + * MIT License + */ +#ifndef CHOLESKYDECOMPOSITION_H_ +#define CHOLESKYDECOMPOSITION_H_ +#include +// typedef unsigned int uint8_t; + +template +class CholeskyDecomposition { + public: + static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) { + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + return cholsl(matrix, result, tempMatrix, dimension); + } + + private: + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t choldc1(double *a, double *p, uint8_t n) { + int8_t i, j, k; + double sum; + + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = a[i * n + j]; + for (k = i - 1; k >= 0; k--) { + sum -= a[i * n + k] * a[j * n + k]; + } + if (i == j) { + if (sum <= 0) { + return 1; /* error */ + } + p[i] = sqrt(sum); + } else { + a[j * n + i] = sum / p[i]; + } + } + } + + return 0; /* success */ + } + + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) { + uint8_t i, j, k; + double sum; + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j]; + if (choldc1(a, p, n)) return 1; + for (i = 0; i < n; i++) { + a[i * n + i] = 1 / p[i]; + for (j = i + 1; j < n; j++) { + sum = 0; + for (k = i; k < j; k++) { + sum -= a[j * n + k] * a[k * n + i]; + } + a[j * n + i] = sum / p[j]; + } + } + + return 0; /* success */ + } + + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) { + uint8_t i, j, k; + if (choldcsl(A, a, p, n)) return 1; + for (i = 0; i < n; i++) { + for (j = i + 1; j < n; j++) { + a[i * n + j] = 0.0; + } + } + for (i = 0; i < n; i++) { + a[i * n + i] *= a[i * n + i]; + for (k = i + 1; k < n; k++) { + a[i * n + i] += a[k * n + i] * a[k * n + i]; + } + for (j = i + 1; j < n; j++) { + for (k = j; k < n; k++) { + a[i * n + j] += a[k * n + i] * a[k * n + j]; + } + } + } + for (i = 0; i < n; i++) { + for (j = 0; j < i; j++) { + a[i * n + j] = a[j * n + i]; + } + } + + return 0; /* success */ + } +}; + +#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */ diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h new file mode 100644 index 00000000..6484a72d --- /dev/null +++ b/mission/controller/acs/util/MathOperations.h @@ -0,0 +1,395 @@ +#ifndef MATH_MATHOPERATIONS_H_ +#define MATH_MATHOPERATIONS_H_ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace Math; + +template +class MathOperations { + public: + static void skewMatrix(const T1 vector[], T2 *result) { + // Input Dimension [3], Output [3][3] + result[0] = 0; + result[1] = -vector[2]; + result[2] = vector[1]; + result[3] = vector[2]; + result[4] = 0; + result[5] = -vector[0]; + result[6] = -vector[1]; + result[7] = vector[0]; + result[8] = 0; + } + static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result, + uint8_t size = 3) { + // Looks like MatrixOpertions::multiply is able to do the same thing + for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) { + for (uint8_t resultRow = 0; resultRow < size; resultRow++) { + result[resultColumn + size * resultRow] = + vector1[resultRow] * transposeVector2[resultColumn]; + } + } + /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; + matrixMag[i][j] = magEstB[i] * magEstB[j]; + matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; + matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ + } + + static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { + int min_idx; + T1 temp; + memcpy(result, matrix, rowSize * colSize * sizeof(*result)); + // One by one move boundary of unsorted subarray + for (int k = 0; k < rowSize; k++) { + for (int i = 0; i < colSize - 1; i++) { + // Find the minimum element in unsorted array + min_idx = i; + for (int j = i + 1; j < colSize; j++) { + if (result[j + k * colSize] < result[min_idx + k * colSize]) { + min_idx = j; + } + } + // Swap the found minimum element with the first element + temp = result[i + k * colSize]; + result[i + k * colSize] = result[min_idx + k * colSize]; + result[min_idx + k * colSize] = temp; + } + } + } + + static void convertDateToJD2000(const T1 time, T2 julianDate) { + // time = { Y, M, D, h, m,s} + // time in sec and microsec -> The Epoch (unixtime) + julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) + + floor(275 * time[1] / 9) + time[2] + + (60 * time[3] + time[4] + (time(5) / 60)) / 1440; + } + + static T1 convertUnixToJD2000(timeval time) { + // time = {{s},{us}} + T1 julianDate2000; + julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545; + return julianDate2000; + } + + static void dcmFromQuat(const T1 vector[], T1 *outputDcm) { + // convention q = [qx,qy,qz, qw] + outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); + outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]); + outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]); + + outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]); + outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); + outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]); + + outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]); + outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]); + outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2); + } + + static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, + T2 *cartesianOutput) { + double radiusPolar = 6378137; + double radiusEqua = 6356752.314; + + double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); + double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); + + cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); + cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); + cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); + } + + /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame + * @param: time Current time + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff + * Landis Markley and John L. Crassidis*/ + static void dcmEJ(timeval time, T1 *outputDcmEJ) { + double JD2000Floor = 0; + double JD2000 = convertUnixToJD2000(time); + // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) + JD2000Floor = floor(JD2000); + if ((JD2000 - JD2000Floor) < 0.5) { + JD2000Floor -= 0.5; + } else { + JD2000Floor += 0.5; + } + + double JC2000 = JD2000Floor / 36525; + double sec = (JD2000 - JD2000Floor) * 86400; + double gmst = 0; // greenwich mean sidereal time + gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) - + 0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec; + double rest = gmst / 86400; + double FloorRest = floor(rest); + double secOfDay = rest - FloorRest; + secOfDay *= 86400; + gmst = secOfDay / 240 * PI / 180; + + outputDcmEJ[0] = cos(gmst); + outputDcmEJ[1] = sin(gmst); + outputDcmEJ[2] = 0; + outputDcmEJ[3] = -sin(gmst); + outputDcmEJ[4] = cos(gmst); + outputDcmEJ[5] = 0; + outputDcmEJ[6] = 0; + outputDcmEJ[7] = 0; + outputDcmEJ[8] = 1; + } + + /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame + * give also the back the derivative of this matrix + * @param: unixTime Current time in Unix format + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und + Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff + * Oliver Zeile + * + https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/ + static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + // TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 + //(initial Offset) International Atomic Time (TAI) + + double JD2000UTC1 = convertUnixToJD2000(unixTime); + + // Julian Date / century from TT + timeval terestrialTime = unixTime; + terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; + double JD2000TT = convertUnixToJD2000(terestrialTime); + double JC2000TT = JD2000TT / 36525; + + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth rotation Theta + //------------------------------------------------------------------------------------- + double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // Earth Rotation angle + double era = 0; + era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); + // Greenwich Mean Sidereal Time + double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) - + 0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4); + double arcsecFactor = 1 * PI / (180 * 3600); + gmst2000 *= arcsecFactor; + gmst2000 += era; + + theta[0][0] = cos(gmst2000); + theta[0][1] = sin(gmst2000); + theta[0][2] = 0; + theta[1][0] = -sin(gmst2000); + theta[1][1] = cos(gmst2000); + theta[1][2] = 0; + theta[2][0] = 0; + theta[2][1] = 0; + theta[2][2] = 1; + + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Precession P + //------------------------------------------------------------------------------------- + double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + + double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3); + double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3); + double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3); + + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; + + precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta); + precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta); + precession[2][0] = sin(theta2) * cos(zeta); + precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta); + precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta); + precession[2][1] = -sin(theta2) * sin(zeta); + precession[0][2] = -cos(ze) * sin(theta2); + precession[1][2] = -sin(ze) * sin(theta2); + precession[2][2] = cos(theta2); + + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Nutation size + //------------------------------------------------------------------------------------- + double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // lunar asc node + double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + + 7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3); + Om *= arcsecFactor; + // delta psi approx + double dp = -17.2 * arcsecFactor * sin(Om); + + // delta eps approx + double de = 9.203 * arcsecFactor * cos(Om); + + // % true obliquity of the ecliptic eps p.71 (simplified) + double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + ; + + nutation[0][0] = cos(dp); + nutation[1][0] = cos(e + de) * sin(dp); + nutation[2][0] = sin(e + de) * sin(dp); + nutation[0][1] = -cos(e) * sin(dp); + nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de); + nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de); + nutation[0][2] = -sin(e) * sin(dp); + nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de); + nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de); + + //------------------------------------------------------------------------------------- + // Calculation of Derivative of rotation matrix from earth + //------------------------------------------------------------------------------------- + double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); + + //------------------------------------------------------------------------------------- + // Calculation of transformation matrix and Derivative of transformation matrix + //------------------------------------------------------------------------------------- + double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); + MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); + + MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); + } + + static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { + int i, j; + double determinant; + double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, + {matrix[3], matrix[4], matrix[5]}, + {matrix[6], matrix[7], matrix[8]}}; + + for (i = 0; i < 3; i++) { + 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])); + } + // cout<<"\size\ndeterminant: "<::matrixDeterminant(*submatrix, size - 1)); + } + } + return det; + } + + static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { + if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { + return 1; // Matrix is singular and not invertible + } + T1 matrix[size][size], identity[size][size]; + // reformat array to matrix + for (uint8_t row = 0; row < size; row++) { + for (uint8_t col = 0; col < size; col++) { + matrix[row][col] = inputMatrix[row * size + col]; + } + } + // init identity matrix + std::memset(identity, 0.0, sizeof(identity)); + for (uint8_t diag = 0; diag < size; diag++) { + identity[diag][diag] = 1; + } + // gauss-jordan algo + // sort matrix such as no diag entry shall be 0 + // should not be needed as such a matrix has a det=0 + for (uint8_t row = 0; row < size; row++) { + if (matrix[row][row] == 0.0) { + bool swaped = false; + uint8_t rowIndex = 0; + while ((rowIndex < size) && !swaped) { + if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) { + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + swaped = true; + } + rowIndex++; + } + if (!swaped) { + return 1; // matrix not invertible + } + } + } + + for (int row = 0; row < size; row++) { + if (matrix[row][row] == 0.0) { + uint8_t rowIndex; + if (row == 0) { + rowIndex = size - 1; + } else { + rowIndex = row - 1; + } + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + row--; + if (row < 0) { + return 1; // Matrix is not invertible + } + } + } + // remove non diag elements in matrix (jordan) + for (int row = 0; row < size; row++) { + for (int rowIndex = 0; rowIndex < size; rowIndex++) { + if (row != rowIndex) { + double ratio = matrix[rowIndex][row] / matrix[row][row]; + for (int colIndex = 0; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex]; + identity[rowIndex][colIndex] -= ratio * identity[row][colIndex]; + } + } + } + } + // normalize rows in matrix (gauss) + for (int row = 0; row < size; row++) { + for (int col = 0; col < size; col++) { + identity[row][col] = identity[row][col] / matrix[row][row]; + } + } + std::memcpy(inverse, identity, sizeof(identity)); + return 0; // successful inversion + } +}; + +#endif /* ACS_MATH_MATHOPERATIONS_H_ */ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 9a2b1abd..2652ca3c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -8,16 +8,37 @@ namespace acsctrl { -enum SetIds : uint32_t { MGM_SENSOR_DATA, SUS_SENSOR_DATA }; +enum SetIds : uint32_t { + MGM_SENSOR_DATA, + MGM_PROCESSED_DATA, + SUS_SENSOR_DATA, + SUS_PROCESSED_DATA, + GYR_SENSOR_DATA, + GYR_PROCESSED_DATA, + GPS_PROCESSED_DATA, + MEKF_DATA, + CTRL_VAL_DATA, + ACTUATOR_CMD_DATA +}; enum PoolIds : lp_id_t { + // MGM Raw MGM_0_LIS3_UT, MGM_1_RM3100_UT, MGM_2_LIS3_UT, MGM_3_RM3100_UT, MGM_IMTQ_CAL_NT, MGM_IMTQ_CAL_ACT_STATUS, - + // MGM Processed + MGM_0_VEC, + MGM_1_VEC, + MGM_2_VEC, + MGM_3_VEC, + MGM_4_VEC, + MGM_VEC_TOT, + MGM_VEC_TOT_DERIVATIVE, + MAG_IGRF_MODEL, + // SUS Raw SUS_0_N_LOC_XFYFZM_PT_XF, SUS_6_R_LOC_XFYBZM_PT_XF, @@ -35,15 +56,64 @@ enum PoolIds : lp_id_t { SUS_5_N_LOC_XFYMZB_PT_ZB, SUS_11_R_LOC_XBYMZB_PT_ZB, + // SUS Processed + SUS_0_VEC, + SUS_1_VEC, + SUS_2_VEC, + SUS_3_VEC, + SUS_4_VEC, + SUS_5_VEC, + SUS_6_VEC, + SUS_7_VEC, + SUS_8_VEC, + SUS_9_VEC, + SUS_10_VEC, + SUS_11_VEC, + SUS_VEC_TOT, + SUS_VEC_TOT_DERIVATIVE, + SUN_IJK_MODEL, + // GYR Raw + GYR_0_ADIS, + GYR_1_L3, + GYR_2_ADIS, + GYR_3_L3, + // GYR Processed + GYR_0_VEC, + GYR_1_VEC, + GYR_2_VEC, + GYR_3_VEC, + GYR_VEC_TOT, + // GPS Processed + GC_LATITUDE, + GD_LONGITUDE, + // MEKF + SAT_ROT_RATE_MEKF, + QUAT_MEKF, + // Ctrl Values + TGT_QUAT, + ERROR_QUAT, + ERROR_ANG, + // Actuator Cmd + RW_TARGET_TORQUE, + RW_TARGET_SPEED, + MTQ_TARGET_DIPOLE, }; -static constexpr uint8_t MGM_SET_ENTRIES = 10; -static constexpr uint8_t SUS_SET_ENTRIES = 12; +static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10; +static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8; +static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; +static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; +static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; +static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2; +static constexpr uint8_t MEKF_SET_ENTRIES = 2; +static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; +static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. */ -class MgmDataRaw : public StaticLocalDataSet { +class MgmDataRaw : public StaticLocalDataSet { public: MgmDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_SENSOR_DATA) {} @@ -60,9 +130,133 @@ class MgmDataRaw : public StaticLocalDataSet { private: }; -class SusData : public StaticLocalDataSet { +class MgmDataProcessed : public StaticLocalDataSet { public: - SusData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {} + MgmDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_PROCESSED_DATA) {} + + lp_vec_t mgm0vec = lp_vec_t(sid.objectId, MGM_0_VEC, this); + lp_vec_t mgm1vec = lp_vec_t(sid.objectId, MGM_1_VEC, this); + lp_vec_t mgm2vec = lp_vec_t(sid.objectId, MGM_2_VEC, this); + lp_vec_t mgm3vec = lp_vec_t(sid.objectId, MGM_3_VEC, this); + lp_vec_t mgm4vec = lp_vec_t(sid.objectId, MGM_4_VEC, this); + lp_vec_t mgmVecTot = lp_vec_t(sid.objectId, MGM_VEC_TOT, this); + lp_vec_t mgmVecTotDerivative = + lp_vec_t(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this); + lp_vec_t magIgrfModel = lp_vec_t(sid.objectId, MAG_IGRF_MODEL, this); + + private: +}; + +class SusDataRaw : public StaticLocalDataSet { + public: + SusDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {} + + lp_vec_t sus0 = lp_vec_t(sid.objectId, SUS_0_N_LOC_XFYFZM_PT_XF, this); + lp_vec_t sus1 = lp_vec_t(sid.objectId, SUS_1_N_LOC_XBYFZM_PT_XB, this); + lp_vec_t sus2 = lp_vec_t(sid.objectId, SUS_2_N_LOC_XFYBZB_PT_YB, this); + lp_vec_t sus3 = lp_vec_t(sid.objectId, SUS_3_N_LOC_XFYBZF_PT_YF, this); + lp_vec_t sus4 = lp_vec_t(sid.objectId, SUS_4_N_LOC_XMYFZF_PT_ZF, this); + lp_vec_t sus5 = lp_vec_t(sid.objectId, SUS_5_N_LOC_XFYMZB_PT_ZB, this); + lp_vec_t sus6 = lp_vec_t(sid.objectId, SUS_6_R_LOC_XFYBZM_PT_XF, this); + lp_vec_t sus7 = lp_vec_t(sid.objectId, SUS_7_R_LOC_XBYBZM_PT_XB, this); + lp_vec_t sus8 = lp_vec_t(sid.objectId, SUS_8_R_LOC_XBYBZB_PT_YB, this); + lp_vec_t sus9 = lp_vec_t(sid.objectId, SUS_9_R_LOC_XBYBZB_PT_YF, this); + lp_vec_t sus10 = + lp_vec_t(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this); + lp_vec_t sus11 = + lp_vec_t(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this); + + private: +}; + +class SusDataProcessed : public StaticLocalDataSet { + public: + SusDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_PROCESSED_DATA) {} + + lp_vec_t sus0vec = lp_vec_t(sid.objectId, SUS_0_VEC, this); + lp_vec_t sus1vec = lp_vec_t(sid.objectId, SUS_1_VEC, this); + lp_vec_t sus2vec = lp_vec_t(sid.objectId, SUS_2_VEC, this); + lp_vec_t sus3vec = lp_vec_t(sid.objectId, SUS_3_VEC, this); + lp_vec_t sus4vec = lp_vec_t(sid.objectId, SUS_4_VEC, this); + lp_vec_t sus5vec = lp_vec_t(sid.objectId, SUS_5_VEC, this); + lp_vec_t sus6vec = lp_vec_t(sid.objectId, SUS_6_VEC, this); + lp_vec_t sus7vec = lp_vec_t(sid.objectId, SUS_7_VEC, this); + lp_vec_t sus8vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); + lp_vec_t sus9vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); + lp_vec_t sus10vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); + lp_vec_t sus11vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); + lp_vec_t susVecTot = lp_vec_t(sid.objectId, SUS_VEC_TOT, this); + lp_vec_t susVecTotDerivative = + lp_vec_t(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this); + lp_vec_t sunIjkModel = lp_vec_t(sid.objectId, SUN_IJK_MODEL, this); + + private: +}; + +class GyrDataRaw : public StaticLocalDataSet { + public: + GyrDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_SENSOR_DATA) {} + + lp_vec_t gyr0Adis = lp_vec_t(sid.objectId, GYR_0_ADIS, this); + lp_vec_t gyr1L3 = lp_vec_t(sid.objectId, GYR_1_L3, this); + lp_vec_t gyr2Adis = lp_vec_t(sid.objectId, GYR_2_ADIS, this); + lp_vec_t gyr3L3 = lp_vec_t(sid.objectId, GYR_3_L3, this); + + private: +}; + +class GyrDataProcessed : public StaticLocalDataSet { + public: + GyrDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_PROCESSED_DATA) {} + + lp_vec_t gyr0vec = lp_vec_t(sid.objectId, GYR_0_VEC, this); + lp_vec_t gyr1vec = lp_vec_t(sid.objectId, GYR_1_VEC, this); + lp_vec_t gyr2vec = lp_vec_t(sid.objectId, GYR_2_VEC, this); + lp_vec_t gyr3vec = lp_vec_t(sid.objectId, GYR_3_VEC, this); + lp_vec_t gyrVecTot = lp_vec_t(sid.objectId, GYR_VEC_TOT, this); + + private: +}; + +class GpsDataProcessed : public StaticLocalDataSet { + public: + GpsDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GPS_PROCESSED_DATA) {} + + lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); + lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + + private: +}; + +class MekfData : public StaticLocalDataSet { + public: + MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} + + lp_vec_t quatMekf = lp_vec_t(sid.objectId, QUAT_MEKF, this); + lp_vec_t satRotRateMekf = lp_vec_t(sid.objectId, SAT_ROT_RATE_MEKF, this); + + private: +}; + +class CtrlValData : public StaticLocalDataSet { + public: + CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {} + + lp_vec_t tgtQuat = lp_vec_t(sid.objectId, TGT_QUAT, this); + lp_vec_t errQuat = lp_vec_t(sid.objectId, ERROR_QUAT, this); + lp_var_t errAng = lp_var_t(sid.objectId, ERROR_ANG, this); + + private: +}; + +class ActuatorCmdData : public StaticLocalDataSet { + public: + ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} + + lp_vec_t rwTargetTorque = lp_vec_t(sid.objectId, RW_TARGET_TORQUE, this); + lp_vec_t rwTargetSpeed = lp_vec_t(sid.objectId, RW_TARGET_SPEED, this); + lp_vec_t mtqTargetDipole = + lp_vec_t(sid.objectId, MTQ_TARGET_DIPOLE, this); private: }; diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index e842f54b..7653745f 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -3,6 +3,7 @@ #include "fsfw/datapoollocal/StaticLocalDataSet.h" #include "fsfw/devicehandlers/DeviceHandlerIF.h" +#include "eive/eventSubsystemIds.h" namespace GpsHyperion { diff --git a/unittest/controller/testThermalController.cpp b/unittest/controller/testThermalController.cpp index 3fac0f4e..1c5f63fe 100644 --- a/unittest/controller/testThermalController.cpp +++ b/unittest/controller/testThermalController.cpp @@ -13,7 +13,7 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { const object_id_t THERMAL_CONTROLLER_ID = 0x123; new TemperatureSensorsDummy(); - new SusDummy(); + //new SusDummy(); // testEnvironment::initialize();