From fc1dedeb7a453a9dc7883cd8ac29e277e7422c72 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 2 Feb 2023 15:33:04 +0100 Subject: [PATCH 01/60] moved transmitter timer and event handling of carrier and bitlock to ComSubsystem --- bsp_q7s/core/ObjectFactory.cpp | 9 +- mission/system/objects/ComSubsystem.cpp | 115 +++++++++++++++++++++++- mission/system/objects/ComSubsystem.h | 37 +++++++- mission/system/tree/comModeTree.cpp | 5 +- mission/system/tree/comModeTree.h | 5 ++ mission/tmtc/CcsdsIpCoreHandler.cpp | 105 +--------------------- mission/tmtc/CcsdsIpCoreHandler.h | 26 +----- 7 files changed, 163 insertions(+), 139 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 847cce5f..1fb9efed 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -783,15 +783,10 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, AxiPtmeConfig* axiPtmeConfig = new AxiPtmeConfig(objects::AXI_PTME_CONFIG, q7s::UIO_PTME, q7s::uiomapids::PTME_CONFIG); PtmeConfig* ptmeConfig = new PtmeConfig(objects::PTME_CONFIG, axiPtmeConfig); -#if OBSW_ENABLE_SYRLINKS_TRANSMIT_TIMEOUT == 1 - // Set to high value when not sending via syrlinks - static const uint32_t TRANSMITTER_TIMEOUT = 86400000; // 1 day -#else - static const uint32_t TRANSMITTER_TIMEOUT = 900000; // 15 minutes -#endif + *ipCoreHandler = new CcsdsIpCoreHandler( objects::CCSDS_HANDLER, objects::PTME, objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, - gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA, TRANSMITTER_TIMEOUT); + gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA); VirtualChannel* vc = nullptr; vc = new VirtualChannel(ccsds::VC0, config::VC0_QUEUE_SIZE, objects::CCSDS_HANDLER); (*ipCoreHandler)->addVirtualChannel(ccsds::VC0, vc); diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index 426e9c6c..5c98dfd8 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -1,13 +1,32 @@ +#include + #include "ComSubsystem.h" +#include #include +#include +#include +#include +#include -#include "mission/config/comCfg.h" +extern std::pair> COM_SEQUENCE_RX_ONLY; +extern std::pair> COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE; +extern std::pair> COM_SEQUENCE_RX_AND_TX_LOW_RATE; +extern std::pair> COM_SEQUENCE_RX_AND_TX_HIGH_RATE; ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, - uint32_t maxNumberOfTables) + uint32_t maxNumberOfTables, uint32_t transmitterTimeout) : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) { com::setCurrentDatarate(com::Datarate::LOW_RATE_MODULATION_BPSK); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + eventQueue = + QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); + transmitterCountdown.setTimeout(transmitterTimeout); +} + +void ComSubsystem::performChildOperation() { + readEventQueue(); + checkTransmitterCountdown(); } MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); } @@ -44,5 +63,97 @@ ReturnValue_t ComSubsystem::initialize() { if (result != returnvalue::OK) { return result; } + EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); + if (manager == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl; +#endif + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->registerListener(eventQueue->getId()); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event " + "listener" << std::endl; +#endif + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEventRange(eventQueue->getId(), + event::getEventId(PdecHandler::CARRIER_LOCK), + event::getEventId(PdecHandler::BIT_LOCK_PDEC)); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC " + "handler" + << std::endl; +#endif + return result; + } return Subsystem::initialize(); } + +void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) { + // Depending on the mode the transmitter timeout is enabled or + // disabled here + if (mode == COM_SEQUENCE_RX_ONLY.first) { + transmitterCountdown.timeOut(); + } else if ((mode == COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first) || + (mode == COM_SEQUENCE_RX_AND_TX_LOW_RATE.first) || + (mode == COM_SEQUENCE_RX_AND_TX_HIGH_RATE.first)) { + transmitterCountdown.resetTimer(); + } + Subsystem::startTransition(mode, submode); +} + +void ComSubsystem::readEventQueue() { + EventMessage event; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; + result = eventQueue->receiveMessage(&event)) { + switch (event.getMessageId()) { + case EventMessage::EVENT_MESSAGE: + handleEventMessage(&event); + break; + default: + sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message" + << std::endl; + break; + } + } +} + +void ComSubsystem::handleEventMessage(EventMessage *eventMessage) { + Event event = eventMessage->getEvent(); + switch (event) { + case PdecHandler::BIT_LOCK_PDEC: { + handleBitLockEvent(); + break; + } + case PdecHandler::CARRIER_LOCK: { + handleCarrierLockEvent(); + break; + } + default: + sif::debug << "ComSubsystem::handleEvent: Did not subscribe to this event" << std::endl; + break; + } +} + +void ComSubsystem::handleBitLockEvent() { startRxAndTxDefaultSeq(); } + +void ComSubsystem::handleCarrierLockEvent() { + if (!enableTxWhenCarrierLock) { + return; + } + startRxAndTxDefaultSeq(); +} + +void ComSubsystem::startRxAndTxDefaultSeq() { + // Turns transmitter on + startTransition(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first, SUBMODE_NONE); +} + +void ComSubsystem::checkTransmitterCountdown() { + if (transmitterCountdown.hasTimedOut()) { + startTransition(COM_SEQUENCE_RX_ONLY.first, SUBMODE_NONE); + } +} diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 74f0c30e..3f1ef5d2 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -4,26 +4,61 @@ #include #include #include +#include #include "mission/comDefs.h" class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { public: - ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); + /** + * @brief Constructor + * + * @param setObjectId + * @param maxNumberOfSequences + * @param maxNumberOfTables + * @param transmitterTimeout Maximum time the transmitter of the syrlinks + * will be enabled + */ + ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, + uint32_t transmitterTimeout); virtual ~ComSubsystem() = default; MessageQueueId_t getCommandQueue() const override; ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, uint16_t startAtIndex) override; + virtual void performChildOperation() override; private: ReturnValue_t handleCommandMessage(CommandMessage *message) override; ReturnValue_t initialize() override; + void startTransition(Mode_t mode, Submode_t submode) override; + + void readEventQueue(); + void handleEventMessage(EventMessage* eventMessage); + void handleBitLockEvent(); + void handleCarrierLockEvent(); + void checkTransmitterCountdown(); + /** + * @brief Enables transmitter in default (low) rate mode + */ + void startRxAndTxDefaultSeq(); + uint8_t datarateCfg = static_cast(com::Datarate::LOW_RATE_MODULATION_BPSK); ParameterHelper paramHelper; + + MessageQueueIF* eventQueue = nullptr; + + bool enableTxWhenCarrierLock = false; + + // Maximum time after which the transmitter will be turned of. This is a + // protection mechanism due prevent the syrlinks from overheating + uint32_t transmitterTimeout = 0; + + // Countdown will be started as soon as the transmitter was enabled + Countdown transmitterCountdown; }; #endif /* MISSION_SYSTEM_COMSUBSYSTEM_H_ */ diff --git a/mission/system/tree/comModeTree.cpp b/mission/system/tree/comModeTree.cpp index e08ba5ba..d0811dd7 100644 --- a/mission/system/tree/comModeTree.cpp +++ b/mission/system/tree/comModeTree.cpp @@ -1,6 +1,5 @@ #include "comModeTree.h" -#include #include #include #include @@ -11,7 +10,7 @@ const auto check = subsystem::checkInsert; -ComSubsystem satsystem::com::SUBSYSTEM = ComSubsystem(objects::COM_SUBSYSTEM, 12, 24); +ComSubsystem satsystem::com::SUBSYSTEM = ComSubsystem(objects::COM_SUBSYSTEM, 12, 24, TRANSMITTER_TIMEOUT); static const auto OFF = HasModesIF::MODE_OFF; static const auto ON = HasModesIF::MODE_ON; @@ -94,7 +93,7 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) { // Build RX Only table. We could track the state of the CCSDS IP core handler // as well but I do not think this is necessary because enabling that should - // not intefere with the Syrlinks Handler. + // not interfere with the Syrlinks Handler. iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second); check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc); diff --git a/mission/system/tree/comModeTree.h b/mission/system/tree/comModeTree.h index ccc7f15f..587cc8ed 100644 --- a/mission/system/tree/comModeTree.h +++ b/mission/system/tree/comModeTree.h @@ -2,12 +2,17 @@ #define MISSION_SYSTEM_TREE_COMMODETREE_H_ #include +#include namespace satsystem { namespace com { extern ComSubsystem SUBSYSTEM; +// The syrlinks must not transmitting longer then 15 minutes otherwise the +// transceiver might be damaged due to overheating +static const uint32_t TRANSMITTER_TIMEOUT = 900000; // 15 minutes + void init(); } // namespace com diff --git a/mission/tmtc/CcsdsIpCoreHandler.cpp b/mission/tmtc/CcsdsIpCoreHandler.cpp index 85d1143b..0b6a210d 100644 --- a/mission/tmtc/CcsdsIpCoreHandler.cpp +++ b/mission/tmtc/CcsdsIpCoreHandler.cpp @@ -1,12 +1,10 @@ #include "CcsdsIpCoreHandler.h" #include -#include #include #include #include "eive/definitions.h" -#include "fsfw/events/EventManagerIF.h" #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serialize/SerializeAdapter.h" @@ -16,8 +14,7 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination, PtmeConfig* ptmeConfig, - GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData, - uint32_t transmitterTimeout) + GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData) : SystemObject(objectId), ptmeId(ptmeId), tcDestination(tcDestination), @@ -27,8 +24,7 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, ptmeConfig(ptmeConfig), gpioIF(gpioIF), enTxClock(enTxClock), - enTxData(enTxData), - transmitterTimeout(transmitterTimeout) { + enTxData(enTxData) { commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE); auto mqArgs = MqArgs(objectId, static_cast(this)); eventQueue = @@ -38,11 +34,9 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {} ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) { - checkEvents(); readCommandQueue(); handleTelemetry(); handleTelecommands(); - checkTxTimer(); return returnvalue::OK; } @@ -98,34 +92,6 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() { iter->second->setPtmeObject(ptme); } - EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); - if (manager == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CcsdsHandler::initialize: Invalid event manager" << std::endl; -#endif - return ObjectManagerIF::CHILD_INIT_FAILED; - } - result = manager->registerListener(eventQueue->getId()); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "CcsdsHandler::initialize: Failed to register CCSDS handler as event " - "listener" - << std::endl; -#endif - return ObjectManagerIF::CHILD_INIT_FAILED; - ; - } - result = manager->subscribeToEventRange(eventQueue->getId(), - event::getEventId(PdecHandler::CARRIER_LOCK), - event::getEventId(PdecHandler::BIT_LOCK_PDEC)); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CcsdsHandler::initialize: Failed to subscribe to events from PDEC " - "handler" - << std::endl; -#endif - return result; - } result = ptmeConfig->initialize(); if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; @@ -135,9 +101,6 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } - transmitterCountdown.setTimeout(transmitterTimeout); - transmitterCountdown.timeOut(); - #if OBSW_SYRLINKS_SIMULATED == 1 // Update data on rising edge ptmeConfig->invertTxClock(false); @@ -286,54 +249,6 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } -void CcsdsIpCoreHandler::checkEvents() { - EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; - result = eventQueue->receiveMessage(&event)) { - switch (event.getMessageId()) { - case EventMessage::EVENT_MESSAGE: - handleEvent(&event); - break; - default: - sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message" - << std::endl; - break; - } - } -} - -void CcsdsIpCoreHandler::handleEvent(EventMessage* eventMessage) { - Event event = eventMessage->getEvent(); - switch (event) { - case PdecHandler::BIT_LOCK_PDEC: { - handleBitLockEvent(); - break; - } - case PdecHandler::CARRIER_LOCK: { - handleCarrierLockEvent(); - break; - } - default: - sif::debug << "CcsdsHandler::handleEvent: Did not subscribe to this event" << std::endl; - break; - } -} - -void CcsdsIpCoreHandler::handleBitLockEvent() { - if (transmitterCountdown.isBusy()) { - // Transmitter already enabled - return; - } - enableTransmit(); -} - -void CcsdsIpCoreHandler::handleCarrierLockEvent() { - if (!enableTxWhenCarrierLock) { - return; - } - enableTransmit(); -} - void CcsdsIpCoreHandler::forwardLinkstate() { VirtualChannelMapIter iter; for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { @@ -342,27 +257,12 @@ void CcsdsIpCoreHandler::forwardLinkstate() { } void CcsdsIpCoreHandler::enableTransmit() { - if (transmitterCountdown.isBusy()) { - // Transmitter already enabled - return; - } #ifndef TE0720_1CFA gpioIF->pullHigh(enTxClock); gpioIF->pullHigh(enTxData); #endif linkState = UP; forwardLinkstate(); - transmitterCountdown.resetTimer(); -} - -void CcsdsIpCoreHandler::checkTxTimer() { - if (linkState == DOWN) { - return; - } - if (transmitterCountdown.hasTimedOut()) { - disableTransmit(); - //TODO: set mode to off (move timer to subsystem) - } } void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) { @@ -431,7 +331,6 @@ void CcsdsIpCoreHandler::disableTransmit() { #endif linkState = DOWN; forwardLinkstate(); - transmitterCountdown.timeOut(); } const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; } diff --git a/mission/tmtc/CcsdsIpCoreHandler.h b/mission/tmtc/CcsdsIpCoreHandler.h index 37212851..356c2f8d 100644 --- a/mission/tmtc/CcsdsIpCoreHandler.h +++ b/mission/tmtc/CcsdsIpCoreHandler.h @@ -59,8 +59,7 @@ class CcsdsIpCoreHandler : public SystemObject, * @param enTxData GPIO ID of RS485 tx data enable */ CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination, - PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData, - uint32_t transmitterTimeout = 900000); + PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData); ~CcsdsIpCoreHandler(); @@ -154,29 +153,16 @@ class CcsdsIpCoreHandler : public SystemObject, PtmeConfig* ptmeConfig = nullptr; GpioIF* gpioIF = nullptr; + // GPIO to enable RS485 transceiver for TX clock gpioId_t enTxClock = gpio::NO_GPIO; + // GPIO to enable RS485 transceiver for TX data signal gpioId_t enTxData = gpio::NO_GPIO; - // Syrlinks must not be transmitting more than 15 minutes (according to datasheet) - // Value initialized by constructor argument - const uint32_t transmitterTimeout = 0; - - // Countdown to disable transmitter after 15 minutes - Countdown transmitterCountdown; - - // When true transmitting is started as soon as carrier lock has been detected - bool enableTxWhenCarrierLock = false; - bool linkState = DOWN; void readCommandQueue(void); void handleTelemetry(); void handleTelecommands(); - void checkEvents(); - void handleEvent(EventMessage* eventMessage); - - void handleBitLockEvent(); - void handleCarrierLockEvent(); /** * @brief Forward link state to virtual channels. @@ -188,12 +174,6 @@ class CcsdsIpCoreHandler : public SystemObject, */ void enableTransmit(); - /** - * @brief Checks Tx timer for timeout and disables RS485 tx clock and tx data in case - * timer has expired. - */ - void checkTxTimer(); - /** * @brief Disables the transmitter by pulling the enable tx clock and tx data pin of the * RS485 transceiver chips to high. From c0f936b757272356d9c375ce40dd2e48dbbea257 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 2 Feb 2023 16:00:53 +0100 Subject: [PATCH 02/60] add parameter command to change the transmitter timeout --- mission/comDefs.h | 2 +- mission/system/objects/ComSubsystem.cpp | 10 ++++++++++ mission/system/objects/ComSubsystem.h | 7 +++---- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/mission/comDefs.h b/mission/comDefs.h index 5538b166..8bb3f329 100644 --- a/mission/comDefs.h +++ b/mission/comDefs.h @@ -24,7 +24,7 @@ enum class CcsdsSubmode : uint8_t { DATARATE_HIGH = 2, DATARATE_DEFAULT = 3 }; -enum class ParameterId : uint8_t { DATARATE = 0 }; +enum class ParameterId : uint8_t { DATARATE = 0, TRANSMITTER_TIMEOUT = 1 }; } // namespace com diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index 5c98dfd8..dc58a7a5 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -47,6 +47,16 @@ ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentif com::setCurrentDatarate(static_cast(newVal)); return returnvalue::OK; } + else if ((domainId == 0) and (uniqueIdentifier == static_cast(com::ParameterId::TRANSMITTER_TIMEOUT))) { + uint8_t newVal = 0; + ReturnValue_t result = newValues->getElement(&newVal); + if (result != returnvalue::OK) { + return result; + } + parameterWrapper->set(transmitterTimeout); + transmitterTimeout = newVal; + return returnvalue::OK; + } return returnvalue::OK; } diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 3f1ef5d2..ac8cc60f 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -47,16 +47,15 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { void startRxAndTxDefaultSeq(); uint8_t datarateCfg = static_cast(com::Datarate::LOW_RATE_MODULATION_BPSK); + // Maximum time after which the transmitter will be turned of. This is a + // protection mechanism due prevent the syrlinks from overheating + uint32_t transmitterTimeout = 0; ParameterHelper paramHelper; MessageQueueIF* eventQueue = nullptr; bool enableTxWhenCarrierLock = false; - // Maximum time after which the transmitter will be turned of. This is a - // protection mechanism due prevent the syrlinks from overheating - uint32_t transmitterTimeout = 0; - // Countdown will be started as soon as the transmitter was enabled Countdown transmitterCountdown; }; From 3a69bc705ae27976d412ba36213426827ed8a067 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 2 Feb 2023 16:02:09 +0100 Subject: [PATCH 03/60] tmtc update --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index c633893d..a3a3aaa8 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit c633893df43030bb26b8422604b569bf8419d454 +Subproject commit a3a3aaa8836b425c923eb97e49ed29b452377bf6 From 305f8aa5613070bbf1f8c02ba2c57d9749587bac Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 3 Feb 2023 10:07:53 +0100 Subject: [PATCH 04/60] update transmitter countdown when parameter command is received --- mission/system/objects/ComSubsystem.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index dc58a7a5..bbe14af0 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -55,6 +55,7 @@ ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentif } parameterWrapper->set(transmitterTimeout); transmitterTimeout = newVal; + transmitterCountdown.setTimeout(transmitterTimeout); return returnvalue::OK; } return returnvalue::OK; From e5271a9ca5bff379ba2e61b9b88a4be8252ef1de Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 16 Feb 2023 13:40:15 +0100 Subject: [PATCH 05/60] cast of submodes --- mission/system/objects/ComSubsystem.cpp | 8 ++++++-- mission/system/tree/comModeTree.cpp | 24 ++++++++++++------------ 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index bbe14af0..8e3f253e 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -10,7 +10,9 @@ #include extern std::pair> COM_SEQUENCE_RX_ONLY; +//extern std::pair> COM_TABLE_RX_ONLY_TGT; extern std::pair> COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE; +extern std::pair> COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT; extern std::pair> COM_SEQUENCE_RX_AND_TX_LOW_RATE; extern std::pair> COM_SEQUENCE_RX_AND_TX_HIGH_RATE; @@ -26,7 +28,9 @@ ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequence void ComSubsystem::performChildOperation() { readEventQueue(); - checkTransmitterCountdown(); + if (mode != COM_SEQUENCE_RX_ONLY.first) { + checkTransmitterCountdown(); + } } MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); } @@ -104,7 +108,7 @@ ReturnValue_t ComSubsystem::initialize() { } void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) { - // Depending on the mode the transmitter timeout is enabled or + // Depending on the submode the transmitter timeout is enabled or // disabled here if (mode == COM_SEQUENCE_RX_ONLY.first) { transmitterCountdown.timeOut(); diff --git a/mission/system/tree/comModeTree.cpp b/mission/system/tree/comModeTree.cpp index 22f51379..7e771892 100644 --- a/mission/system/tree/comModeTree.cpp +++ b/mission/system/tree/comModeTree.cpp @@ -19,38 +19,38 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL; auto COM_SEQUENCE_RX_ONLY = std::make_pair(::com::Submode::RX_ONLY, FixedArrayList()); auto COM_TABLE_RX_ONLY_TGT = - std::make_pair((::com::Submode::RX_ONLY << 24) | 1, FixedArrayList()); + std::make_pair(static_cast(::com::Submode::RX_ONLY << 24) | 1, FixedArrayList()); auto COM_TABLE_RX_ONLY_TRANS_0 = - std::make_pair((::com::Submode::RX_ONLY << 24) | 2, FixedArrayList()); + std::make_pair(static_cast(::com::Submode::RX_ONLY << 24) | 2, FixedArrayList()); auto COM_TABLE_RX_ONLY_TRANS_1 = - std::make_pair((::com::Submode::RX_ONLY << 24) | 3, FixedArrayList()); + std::make_pair(static_cast(::com::Submode::RX_ONLY << 24) | 3, FixedArrayList()); auto COM_SEQUENCE_RX_AND_TX_LOW_RATE = std::make_pair(::com::Submode::RX_AND_TX_LOW_DATARATE, FixedArrayList()); auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT = std::make_pair( - (::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1, FixedArrayList()); auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 = std::make_pair( - (::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2, FixedArrayList()); auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 = std::make_pair( - (::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3, FixedArrayList()); auto COM_SEQUENCE_RX_AND_TX_HIGH_RATE = std::make_pair(::com::Submode::RX_AND_TX_HIGH_DATARATE, FixedArrayList()); auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT = std::make_pair( - (::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1, FixedArrayList()); auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 = std::make_pair( - (::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2, FixedArrayList()); auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 = std::make_pair( - (::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3, FixedArrayList()); auto COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE = std::make_pair(::com::Submode::RX_AND_TX_DEFAULT_DATARATE, FixedArrayList()); auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT = std::make_pair( - (::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1, FixedArrayList()); auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 = std::make_pair( - (::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2, FixedArrayList()); auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 = std::make_pair( - (::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3, FixedArrayList()); + static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3, FixedArrayList()); namespace { From 1518fba6bd1f5c33a958d3e1d1fab7aca2ec4896 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 11:11:48 +0100 Subject: [PATCH 06/60] moved actuatorCmdData to function --- mission/controller/AcsController.cpp | 74 +++++++++++----------------- mission/controller/AcsController.h | 3 ++ 2 files changed, 33 insertions(+), 44 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 74fce754..2403650c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -28,6 +28,14 @@ AcsController::AcsController(object_id_t objectId) ctrlValData(this), actuatorCmdData(this) {} +ReturnValue_t AcsController::initialize() { + ReturnValue_t result = parameterHelper.initialize(); + if (result != returnvalue::OK) { + return result; + } + return ExtendedControllerBase::initialize(); +} + ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { @@ -176,20 +184,7 @@ void AcsController::performSafe() { triggerEvent(acs::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, cmdDipolMtqs, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - + updateActuatorCmdData(cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); @@ -229,19 +224,7 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - + updateActuatorCmdData(nullptr, nullptr, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); @@ -406,16 +389,7 @@ void AcsController::performPointingCtrl() { int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } - } - + updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], @@ -451,6 +425,25 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, return returnvalue::OK; } +void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) { + double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; + int32_t rwTargetSpeed[4] = {0, 0, 0, 0}; + updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole); +} + +void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], + int16_t mtqTargetDipole[3]) { + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); + } + } +} + ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { // MGM Raw @@ -796,10 +789,3 @@ void AcsController::copyGyrData() { } } -ReturnValue_t AcsController::initialize() { - ReturnValue_t result = parameterHelper.initialize(); - if (result != returnvalue::OK) { - return result; - } - return ExtendedControllerBase::initialize(); -} diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e519d2e8..9bb5c4fd 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -79,6 +79,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); + void updateActuatorCmdData(int16_t mtqTargetDipole[3]); + void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], + int16_t mtqTargetDipole[3]); /* ACS Sensor Values */ ACS::SensorValues sensorValues; From 2f7abb2da833f1bc55958f25d9b598b20229088f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 11:39:18 +0100 Subject: [PATCH 07/60] stuff --- mission/controller/acs/Guidance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index d7db0538..f9238755 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -417,12 +417,12 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me // Assign helper vector (north pole inertial) double helperVec[3] = {0, 0, 1}; - // + // Construct y-axis from helper vector and z-axis double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // + // Construct x-axis from y- and z-axes double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); From d709e190e25769e967bdecba907f06e1b185069f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 13:28:29 +0100 Subject: [PATCH 08/60] added function for updating ctrlValData --- mission/controller/AcsController.cpp | 46 ++++++++++++++++++---------- mission/controller/AcsController.h | 2 ++ 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2403650c..f1bef501 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -152,20 +152,6 @@ void AcsController::performSafe() { int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0, 0, 0, 1}; - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 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) > @@ -184,6 +170,7 @@ void AcsController::performSafe() { triggerEvent(acs::SAFE_RATE_VIOLATION); } + updateCtrlValData(errAng); updateActuatorCmdData(cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, @@ -224,7 +211,7 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } - updateActuatorCmdData(nullptr, nullptr, cmdDipolMtqs); + updateActuatorCmdData(cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); @@ -283,7 +270,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - break; case acs::PTG_TARGET: @@ -444,6 +430,33 @@ void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTa } } +void AcsController::updateCtrlValData(double errAng) { + double unitQuat[4] = {0, 0, 0, 1}; + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); + } + } +} +void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) { + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); + ctrlValData.errAng.value = errAng; + ctrlValData.setValidity(true, true); + } + } +} + ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { // MGM Raw @@ -788,4 +801,3 @@ void AcsController::copyGyrData() { } } } - diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 9bb5c4fd..e74681d4 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -82,6 +82,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void updateActuatorCmdData(int16_t mtqTargetDipole[3]); void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], int16_t mtqTargetDipole[3]); + void updateCtrlValData(double errAng); + void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng); /* ACS Sensor Values */ ACS::SensorValues sensorValues; From f0d77125e16dbe74c702ecca2d7d8d8bf1cdce44 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 13:44:44 +0100 Subject: [PATCH 09/60] broke everything to fix everything --- mission/controller/acs/Guidance.cpp | 65 +++++------------------------ 1 file changed, 11 insertions(+), 54 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f9238755..506f6dfc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -36,9 +36,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 3 * sizeof(double)); } -void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, +void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude @@ -52,13 +52,6 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - // Target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); @@ -108,8 +101,6 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- - double velSatE[3] = {0, 0, 0}; - std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); 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); @@ -221,9 +212,7 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn savedQuaternion[3] = quatInertialTarget[3]; } -void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, +void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing @@ -231,14 +220,10 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double targetCart[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double)); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); @@ -300,10 +285,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -315,12 +297,6 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); @@ -379,10 +355,7 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::sunQuatPtg(timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -448,18 +421,11 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me refSatRate[2] = 0; } -void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - timeval now, double targetQuat[4], +void Guidance::quatNadirPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); @@ -512,19 +478,11 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:: refSatRate[2] = 0; } -void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]) { +void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); @@ -548,7 +506,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // z-Axis parallel to long side of picture resolution - double zAxis[3] = {0, 0, 0}, velocityE[3]; + double zAxis[3] = {0, 0, 0}; std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); @@ -584,9 +542,8 @@ void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { 3 * sizeof(double)); } -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], - double refSatRate[3], double quatErrorComplete[4], double quatError[3], - double deltaRate[3]) { +void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], + double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { double satRate[3] = {0, 0, 0}; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); From f12fa77644683d782af1c4bc04cfc505684ddfc2 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 13:54:36 +0100 Subject: [PATCH 10/60] still broken --- mission/controller/AcsController.cpp | 10 +++---- mission/controller/acs/Guidance.cpp | 13 ++------- mission/controller/acs/Guidance.h | 42 ++++++---------------------- 3 files changed, 17 insertions(+), 48 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f1bef501..c402fe9e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -250,8 +250,8 @@ void AcsController::performPointingCtrl() { switch (submode) { case acs::PTG_IDLE: - guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); + guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; @@ -319,8 +319,8 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_NADIR: - guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); + guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, + targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, @@ -341,7 +341,7 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_INERTIAL: - guidance.inertialQuatPtg(targetQuat, refSatRate); + guidance.targetQuatPtgInertial(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 506f6dfc..fc5e839e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -1,10 +1,3 @@ -/* - * Guidance.cpp - * - * Created on: 6 Jun 2022 - * Author: Robin Marquardt - */ - #include "Guidance.h" #include @@ -355,7 +348,7 @@ void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatR QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::sunQuatPtg(timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -421,7 +414,7 @@ void Guidance::sunQuatPtg(timeval now, double targetQuat[4], double refSatRate[3 refSatRate[2] = 0; } -void Guidance::quatNadirPtgSingleAxis(timeval now, double targetQuat[4], +void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -535,7 +528,7 @@ void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], tim QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) { std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 32c013fc..1b3ee607 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -1,10 +1,3 @@ -/* - * Guidance.h - * - * Created on: 6 Jun 2022 - * Author: Robin Marquardt - */ - #ifndef GUIDANCE_H_ #define GUIDANCE_H_ @@ -23,44 +16,27 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station - void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]); - void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]); - void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]); + void targetQuatPtgThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); + void targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]); - void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from parameters for inertial // pointing - void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); + void targetQuatPtgInertial(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 quatRef[4], - double refSatRate[3], double quatErrorComplete[4], double quatError[3], - double deltaRate[3]); + void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], + double quatErrorComplete[4], double quatError[3], double deltaRate[3]); void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate); From 6352b65f461d86af2ad4c96fcb249e7c2c7a4334 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 14:46:41 +0100 Subject: [PATCH 11/60] some things also broken some things fixed again --- mission/controller/AcsController.cpp | 17 +- mission/controller/acs/Guidance.cpp | 226 +++++++++++++-------------- mission/controller/acs/Guidance.h | 11 +- 3 files changed, 123 insertions(+), 131 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c402fe9e..86f67bfd 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -227,10 +227,7 @@ void AcsController::performPointingCtrl() { navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); - double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 0}; uint8_t enableAntiStiction = true; - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -248,6 +245,7 @@ void AcsController::performPointingCtrl() { double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}; + double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, @@ -273,11 +271,14 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_TARGET: - guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + if (!gpsDataProcessed.gpsPosition.isValid() || !gpsDataProcessed.gpsVelocity.isValid()) { + // ToDo: triggerEvent + return; + } + guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index fc5e839e..cdb3ffe7 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -12,27 +12,12 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } +Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} Guidance::~Guidance() {} -void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { - if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or - not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, - 3 * sizeof(double)); - } else { - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, - 3 * sizeof(double)); - } - std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, - 3 * sizeof(double)); -} - -void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4], + double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- @@ -49,7 +34,7 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData, double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); - // Transformation between ECEF and IJK frame + // Transformation between ECEF and ECI frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -60,13 +45,12 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData, 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)); + // 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}; - QuaternionOperations::toDcm(quatBJ, dcmBJ); - MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); + // QuaternionOperations::toDcm(quatBJ, dcmBJ); + // MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); // Target Direction in the body frame double targetDirB[3] = {0, 0, 0}; @@ -163,119 +147,70 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData, } } -void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *refSatRate) { - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); - if (timeElapsed < timeElapsedMax) { - double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; - double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); - omegaRefSaved[0] = omegaRefNew[0]; - omegaRefSaved[1] = omegaRefNew[1]; - omegaRefSaved[2] = omegaRefNew[2]; - } else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - } - - timeSavedQuaternion = now; - savedQuaternion[0] = quatInertialTarget[0]; - savedQuaternion[1] = quatInertialTarget[1]; - savedQuaternion[2] = quatInertialTarget[2]; - savedQuaternion[3] = quatInertialTarget[3]; -} - -void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], + double quatIX[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double targetCart[3] = {0, 0, 0}; + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); + acsParameters.targetModeControllerParameters.altitudeTgt, targetE); double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); + VectorOperations::subtract(targetE, posSatE, targetDirE, 3); - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // Target Direction and position vector in the inertial frame - double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + // target direction and position vector in the inertial frame + double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - // negative x-Axis aligned with target (Camera/E-band transmitter position) + // negative x-axis aligned with target + // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::normalize(targetDirI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // Transform velocity into inertial frame - double velocityE[3]; - std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); + // transform velocity into inertial frame + double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityI, 3); - // orbital normal vector - double orbitalNormalJ[3] = {0, 0, 0}; - VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); - VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); + // orbital normal vector of target and velocity vector + double orbitalNormalI[3] = {0, 0, 0}; + VectorOperations::cross(posSatI, velocityI, orbitalNormalI); + VectorOperations::normalize(orbitalNormalI, orbitalNormalI, 3); - // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution + // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture + // resolution double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); + VectorOperations::cross(orbitalNormalI, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // z-Axis completes RHS + // z-axis completes RHS double zAxis[3] = {0, 0, 0}; VectorOperations::cross(xAxis, yAxis, zAxis); - // Complete transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); + // join transformation matrix + double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmIX, quatIX); int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); + refRotationRate(timeElapsedMax, now, quatIX, refSatRate); } void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { @@ -293,7 +228,7 @@ void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatR double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); - // Transformation between ECEF and IJK frame + // Transformation between ECEF and ECI frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -367,7 +302,7 @@ void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSat return; } - // Transformation between ECEF and IJK frame + // Transformation between ECEF and ECI frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -422,7 +357,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - // Transformation between ECEF and IJK frame + // Transformation between ECEF and ECI frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -479,7 +414,7 @@ void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], tim double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - // Transformation between ECEF and IJK frame + // Transformation between ECEF and ECI frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -560,6 +495,48 @@ void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSat // under 150 arcsec ?? } +void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate) { + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - + (timeSavedQuaternion.tv_sec + + timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + if (timeElapsed < timeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); + + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); + omegaRefSaved[0] = omegaRefNew[0]; + omegaRefSaved[1] = omegaRefNew[1]; + omegaRefSaved[2] = omegaRefNew[2]; + } else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } + + timeSavedQuaternion = now; + savedQuaternion[0] = quatInertialTarget[0]; + savedQuaternion[1] = quatInertialTarget[1]; + savedQuaternion[2] = quatInertialTarget[2]; + savedQuaternion[3] = quatInertialTarget[3]; +} + ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); @@ -590,3 +567,16 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, return returnvalue::FAILED; } } + +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { + if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or + not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore + std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, + 3 * sizeof(double)); + } else { + std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, + 3 * sizeof(double)); + } + std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + 3 * sizeof(double)); +} diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 1b3ee607..d68936bd 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -16,9 +16,10 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station - void targetQuatPtgThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]); - void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]); void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], + double refSatRate[3]); + void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station @@ -26,15 +27,15 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]); void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from parameters for inertial // pointing void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]); - // @note: compares target Quaternion and reference quaternion, also actual satellite rate and - // desired + // @note: compares target Quaternion and reference quaternion, also actual and desired satellite + // rate void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], double quatErrorComplete[4], double quatError[3], double deltaRate[3]); From 0d32bc0c0afff1d13e2141e7a5e90f8f5b18338f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 15:15:47 +0100 Subject: [PATCH 12/60] added target rotation rate to ctrlValData set --- mission/controller/AcsController.cpp | 8 +++++--- mission/controller/AcsController.h | 1 + .../controller/controllerdefinitions/AcsCtrlDefinitions.h | 4 +++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 86f67bfd..73730649 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -245,7 +245,8 @@ void AcsController::performPointingCtrl() { double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}; - double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}; + double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, + errorAngle = 0; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, @@ -278,8 +279,8 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle); + guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); @@ -536,6 +537,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); + localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate); poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); // Actuator CMD localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e74681d4..ba76c7da 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -180,6 +180,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry tgtQuat = PoolEntry(4); PoolEntry errQuat = PoolEntry(4); PoolEntry errAng = PoolEntry(); + PoolEntry tgtRotRate = PoolEntry(4); // Actuator CMD acsctrl::ActuatorCmdData actuatorCmdData; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index e3908bd0..71158868 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -95,6 +95,7 @@ enum PoolIds : lp_id_t { TGT_QUAT, ERROR_QUAT, ERROR_ANG, + TGT_ROT_RATE, // Actuator Cmd RW_TARGET_TORQUE, RW_TARGET_SPEED, @@ -109,7 +110,7 @@ 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 = 4; static constexpr uint8_t MEKF_SET_ENTRIES = 2; -static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; +static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** @@ -249,6 +250,7 @@ class CtrlValData : public StaticLocalDataSet { 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); + lp_vec_t tgtRotRate = lp_vec_t(sid.objectId, TGT_ROT_RATE, this); private: }; From 0dae3b04bec484fd217358eb4d060139d19b2a07 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 17 Feb 2023 15:57:07 +0100 Subject: [PATCH 13/60] PTG_TARGET should work now --- mission/controller/AcsController.cpp | 23 +++++++++++------ mission/controller/acs/Guidance.cpp | 30 ++++++++++++---------- mission/controller/acs/Guidance.h | 12 ++++++--- mission/controller/acs/control/PtgCtrl.cpp | 4 ++- 4 files changed, 43 insertions(+), 26 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 73730649..943e2c84 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -228,8 +228,6 @@ void AcsController::performPointingCtrl() { &mekfData, &validMekf); uint8_t enableAntiStiction = true; - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { @@ -245,8 +243,10 @@ void AcsController::performPointingCtrl() { double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}; - double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, - errorAngle = 0; + // Variables required for guidance + double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, + errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, + satRotRate[3] = {0, 0, 0}, satRotRateError[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, @@ -279,10 +279,17 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle); - guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuatInterim, + errorAngle); + if (mekfData.satRotRateMekf.isValid()) { + std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double)); + } else { + std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double)); + } + guidance.comparePtg(errorQuatInterim, acsParameters.targetModeControllerParameters.quatRef, + errorQuat, satRotRate, targetSatRotRate, + acsParameters.targetModeControllerParameters.refRotRate, satRotRateError); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, satRotRateError, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index cdb3ffe7..86d70840 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -470,27 +470,23 @@ void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) 3 * sizeof(double)); } -void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { - double satRate[3] = {0, 0, 0}; - std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); - VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); - // valid checks ? +void Guidance::comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4], + double satRotRate[3], double satRotRateGuidance[3], + double satRotRateRef[3], double satRotRateError[3]) { + double combinedRefSatRate[3] = {0, 0, 0}; + VectorOperations::add(satRotRateGuidance, satRotRateRef, combinedRefSatRate, 3); + VectorOperations::subtract(satRotRate, combinedRefSatRate, satRotRateError, 3); + 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); + MatrixOperations::multiply(*quatErrorMtx, oldErrorQuat, newErrorQuatComplete, 4, 4, 1); - if (quatErrorComplete[3] < 0) { - quatErrorComplete[3] *= -1; + if (newErrorQuatComplete[3] < 0) { + VectorOperations::mulScalar(newErrorQuatComplete, -1, newErrorQuatComplete, 4); } - - 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 ?? } @@ -537,6 +533,12 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn savedQuaternion[3] = quatInertialTarget[3]; } +void Guidance::calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4], + double errorAngle) { + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + errorAngle = 2 * acos(errorQuat[3]); +} + ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index d68936bd..985c858d 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -34,15 +34,21 @@ class Guidance { // pointing void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]); + // @note: compares the target Quaternion in the ECI to the current orientation in ECI to compute + // the error quaternion and error angle + void calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4], + double errorAngle); + // @note: compares target Quaternion and reference quaternion, also actual and desired satellite // rate - void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + void comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4], + double satRotRate[3], double satRotRateGuidance[3], double satRotRateRef[3], + double satRotRateError[3]); void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate); - // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid + // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 60394421..74a32a4a 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { } void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, - const double *qError, const double *deltaRate, const double *rwPseudoInv, + const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix @@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorMin = pointingLawParameters->qiMin; double omMax = pointingLawParameters->omMax; + double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]}; + double cInt = 2 * om * zeta; double kInt = 2 * pow(om, 2); From bf65d13849e22dd8745bbc8cf159bfe2be9f92ec Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 18 Feb 2023 11:53:01 +0100 Subject: [PATCH 14/60] pdec handler check locks during init --- linux/ipcore/PdecHandler.cpp | 1 + mission/system/tree/comModeTree.cpp | 2 +- mission/system/tree/system.cpp | 3 ++- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp index b6b684e9..b59dc904 100644 --- a/linux/ipcore/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -164,6 +164,7 @@ ReturnValue_t PdecHandler::irqOperation() { state = State::WAIT_FOR_RECOVERY; return result; } + checkLocks(); state = State::RUNNING; break; case State::RUNNING: { diff --git a/mission/system/tree/comModeTree.cpp b/mission/system/tree/comModeTree.cpp index 7e771892..222d1f7d 100644 --- a/mission/system/tree/comModeTree.cpp +++ b/mission/system/tree/comModeTree.cpp @@ -67,7 +67,7 @@ Subsystem& satsystem::com::init() { buildTxAndRxLowRateSequence(SUBSYSTEM, entry); buildTxAndRxHighRateSequence(SUBSYSTEM, entry); buildTxAndRxDefaultRateSequence(SUBSYSTEM, entry); - SUBSYSTEM.setInitialMode(NML, ::com::Submode::RX_ONLY); + SUBSYSTEM.setInitialMode(COM_SEQUENCE_RX_ONLY.first); return SUBSYSTEM; } diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp index 69345bf3..cb6202f1 100644 --- a/mission/system/tree/system.cpp +++ b/mission/system/tree/system.cpp @@ -3,7 +3,7 @@ #include #include #include - +#include "mission/comDefs.h" #include "acsModeTree.h" #include "comModeTree.h" #include "eive/objects.h" @@ -85,6 +85,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { // Build SAFE transition 0. Two transitions to reduce number of consecutive events and because // consecutive commanding of TCS and ACS can lead to SPI issues. iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second); + iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second); check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)), ctxc); From 700d7ced643d8563878bdc8d0433e31179df8965 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 11:52:53 +0100 Subject: [PATCH 15/60] only one guidance function to calculate error quaternion --- mission/controller/AcsController.cpp | 18 +++------ mission/controller/acs/Guidance.cpp | 58 +++++++++++++++------------- mission/controller/acs/Guidance.h | 35 ++++++++--------- 3 files changed, 53 insertions(+), 58 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 943e2c84..ccdeb077 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -246,7 +246,7 @@ void AcsController::performPointingCtrl() { // Variables required for guidance double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, - satRotRate[3] = {0, 0, 0}, satRotRateError[3] = {0, 0, 0}; + satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, @@ -279,17 +279,11 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuatInterim, - errorAngle); - if (mekfData.satRotRateMekf.isValid()) { - std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double)); - } else { - std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double)); - } - guidance.comparePtg(errorQuatInterim, acsParameters.targetModeControllerParameters.quatRef, - errorQuat, satRotRate, targetSatRotRate, - acsParameters.targetModeControllerParameters.refRotRate, satRotRateError); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, satRotRateError, + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, + acsParameters.targetModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 86d70840..4d8dfa88 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -148,7 +148,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta } void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], - double quatIX[4], double refSatRate[3]) { + double quatIX[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -210,7 +210,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel QuaternionOperations::fromDcm(dcmIX, quatIX); int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatIX, refSatRate); + targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate); } void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { @@ -470,31 +470,41 @@ void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) 3 * sizeof(double)); } -void Guidance::comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4], - double satRotRate[3], double satRotRateGuidance[3], - double satRotRateRef[3], double satRotRateError[3]) { - double combinedRefSatRate[3] = {0, 0, 0}; - VectorOperations::add(satRotRateGuidance, satRotRateRef, combinedRefSatRate, 3); - VectorOperations::subtract(satRotRate, combinedRefSatRate, satRotRateError, 3); - - 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, oldErrorQuat, newErrorQuatComplete, 4, 4, 1); - - if (newErrorQuatComplete[3] < 0) { - VectorOperations::mulScalar(newErrorQuatComplete, -1, newErrorQuatComplete, 4); +void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], + double errorQuat[4], double errorSatRotRate[3], double errorAngle) { + // First calculate error quaternion between current and target orientation + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + // Last calculate add rotation from reference quaternion + QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); + // Keep scalar part of quaternion positive + if (errorQuat[3] < 0) { + VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); } + // Calculate error angle + errorAngle = QuaternionOperations::getAngle(errorQuat, true); + + // Only give back error satellite rotational rate if orientation has already been aquired + if (errorAngle < 2. / 180. * M_PI) { + // First combine the target and reference satellite rotational rates + double combinedRefSatRotRate[3] = {0, 0, 0}; + VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); + // Then substract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, + 3); + } else { + // If orientation has not been aquired yet set satellite rotational rate to zero + errorSatRotRate = 0; + } + // target flag in matlab, importance, does look like it only gives feedback if pointing control is // under 150 arcsec ?? } -void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *refSatRate) { +void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate) { //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate + // Calculation of target rotation rate //------------------------------------------------------------------------------------- double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternion.tv_sec + @@ -533,12 +543,6 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn savedQuaternion[3] = quatInertialTarget[3]; } -void Guidance::calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4], - double errorAngle) { - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); - errorAngle = 2 * acos(errorQuat[3]); -} - ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 985c858d..d67431d7 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -16,37 +16,34 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station - void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], - double refSatRate[3]); - void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]); + double targetSatRotRate[3]); + void targetQuatPtgGs(timeval now, double targetQuat[4], double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSun(timeval now, double targetQuat[4], double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); - void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from parameters for inertial // pointing - void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]); + void targetQuatPtgInertial(double targetQuat[4], double targetSatRotRate[3]); - // @note: compares the target Quaternion in the ECI to the current orientation in ECI to compute - // the error quaternion and error angle - void calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4], - double errorAngle); + // @note: Calculates the error quaternion between the current orientation and the target + // quaternion, considering a reference quaternion. Additionally the difference between the actual + // and a desired satellite rotational rate is calculated, again considering a reference rotational + // rate. Lastly gives back the error angle of the error quaternion. + void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], + double errorQuat[4], double errorSatRotRate[3], double errorAngle); - // @note: compares target Quaternion and reference quaternion, also actual and desired satellite - // rate - void comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4], - double satRotRate[3], double satRotRateGuidance[3], double satRotRateRef[3], - double satRotRateError[3]); - - void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *refSatRate); + void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *targetSatRotRate); // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" From 4c079b7d333428383c0a34f2939c3c2fa2a324a6 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Mon, 20 Feb 2023 15:13:58 +0100 Subject: [PATCH 16/60] removed init lock check --- linux/ipcore/PdecHandler.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp index 8acbce16..3244280e 100644 --- a/linux/ipcore/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -164,7 +164,6 @@ ReturnValue_t PdecHandler::irqOperation() { state = State::WAIT_FOR_RECOVERY; return result; } - checkLocks(); state = State::RUNNING; break; case State::RUNNING: { From 04e1cb52acd98fe95a8113741e78af6b06ed215d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 15:59:01 +0100 Subject: [PATCH 17/60] when did i push this last --- mission/controller/AcsController.cpp | 38 ++-- mission/controller/acs/AcsParameters.h | 16 +- mission/controller/acs/Guidance.cpp | 184 +++++++++----------- mission/controller/acs/Guidance.h | 8 +- mission/controller/acs/SensorProcessing.cpp | 7 - 5 files changed, 120 insertions(+), 133 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ccdeb077..172ea4ff 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -249,26 +249,21 @@ void AcsController::performPointingCtrl() { satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, - *rwPseudoInv, torquePtgRws); + guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + &acsParameters.idleModeControllerParameters, 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); + enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET: @@ -296,17 +291,15 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET_GS: - guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value, + susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -319,16 +312,16 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, + ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -341,13 +334,13 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: guidance.targetQuatPtgInertial(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, @@ -363,6 +356,7 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 365a04bd..f0448b67 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF { uint8_t enableAntiStiction = true; } pointingLawParameters; + struct IdleModeControllerParameters : PointingLawParameters { + } idleModeControllerParameters; + struct TargetModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna - double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to - // give this as an input- currently en calculation is done + double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; int8_t timeElapsedMax = 10; // rot rate calculations @@ -860,6 +862,16 @@ class AcsParameters : public HasParametersIF { double blindRotRate = 1 * M_PI / 180; } targetModeControllerParameters; + struct GsTargetModeControllerParameters : PointingLawParameters { + double refDirection[3] = {-1, 0, 0}; // Antenna + int8_t timeElapsedMax = 10; // rot rate calculations + + // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] + } gsTargetModeControllerParameters; + struct NadirModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna double quatRef[4] = {0, 0, 0, 1}; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4d8dfa88..f2d3cceb 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -17,12 +17,12 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters Guidance::~Guidance() {} void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4], - double refSatRate[3]) { + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) + // transform longitude, latitude and altitude to cartesian coordiantes (earth + // fixed/centered frame) double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( @@ -30,19 +30,19 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Target direction in the ECEF frame + // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // Transformation between ECEF and Body frame // double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -52,20 +52,16 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta // QuaternionOperations::toDcm(quatBJ, dcmBJ); // MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); - // Target Direction in the body frame + // 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 refDirB[3] = {0, 0, 0}; + std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection, + 3 * sizeof(double)); double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); + double normRefDirB = VectorOperations::norm(refDir, 3); double crossDir[3] = {0, 0, 0}; double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); @@ -152,7 +148,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, @@ -161,7 +157,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, targetDirE, 3); - // transformation between ECEF and ECI frame + // transformation between ECEF and ECI frame double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -171,7 +167,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // target direction and position vector in the inertial frame + // target direction and position vector in the inertial frame double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); @@ -213,133 +209,96 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate); } -void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + double groundStationE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); + acsParameters.gsTargetModeControllerParameters.latitudeTgt, + acsParameters.gsTargetModeControllerParameters.longitudeTgt, + acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // Target Direction and position vector in the inertial frame - double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + // target direction and position vector in the inertial frame + double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - // negative x-Axis aligned with target (Camera/E-band transmitter position) + // negative x-axis aligned with target + // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::normalize(targetDirI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // get Sun Vector Model in ECI - double sunJ[3]; - std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - VectorOperations::normalize(sunJ, sunJ, 3); + // get sun vector model in ECI + VectorOperations::normalize(sunDirI, sunDirI, 3); // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x - double xDotS = VectorOperations::dot(xAxis, sunJ); + double xDotS = VectorOperations::dot(xAxis, sunDirI); xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); double sunParallel[3], zAxis[3]; VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); - VectorOperations::subtract(sunJ, sunParallel, zAxis, 3); + VectorOperations::subtract(sunDirI, sunParallel, zAxis, 3); VectorOperations::normalize(zAxis, zAxis, 3); - // calculate y-axis + // y-axis completes RHS double yAxis[3]; VectorOperations::cross(zAxis, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // Complete transformation matrix + // join transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); + int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate); } -void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- - double quatBJ[4] = {0, 0, 0, 0}; - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::toDcm(quatBJ, dcmBJ); - - double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; - if (susDataProcessed->sunIjkModel.isValid()) { - std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } else if (susDataProcessed->susVecTot.isValid()) { - std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); - } else { - return; - } - - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // positive z-Axis of EIVE in direction of sun + // positive z-Axis of EIVE in direction of sun double zAxis[3] = {0, 0, 0}; - VectorOperations::normalize(sunDirB, zAxis, 3); + VectorOperations::normalize(sunDirI, zAxis, 3); - // Assign helper vector (north pole inertial) + // assign helper vector (north pole inertial) double helperVec[3] = {0, 0, 1}; - // Construct y-axis from helper vector and z-axis + // construct y-axis from helper vector and z-axis double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // Construct x-axis from y- and z-axes + // construct x-axis from y- and z-axes double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); - // Transformation matrix to Sun, no further transforamtions, reference is already - // the EIVE body frame + // join transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatSun[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatSun); - - targetQuat[0] = quatSun[0]; - targetQuat[1] = quatSun[1]; - targetQuat[2] = quatSun[2]; - targetQuat[3] = quatSun[3]; + QuaternionOperations::fromDcm(dcmTgt, targetQuat); //---------------------------------------------------------------------------- // Calculation of reference rotation rate @@ -501,6 +460,31 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // under 150 arcsec ?? } +void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double errorQuat[4], + double errorSatRotRate[3], double errorAngle) { + // First calculate error quaternion between current and target orientation + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + // Keep scalar part of quaternion positive + if (errorQuat[3] < 0) { + VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); + } + // Calculate error angle + errorAngle = QuaternionOperations::getAngle(errorQuat, true); + + // Only give back error satellite rotational rate if orientation has already been aquired + if (errorAngle < 2. / 180. * M_PI) { + // Then substract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); + } else { + // If orientation has not been aquired yet set satellite rotational rate to zero + errorSatRotRate = 0; + } + + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? +} + void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate) { //------------------------------------------------------------------------------------- diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index d67431d7..4dc8ec86 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -19,11 +19,12 @@ class Guidance { void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], double targetSatRotRate[3]); - void targetQuatPtgGs(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void targetQuatPtgSun(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing @@ -41,6 +42,9 @@ class Guidance { void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double errorAngle); + void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], + double errorAngle); void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *targetSatRotRate); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0845768a..e268d786 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,10 +1,3 @@ -/* - * SensorProcessing.cpp - * - * Created on: 7 Mar 2022 - * Author: Robin Marquardt - */ - #include "SensorProcessing.h" #include From 6016063addbfdb0d976510931d0d60d720a6d0b9 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:00:17 +0100 Subject: [PATCH 18/60] fixed unused variables --- mission/controller/AcsController.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 172ea4ff..b2cd74f7 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -244,9 +244,8 @@ void AcsController::performPointingCtrl() { double mgtDpDes[3] = {0, 0, 0}; // Variables required for guidance - double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, - errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, - satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0}; + double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, + errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); From 2c8cfa3874ef2805089105fd0d5c3e1da07960e0 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:00:54 +0100 Subject: [PATCH 19/60] can never happen --- mission/controller/AcsController.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b2cd74f7..5df63342 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -266,10 +266,6 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_TARGET: - if (!gpsDataProcessed.gpsPosition.isValid() || !gpsDataProcessed.gpsVelocity.isValid()) { - // ToDo: triggerEvent - return; - } guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); From 48b0ee8662ed78e49e8f4c5eac169a933d2fae0d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:14:40 +0100 Subject: [PATCH 20/60] naming fixes --- mission/controller/acs/Guidance.cpp | 67 +++++++++++++---------------- 1 file changed, 31 insertions(+), 36 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f2d3cceb..a937f950 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -144,7 +144,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta } void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], - double quatIX[4], double targetSatRotRate[3]) { + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -203,14 +203,14 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmIX, quatIX); + QuaternionOperations::fromDcm(dcmIX, targetQuat); int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate); + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -266,11 +266,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); + QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate); + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { @@ -289,7 +288,7 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // construct x-axis from y- and z-axes + // x-axis completes RHS double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); @@ -365,53 +364,49 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], refSatRate[2] = 0; } -void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now, +void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + // satellite position in inertial reference frame + double posSatI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - // Target Direction in the body frame - double targetDirJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - - // negative x-Axis aligned with target (Camera position) + // negative x-axis aligned with position vector + // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::normalize(posSatI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // z-Axis parallel to long side of picture resolution + // make z-Axis parallel to major part of camera resolution double zAxis[3] = {0, 0, 0}; - std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); - VectorOperations::cross(xAxis, velocityJ, zAxis); + double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityI, 3); + VectorOperations::cross(xAxis, velocityI, zAxis); VectorOperations::normalize(zAxis, zAxis, 3); // y-Axis completes RHS double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, xAxis, yAxis); - // Complete transformation matrix + // join transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); + QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); From 81a4112c45b5a4d0dba4714ff82edde374ad28c3 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:20:54 +0100 Subject: [PATCH 21/60] nadir should work again --- mission/controller/AcsController.cpp | 12 +++++++----- mission/controller/acs/Guidance.cpp | 11 +++-------- mission/controller/acs/Guidance.h | 3 ++- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 5df63342..ff8be5ad 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -311,11 +311,13 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_NADIR: - guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); + guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, + acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index a937f950..4f30e636 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -364,8 +364,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], refSatRate[2] = 0; } -void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- @@ -409,12 +409,7 @@ void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velS QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); + targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 4dc8ec86..7306d4f6 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -29,7 +29,8 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); - void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], + double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from parameters for inertial // pointing From 1eb26240b1526f390f411cd0133f25662922d656 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:27:13 +0100 Subject: [PATCH 22/60] added missing param --- mission/controller/acs/AcsParameters.h | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f0448b67..f1b538c8 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -875,6 +875,7 @@ class AcsParameters : public HasParametersIF { struct NadirModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna double quatRef[4] = {0, 0, 0, 1}; + double refRotRate[3] = {0, 0, 0}; int8_t timeElapsedMax = 10; // rot rate calculations } nadirModeControllerParameters; From 40446b1fea7c8e611182456e416e22d04aa96254 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:30:53 +0100 Subject: [PATCH 23/60] all used guidance parts and their calls in AcsCrtl should work now --- mission/controller/AcsController.cpp | 11 ++++++----- mission/controller/acs/Guidance.cpp | 7 ------- mission/controller/acs/Guidance.h | 4 ---- 3 files changed, 6 insertions(+), 16 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ff8be5ad..3f369ced 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -335,12 +335,13 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_INERTIAL: - guidance.targetQuatPtgInertial(targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, + std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, + acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4f30e636..1c5dca92 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -412,13 +412,6 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } -void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) { - std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); - std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, - 3 * sizeof(double)); -} - void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double errorAngle) { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 7306d4f6..e5560379 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -32,10 +32,6 @@ class Guidance { void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from parameters for inertial - // pointing - void targetQuatPtgInertial(double targetQuat[4], double targetSatRotRate[3]); - // @note: Calculates the error quaternion between the current orientation and the target // quaternion, considering a reference quaternion. Additionally the difference between the actual // and a desired satellite rotational rate is calculated, again considering a reference rotational From 57054c46ab3b95075f7a296f8c143d3cc2249493 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:35:48 +0100 Subject: [PATCH 24/60] added missing param structs --- mission/controller/acs/AcsParameters.cpp | 106 +++++++++++++++++++++-- 1 file changed, 97 insertions(+), 9 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a06d6a58..2e5fbb1b 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -342,7 +342,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x9): // TargetModeControllerParameters + case (0x9): // IdleModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + break; + + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // TargetModeControllerParameters switch (parameterId) { case 0x0: parameterWrapper->set(targetModeControllerParameters.zeta); @@ -408,7 +442,61 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xA): // NadirModeControllerParameters + case (0xB): // GsTargetModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // NadirModeControllerParameters switch (parameterId) { case 0x0: parameterWrapper->set(nadirModeControllerParameters.zeta); @@ -450,7 +538,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xB): // InertialModeControllerParameters + case (0xD): // InertialModeControllerParameters switch (parameterId) { case 0x0: parameterWrapper->set(inertialModeControllerParameters.zeta); @@ -492,7 +580,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xC): // StrParameters + case (0xE): // StrParameters switch (parameterId) { case 0x0: parameterWrapper->set(strParameters.exclusionAngle); @@ -504,7 +592,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xD): // GpsParameters + case (0xF): // GpsParameters switch (parameterId) { case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); @@ -513,7 +601,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xE): // SunModelParameters + case (0x10): // SunModelParameters switch (parameterId) { case 0x0: parameterWrapper->set(sunModelParameters.domega); @@ -543,7 +631,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xF): // KalmanFilterParameters + case (0x11): // KalmanFilterParameters switch (parameterId) { case 0x0: parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); @@ -567,7 +655,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x10): // MagnetorquesParameter + case (0x12): // MagnetorquesParameter switch (parameterId) { case 0x0: parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); @@ -594,7 +682,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x11): // DetumbleParameter + case (0x13): // DetumbleParameter switch (parameterId) { case 0x0: parameterWrapper->set(detumbleParameter.detumblecounter); From 7275454f8a10b760b10fa9b105433eed90949bfa Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 16:59:05 +0100 Subject: [PATCH 25/60] actually calculate the vector from position to target --- mission/controller/acs/Guidance.cpp | 49 +++++++++++------------------ 1 file changed, 19 insertions(+), 30 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 1c5dca92..fb64f4a4 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -21,18 +21,13 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double targetCart[3] = {0, 0, 0}; + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - - // target direction in the ECEF frame - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); + acsParameters.targetModeControllerParameters.altitudeTgt, targetE); // transformation between ECEF and ECI frame double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -44,17 +39,11 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // 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}; - - // 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); + // target direction in the ECI frame + double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); + VectorOperations::subtract(targetI, posSatI, &targetDirI, 3); // rotation quaternion from two vectors double refDirB[3] = {0, 0, 0}; @@ -167,16 +156,16 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // target direction and position vector in the inertial frame - double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); + // target direction in the ECI frame + double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); + VectorOperations::subtract(targetI, posSatI, targetDirI, 3); - // negative x-axis aligned with target + // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(targetDirI, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // transform velocity into inertial frame double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; @@ -234,16 +223,16 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // target direction and position vector in the inertial frame - double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); + // target direction in the ECI frame + double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1); + VectorOperations::subtract(groundStationI, posSatI, groundStationDirI, 3); - // negative x-axis aligned with target + // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirI, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + VectorOperations::normalize(groundStationDirI, xAxis, 3); // get sun vector model in ECI VectorOperations::normalize(sunDirI, sunDirI, 3); From 3ad6c8a56c305e0b24f259bc995f40aaea7f0171 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 17:39:03 +0100 Subject: [PATCH 26/60] in case we need this later --- mission/controller/acs/Guidance.cpp | 49 +++++++++++++++-------------- 1 file changed, 26 insertions(+), 23 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index fb64f4a4..f5260234 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -16,7 +16,8 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters Guidance::~Guidance() {} -void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4], +void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double refDirB[3], + double quatIB[4], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude @@ -39,25 +40,26 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // target direction in the ECI frame - double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0}; + // target direction in the ECI frame + double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); - VectorOperations::subtract(targetI, posSatI, &targetDirI, 3); + VectorOperations::subtract(targetI, posSatI, targetDirI, 3); + + // reference direction in ECI frame + double refDirI[3] = {0, 0, 0}; + QuaternionOperations::multiplyVector(quatIB, refDirB, refDirI); // rotation quaternion from two vectors - double refDirB[3] = {0, 0, 0}; - std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection, - 3 * sizeof(double)); - double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDirB = 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); + double crossDirI[3] = {0, 0, 0}; + double dotDirections = VectorOperations::dot(targetDirI, refDirI); + VectorOperations::cross(targetDirI, refDirI, crossDirI); + targetQuat[0] = crossDirI[0]; + targetQuat[1] = crossDirI[1]; + targetQuat[2] = crossDirI[2]; + targetQuat[3] = sqrt(pow(VectorOperations::norm(targetDirI, 3), 2) * + pow(VectorOperations::norm(refDirI, 3), 2) + + dotDirections); VectorOperations::normalize(targetQuat, targetQuat, 4); //------------------------------------------------------------------------------------- @@ -229,10 +231,11 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] MatrixOperations::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1); VectorOperations::subtract(groundStationI, posSatI, groundStationDirI, 3); - // x-axis aligned with target direction + // negative x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(groundStationDirI, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // get sun vector model in ECI VectorOperations::normalize(sunDirI, sunDirI, 3); @@ -296,15 +299,15 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[2] = 0; } -void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], - double refSatRate[3]) { // old version of Nadir Pointing +void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refDirB[3], + double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - // Transformation between ECEF and ECI frame + // transformation between ECEF and ECI frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -314,7 +317,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - // Transformation between ECEF and Body frame + // 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}; @@ -322,11 +325,11 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], QuaternionOperations::toDcm(quatBJ, dcmBJ); MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); - // Target Direction in the body frame + // 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 + // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; From 5349fb45e395125570a29895ba6f72e376197ea0 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 10:22:02 +0100 Subject: [PATCH 27/60] revert single axis pointing to original code --- mission/controller/acs/Guidance.cpp | 83 ++++++++++++++--------------- mission/controller/acs/Guidance.h | 4 +- 2 files changed, 44 insertions(+), 43 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f5260234..0add58aa 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -16,13 +16,13 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters Guidance::~Guidance() {} -void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double refDirB[3], - double quatIB[4], double targetQuat[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], + double sunDirI[3], double refDirB[3], double quatBI[4], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + // transform longitude, latitude and altitude to ECEF double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( @@ -30,7 +30,11 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetE); - // transformation between ECEF and ECI frame + // target direction in the ECEF frame + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(targetE, posSatE, targetDirE, 3); + + // transformation between ECEF and ECI frame double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -40,36 +44,44 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // target direction in the ECI frame - double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); - VectorOperations::subtract(targetI, posSatI, targetDirI, 3); + // transformation between ECEF and Body frame + double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - // reference direction in ECI frame - double refDirI[3] = {0, 0, 0}; - QuaternionOperations::multiplyVector(quatIB, refDirB, refDirI); + QuaternionOperations::toDcm(quatBI, dcmBI); + MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - // rotation quaternion from two vectors - double crossDirI[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(targetDirI, refDirI); - VectorOperations::cross(targetDirI, refDirI, crossDirI); - targetQuat[0] = crossDirI[0]; - targetQuat[1] = crossDirI[1]; - targetQuat[2] = crossDirI[2]; - targetQuat[3] = sqrt(pow(VectorOperations::norm(targetDirI, 3), 2) * - pow(VectorOperations::norm(refDirI, 3), 2) + - dotDirections); + // 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 + // calculation of reference rotation rate //------------------------------------------------------------------------------------- double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; - // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E + // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E MatrixOperations::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(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3); MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); @@ -79,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re double satRateDir[3] = {0, 0, 0}; VectorOperations::cross(velSatB, targetDirB, satRateDir); VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); + VectorOperations::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; - - if (susDataProcessed->sunIjkModel.isValid()) { - double sunDirJ[3] = {0, 0, 0}; - 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)); - } + MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); double exclAngle = acsParameters.strParameters.exclusionAngle, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, @@ -103,18 +108,14 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re 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; @@ -124,9 +125,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re blindRefRate[1] = 0; blindRefRate[2] = 0; } - - VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); - + VectorOperations::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3); } else { strBlindAvoidFlag = false; } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index e5560379..b494c7a9 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -16,7 +16,9 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station - void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], + double refDirB[3], double quatBI[4], double targetQuat[4], + double targetSatRotRate[3]); void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], double targetSatRotRate[3]); void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], From 35ad0a40d3b59e9dfc32124d1d434a1c19d8dd4b Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 10:38:10 +0100 Subject: [PATCH 28/60] output is now target quaternion --- mission/controller/acs/Guidance.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 0add58aa..1213df7a 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -131,6 +131,10 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve } } } + // revert calculated quaternion from qBX to qIX + double quatIB[4] = {0, 0, 0, 1}; + QuaternionOperations::inverse(quatBI, quatIB); + QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], From 1d5856b4d42e209480daee682df9c91ec0bbd580 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 10:44:02 +0100 Subject: [PATCH 29/60] same for nadir single axis --- mission/controller/acs/Guidance.cpp | 30 ++++++++++++++++------------- mission/controller/acs/Guidance.h | 3 ++- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 1213df7a..2346d18f 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -302,7 +302,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[2] = 0; } -void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refDirB[3], +void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], + double targetQuat[4], double refDirB[3], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -311,22 +312,20 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], d VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); // transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // transformation between ECEF and Body frame - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmBI[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); + QuaternionOperations::toDcm(quatBI, dcmBI); + MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); // target Direction in the body frame double targetDirB[3] = {0, 0, 0}; @@ -357,6 +356,11 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], d refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; + + // revert calculated quaternion from qBX to qIX + double quatIB[4] = {0, 0, 0, 1}; + QuaternionOperations::inverse(quatBI, quatIB); + QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index b494c7a9..65c9aa12 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -30,7 +30,8 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], + double targetQuat[4], double refDirB[3], double refSatRate[3]); void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]); From 0d7b77e5d109146928c56979fbf85556e1e910c2 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 10:50:08 +0100 Subject: [PATCH 30/60] this should make steffen happy --- mission/controller/acs/SusConverter.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 7020742a..f56ad32e 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -63,8 +63,7 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) { } void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { - uint8_t index; - float k, l; + uint8_t index, k, l; // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -75,10 +74,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB 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))) { + 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] + From b3b736a7bdbf6b1eca33f367ef436e5017b579f8 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 11:13:06 +0100 Subject: [PATCH 31/60] ctrlValData gets updated in every perform function --- mission/controller/AcsController.cpp | 17 +++++++++++++++++ mission/controller/AcsController.h | 1 + 2 files changed, 18 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3f369ced..d1bf2fcf 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -211,6 +211,7 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } + disableCtrlValData(); updateActuatorCmdData(cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, @@ -370,6 +371,7 @@ void AcsController::performPointingCtrl() { int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); + updateCtrlValData(targetQuat, errorQuat, errorAngle); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], @@ -440,6 +442,7 @@ void AcsController::updateCtrlValData(double errAng) { } } } + void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) { { PoolReadGuard pg(&ctrlValData); @@ -452,6 +455,20 @@ void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], doub } } +void AcsController::disableCtrlValData() { + double unitQuat[4] = {0, 0, 0, 1}; + double errAng = 0; + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.errAng.value = errAng; + ctrlValData.setValidity(false, true); + } + } +} + ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { // MGM Raw diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ba76c7da..ebf586b0 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -84,6 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes int16_t mtqTargetDipole[3]); void updateCtrlValData(double errAng); void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng); + void disableCtrlValData(); /* ACS Sensor Values */ ACS::SensorValues sensorValues; From a2a822c118a328b9e1f4079c087e20244c603c85 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 11:30:42 +0100 Subject: [PATCH 32/60] get --- mission/controller/AcsController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d1bf2fcf..746e36a9 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -128,7 +128,7 @@ void AcsController::performSafe() { navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); - // give desired satellite rate and sun direction to align + // get 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 From 2a9c1aab7f17217aab5488e4178258f072366f66 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 16:02:26 +0100 Subject: [PATCH 33/60] added mekf status to dataset --- mission/controller/AcsController.cpp | 1 + mission/controller/AcsController.h | 1 + mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 2 ++ 3 files changed, 4 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 746e36a9..65515b78 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -542,6 +542,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD // MEKF localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); + localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0}); // Ctrl Values localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 99309de5..715baef4 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -175,6 +175,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::MekfData mekfData; PoolEntry quatMekf = PoolEntry(4); PoolEntry satRotRateMekf = PoolEntry(3); + PoolEntry mekfStatus = PoolEntry(); // Ctrl Values acsctrl::CtrlValData ctrlValData; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 71158868..0675262c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -91,6 +91,7 @@ enum PoolIds : lp_id_t { // MEKF SAT_ROT_RATE_MEKF, QUAT_MEKF, + MEKF_STATUS, // Ctrl Values TGT_QUAT, ERROR_QUAT, @@ -239,6 +240,7 @@ class MekfData : public StaticLocalDataSet { 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); + lp_var_t mekfStatus = lp_var_t(sid.objectId, MEKF_STATUS, this); private: }; From 0ec0d551a3298154fa3b78a5b1a9cb28fa6e060c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 16:08:46 +0100 Subject: [PATCH 34/60] added functions for updating datasets upon errors or success --- .../acs/MultiplicativeKalmanFilter.cpp | 90 ++++++++----------- .../acs/MultiplicativeKalmanFilter.h | 23 +++-- mission/controller/acs/Navigation.cpp | 9 +- 3 files changed, 53 insertions(+), 69 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index f54893a2..d222de84 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -14,7 +14,7 @@ /*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) - : initialQuaternion{0.5, 0.5, 0.5, 0.5}, + : initialQuaternion{0, 0, 0, 1}, 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_); @@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } -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"? + const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { validInit = true; @@ -190,9 +188,11 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; + updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); } else { // no initialisation possible, no valid measurements validInit = false; + updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); } } @@ -208,32 +208,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // Check for GYR Measurements 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; + updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); 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; + updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); return KALMAN_NO_MODEL; } // Check Measurements available from SS, MAG, STR @@ -260,17 +240,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c 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); - } - } + updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA); return returnvalue::FAILED; } @@ -881,17 +851,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c 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; + updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } @@ -1121,20 +1081,40 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); - validMekf = true; - // Discrete Time Step + updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); + return returnvalue::OK; +} - // Check for new data in measurement -> SensorProcessing ? +void MultiplicativeKalmanFilter::reset() {} +void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, + MekfStatus mekfStatus) { { 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)); + 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)); + mekfData->quatMekf.setValid(false); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->satRotRateMekf.setValid(false); + mekfData->mekfStatus = mekfStatus; + mekfData->setValidity(true, false); + } + } +} + +void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, + MekfStatus mekfStatus, double quat[4], + double satRotRate[3]) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double)); + mekfData->mekfStatus = mekfStatus; mekfData->setValidity(true, true); } } - - return returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index a2f81272..e1b4f701 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -35,13 +35,13 @@ class MultiplicativeKalmanFilter { /* @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 + * 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); + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -63,6 +63,16 @@ class MultiplicativeKalmanFilter { const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); + enum MekfStatus : uint8_t { + UNINITIALIZED = 0, + NO_GYR_DATA = 1, + NO_MODEL_VECTORS = 2, + NO_SUS_MGM_STR_DATA = 3, + COVARIANCE_INVERSION_FAILED = 4, + INITIALIZED = 10, + RUNNING = 11, + }; + // resetting Mekf static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1); @@ -80,16 +90,17 @@ class MultiplicativeKalmanFilter { 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_); + void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); + void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], + double satRotRate[3]); }; #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index a9de5817..59803f0f 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,10 +1,3 @@ -/* - * Navigation.cpp - * - * Created on: 23 May 2022 - * Author: Robin Marquardt - */ - #include "Navigation.h" #include @@ -46,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid()); + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); kalmanInit = true; *mekfValid = returnvalue::OK; From 793e6f411954660942e6c9c381a948454ad15ae5 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 16:56:13 +0100 Subject: [PATCH 35/60] returnvalues for all mekf termination cases now --- .../acs/MultiplicativeKalmanFilter.cpp | 21 ++++++++++++----- .../acs/MultiplicativeKalmanFilter.h | 23 +++++++++++-------- 2 files changed, 29 insertions(+), 15 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index d222de84..d90f3f51 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -27,7 +27,7 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } -void MultiplicativeKalmanFilter::init( +ReturnValue_t 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, acsctrl::MekfData *mekfData) { // valids for "model measurements"? @@ -189,10 +189,12 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); + return KALMAN_INITIALIZED; } else { // no initialisation possible, no valid measurements validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); + return KALMAN_UNINITIALIZED; } } @@ -209,12 +211,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); - return KALMAN_NO_GYR_MEAS; + return KALMAN_NO_GYR_DATA; } // Check for Model Calculations else if (!validSSModel || !validMagModel) { updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); - return KALMAN_NO_MODEL; + return KALMAN_NO_MODEL_VECTORS; } // Check Measurements available from SS, MAG, STR if (validSS && validMagField_ && validSTR_) { @@ -852,7 +854,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); if (inversionFailed) { updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); - return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } // [K = P * H' / (H * P * H' + R)] @@ -1083,10 +1085,17 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); - return returnvalue::OK; + return KALMAN_RUNNING; } -void MultiplicativeKalmanFilter::reset() {} +void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { + double resetQuaternion[4] = {0, 0, 0, 1}; + double resetCovarianceMatrix[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}}; + std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); + std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); + updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); +} void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus) { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index e1b4f701..dd02cf9f 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -15,8 +15,7 @@ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ -#include //uint8_t -#include /*purpose, timeval ?*/ +#include #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" @@ -30,7 +29,7 @@ class MultiplicativeKalmanFilter { MultiplicativeKalmanFilter(AcsParameters *acsParameters_); virtual ~MultiplicativeKalmanFilter(); - void reset(); // NOT YET DEFINED - should only reset all mekf variables + void reset(acsctrl::MekfData *mekfData); /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm @@ -39,9 +38,10 @@ class MultiplicativeKalmanFilter { * 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, acsctrl::MekfData *mekfData); + ReturnValue_t 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, + acsctrl::MekfData *mekfData); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -75,9 +75,14 @@ class MultiplicativeKalmanFilter { // resetting Mekf static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; - static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1); - static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2); - static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3); + static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2); + static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3); + static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4); + static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5); + static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED = + returnvalue::makeCode(IF_KAL_ID, 6); + static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7); + static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8); private: /*Parameters*/ From d1402587dc416107e8bcbc60dfaaee7cbb822b84 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 17:06:24 +0100 Subject: [PATCH 36/60] differentiate between mekf not working during safe/detumble or any higer mode --- mission/acsDefs.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index f68ff24a..608191b0 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -25,6 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); //!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); +//!< MEKF was not able to compute a solution during safe or detumble mode. +static constexpr Event MEKF_INVALID_SAFE_MODE = MAKE_EVENT(3, severity::INFO); +//!< MEKF was not able to compute a solution during any pointing ACS mode. +static constexpr Event MEKF_INVALID_PTG_MODE = MAKE_EVENT(4, severity::HIGH); extern const char* getModeStr(AcsMode mode); From 019cc29c24204b944503c496725c192d7de69773 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 17:09:49 +0100 Subject: [PATCH 37/60] useMekf actually now uses returnvalues as inteded --- mission/controller/acs/Navigation.cpp | 34 +++++++++++++-------------- mission/controller/acs/Navigation.h | 19 +++++---------- 2 files changed, 23 insertions(+), 30 deletions(-) diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 59803f0f..c6310302 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -14,37 +14,37 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt 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, +ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MekfData *mekfData) { + double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - bool quatJBValid = sensorValues->strSet.caliQx.isValid() && + bool quatIBValid = 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, + return multiplicativeKalmanFilter.mekfEst( + quatIB, quatIBValid, 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(), acsParameters.onBoardParams.sampleTime, - mekfData); // VALIDS FOR QUAT AND RATE ?? + mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); } else { - multiplicativeKalmanFilter.init( + ReturnValue_t result; + result = 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(), mekfData); 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 ? + return result; } } + +void Navigation::resetMekf(acsctrl::MekfData *mekfData) { + multiplicativeKalmanFilter.reset(mekfData); +} diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 2474cb67..e054bfbf 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -1,10 +1,3 @@ -/* - * Navigation.h - * - * Created on: 19 Apr 2022 - * Author: Robin Marquardt - */ - #ifndef NAVIGATION_H_ #define NAVIGATION_H_ @@ -16,14 +9,14 @@ class Navigation { public: - Navigation(AcsParameters *acsParameters_); // Input mode ? + Navigation(AcsParameters *acsParameters_); virtual ~Navigation(); - void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, - ReturnValue_t *mekfValid); - void processSensorData(); + ReturnValue_t useMekf(ACS::SensorValues *sensorValues, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData); + void resetMekf(acsctrl::MekfData *mekfData); protected: private: From f19729e11d2258013c974720395641b46f679c00 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 21 Feb 2023 17:10:59 +0100 Subject: [PATCH 38/60] throw a single event every time the mekf stops working --- mission/controller/AcsController.cpp | 52 ++++++++++++++++++++-------- mission/controller/AcsController.h | 3 ++ 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 65515b78..c6448a82 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -124,17 +124,25 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_SAFE_MODE); + mekfInvalidFlag = true; + } + return; + } else { + mekfInvalidFlag = false; + } // get 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) { + if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) { safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), @@ -183,10 +191,18 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_SAFE_MODE); + mekfInvalidFlag = true; + } + return; + } else { + mekfInvalidFlag = false; + } double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, @@ -224,13 +240,21 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_PTG_MODE); + mekfInvalidFlag = true; + } + return; + } else { + mekfInvalidFlag = false; + } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { multipleRwUnavailableCounter++; if (multipleRwUnavailableCounter > 4) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 715baef4..9e6c2c4e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -10,6 +10,7 @@ #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" +#include "acs/MultiplicativeKalmanFilter.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" @@ -54,6 +55,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ParameterHelper parameterHelper; + bool mekfInvalidFlag = true; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif From 018f93cfbe5b59ecb6b6b1740b6e3e3b6efc6f86 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 22 Feb 2023 09:53:09 +0100 Subject: [PATCH 39/60] fixed bugs in com subsystem --- mission/system/objects/ComSubsystem.cpp | 6 +++++- mission/system/objects/ComSubsystem.h | 4 ++++ mission/system/tree/comModeTree.h | 5 ++++- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index 8e3f253e..3ca78904 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -28,9 +28,10 @@ ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequence void ComSubsystem::performChildOperation() { readEventQueue(); - if (mode != COM_SEQUENCE_RX_ONLY.first) { + if (countdownActive) { checkTransmitterCountdown(); } + Subsystem::performChildOperation(); } MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); } @@ -112,10 +113,12 @@ void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) { // disabled here if (mode == COM_SEQUENCE_RX_ONLY.first) { transmitterCountdown.timeOut(); + countdownActive = false; } else if ((mode == COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first) || (mode == COM_SEQUENCE_RX_AND_TX_LOW_RATE.first) || (mode == COM_SEQUENCE_RX_AND_TX_HIGH_RATE.first)) { transmitterCountdown.resetTimer(); + countdownActive = true; } Subsystem::startTransition(mode, submode); } @@ -170,5 +173,6 @@ void ComSubsystem::startRxAndTxDefaultSeq() { void ComSubsystem::checkTransmitterCountdown() { if (transmitterCountdown.hasTimedOut()) { startTransition(COM_SEQUENCE_RX_ONLY.first, SUBMODE_NONE); + countdownActive = false; } } diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index ac8cc60f..84ac081a 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -58,6 +58,10 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { // Countdown will be started as soon as the transmitter was enabled Countdown transmitterCountdown; + + // Transmitter countdown only active when sysrlinks transmitter is on (modes: + // rx and tx low rate, rx and tx high rate, rx and tx default rate) + bool countdownActive = false; }; #endif /* MISSION_SYSTEM_COMSUBSYSTEM_H_ */ diff --git a/mission/system/tree/comModeTree.h b/mission/system/tree/comModeTree.h index 0f3c083f..0f57217c 100644 --- a/mission/system/tree/comModeTree.h +++ b/mission/system/tree/comModeTree.h @@ -11,7 +11,10 @@ extern ComSubsystem SUBSYSTEM; // The syrlinks must not transmitting longer then 15 minutes otherwise the // transceiver might be damaged due to overheating -static const uint32_t TRANSMITTER_TIMEOUT = 900000; // 15 minutes +// 15 minutes in milliseconds +//static const uint32_t TRANSMITTER_TIMEOUT = 900000; + +static const uint32_t TRANSMITTER_TIMEOUT = 60000; Subsystem& init(); } // namespace com From 82d428440b24ded062a4a45770b82799082cd69b Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 22 Feb 2023 09:53:39 +0100 Subject: [PATCH 40/60] tmtc update --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index a3a3aaa8..a06f703d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a3a3aaa8836b425c923eb97e49ed29b452377bf6 +Subproject commit a06f703d6f6e882a15a745872cb42dac7f49f63b From fd1c090141f43dcab76544d80d628fa39c31115d Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 22 Feb 2023 13:02:04 +0100 Subject: [PATCH 41/60] tmtc update --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index a06f703d..679df9eb 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a06f703d6f6e882a15a745872cb42dac7f49f63b +Subproject commit 679df9ebe101ff93e36d8b58b5e0be04c5cc4cb2 From 3137ebb86eb82508c93789a21164196f6b92b431 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 22 Feb 2023 13:02:40 +0100 Subject: [PATCH 42/60] com subsystem complete --- bsp_q7s/core/ObjectFactory.cpp | 1 - linux/ipcore/PdecHandler.cpp | 1 + mission/system/objects/ComSubsystem.cpp | 123 ++++++++++++++---------- mission/system/objects/ComSubsystem.h | 17 +++- mission/system/tree/comModeTree.h | 4 +- 5 files changed, 89 insertions(+), 57 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 347cabdd..63539cc3 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -573,7 +573,6 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) { new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie, pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir); syrlinksHandler->setPowerSwitcher(pwrSwitcher); - syrlinksHandler->setStartUpImmediately(); syrlinksHandler->connectModeTreeParent(satsystem::com::SUBSYSTEM); #if OBSW_DEBUG_SYRLINKS == 1 syrlinksHandler->setDebugMode(true); diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp index 3244280e..1cf21ce0 100644 --- a/linux/ipcore/PdecHandler.cpp +++ b/linux/ipcore/PdecHandler.cpp @@ -165,6 +165,7 @@ ReturnValue_t PdecHandler::irqOperation() { return result; } state = State::RUNNING; + checkLocks(); break; case State::RUNNING: { nb = write(fd, &info, sizeof(info)); diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index 3ca78904..3a79ac15 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -1,5 +1,3 @@ -#include - #include "ComSubsystem.h" #include @@ -7,14 +5,11 @@ #include #include #include +#include #include -extern std::pair> COM_SEQUENCE_RX_ONLY; -//extern std::pair> COM_TABLE_RX_ONLY_TGT; -extern std::pair> COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE; -extern std::pair> COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT; -extern std::pair> COM_SEQUENCE_RX_AND_TX_LOW_RATE; -extern std::pair> COM_SEQUENCE_RX_AND_TX_HIGH_RATE; +#include + ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, uint32_t transmitterTimeout) @@ -28,8 +23,13 @@ ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequence void ComSubsystem::performChildOperation() { readEventQueue(); + // Execute default rate sequence after transition has been completed + if (rememberBitLock and not isInTransition) { + startRxAndTxLowRateSeq(); + rememberBitLock = false; + } if (countdownActive) { - checkTransmitterCountdown(); + checkTransmitterCountdown(); } Subsystem::performChildOperation(); } @@ -51,17 +51,17 @@ ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentif parameterWrapper->set(datarateCfg); com::setCurrentDatarate(static_cast(newVal)); return returnvalue::OK; - } - else if ((domainId == 0) and (uniqueIdentifier == static_cast(com::ParameterId::TRANSMITTER_TIMEOUT))) { - uint8_t newVal = 0; - ReturnValue_t result = newValues->getElement(&newVal); - if (result != returnvalue::OK) { - return result; - } - parameterWrapper->set(transmitterTimeout); - transmitterTimeout = newVal; - transmitterCountdown.setTimeout(transmitterTimeout); - return returnvalue::OK; + } else if ((domainId == 0) and + (uniqueIdentifier == static_cast(com::ParameterId::TRANSMITTER_TIMEOUT))) { + uint32_t newVal = 0; + ReturnValue_t result = newValues->getElement(&newVal); + if (result != returnvalue::OK) { + return result; + } + parameterWrapper->set(transmitterTimeout); + transmitterTimeout = newVal; + transmitterCountdown.setTimeout(transmitterTimeout); + return returnvalue::OK; } return returnvalue::OK; } @@ -79,46 +79,48 @@ ReturnValue_t ComSubsystem::initialize() { if (result != returnvalue::OK) { return result; } - EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); - if (manager == nullptr) { + EventManagerIF *manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); + if (manager == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl; + sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl; #endif - return ObjectManagerIF::CHILD_INIT_FAILED; - } - result = manager->registerListener(eventQueue->getId()); - if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->registerListener(eventQueue->getId()); + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event " - "listener" << std::endl; + sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event " + "listener" + << std::endl; #endif - return ObjectManagerIF::CHILD_INIT_FAILED; - } - result = manager->subscribeToEventRange(eventQueue->getId(), - event::getEventId(PdecHandler::CARRIER_LOCK), - event::getEventId(PdecHandler::BIT_LOCK_PDEC)); - if (result != returnvalue::OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + result = manager->subscribeToEventRange(eventQueue->getId(), + event::getEventId(PdecHandler::CARRIER_LOCK), + event::getEventId(PdecHandler::BIT_LOCK_PDEC)); + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC " - "handler" - << std::endl; + sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC " + "handler" + << std::endl; #endif - return result; - } + return result; + } return Subsystem::initialize(); } void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) { // Depending on the submode the transmitter timeout is enabled or // disabled here - if (mode == COM_SEQUENCE_RX_ONLY.first) { + if (mode == com::Submode::RX_ONLY) { transmitterCountdown.timeOut(); countdownActive = false; - } else if ((mode == COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first) || - (mode == COM_SEQUENCE_RX_AND_TX_LOW_RATE.first) || - (mode == COM_SEQUENCE_RX_AND_TX_HIGH_RATE.first)) { - transmitterCountdown.resetTimer(); - countdownActive = true; + } else if (isTxMode(mode)) { + // Only start transmitter countdown if transmitter is not already on + if (not isTxMode(this->mode)) { + transmitterCountdown.resetTimer(); + countdownActive = true; + } } Subsystem::startTransition(mode, submode); } @@ -156,23 +158,42 @@ void ComSubsystem::handleEventMessage(EventMessage *eventMessage) { } } -void ComSubsystem::handleBitLockEvent() { startRxAndTxDefaultSeq(); } +void ComSubsystem::handleBitLockEvent() { + if (isTxMode(mode)) { + // Tx already on + return; + } + if (isInTransition) { + rememberBitLock = true; + return; + } + startRxAndTxLowRateSeq(); +} void ComSubsystem::handleCarrierLockEvent() { if (!enableTxWhenCarrierLock) { return; } - startRxAndTxDefaultSeq(); + startRxAndTxLowRateSeq(); } -void ComSubsystem::startRxAndTxDefaultSeq() { +void ComSubsystem::startRxAndTxLowRateSeq() { // Turns transmitter on - startTransition(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first, SUBMODE_NONE); + startTransition(com::Submode::RX_AND_TX_LOW_DATARATE, SUBMODE_NONE); } void ComSubsystem::checkTransmitterCountdown() { if (transmitterCountdown.hasTimedOut()) { - startTransition(COM_SEQUENCE_RX_ONLY.first, SUBMODE_NONE); + startTransition(com::Submode::RX_ONLY, SUBMODE_NONE); countdownActive = false; } } + +bool ComSubsystem::isTxMode(Mode_t mode) { + if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) || + (mode == com::Submode::RX_AND_TX_LOW_DATARATE) || + (mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) { + return true; + } + return false; +} diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 84ac081a..6b5b41dc 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -30,6 +30,9 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { virtual void performChildOperation() override; private: + + static const Mode_t INITIAL_MODE = 0; + ReturnValue_t handleCommandMessage(CommandMessage *message) override; ReturnValue_t initialize() override; @@ -42,9 +45,14 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { void handleCarrierLockEvent(); void checkTransmitterCountdown(); /** - * @brief Enables transmitter in default (low) rate mode + * @brief Enables transmitter in low rate mode */ - void startRxAndTxDefaultSeq(); + void startRxAndTxLowRateSeq(); + + /** + * @brief Returns true if mode is a mode where the transmitter is on + */ + bool isTxMode(Mode_t mode); uint8_t datarateCfg = static_cast(com::Datarate::LOW_RATE_MODULATION_BPSK); // Maximum time after which the transmitter will be turned of. This is a @@ -62,6 +70,11 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { // Transmitter countdown only active when sysrlinks transmitter is on (modes: // rx and tx low rate, rx and tx high rate, rx and tx default rate) bool countdownActive = false; + + // True when bit lock occurred while COM subsystem is in a transition. This + // variable is used to remember the bit lock and execute the default rate + // sequence after the active transition has been completed + bool rememberBitLock = false; }; #endif /* MISSION_SYSTEM_COMSUBSYSTEM_H_ */ diff --git a/mission/system/tree/comModeTree.h b/mission/system/tree/comModeTree.h index 0f57217c..8641874e 100644 --- a/mission/system/tree/comModeTree.h +++ b/mission/system/tree/comModeTree.h @@ -12,9 +12,7 @@ extern ComSubsystem SUBSYSTEM; // The syrlinks must not transmitting longer then 15 minutes otherwise the // transceiver might be damaged due to overheating // 15 minutes in milliseconds -//static const uint32_t TRANSMITTER_TIMEOUT = 900000; - -static const uint32_t TRANSMITTER_TIMEOUT = 60000; +static const uint32_t TRANSMITTER_TIMEOUT = 900000; Subsystem& init(); } // namespace com From 83b81e1ba8b1c0a47c60c60395b2e6b130305790 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Feb 2023 14:50:38 +0100 Subject: [PATCH 43/60] changed mekf events again --- mission/acsDefs.h | 8 ++++---- mission/controller/AcsController.cpp | 26 +++++++++++++------------- mission/controller/AcsController.h | 8 +++++--- 3 files changed, 22 insertions(+), 20 deletions(-) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 608191b0..61a3c644 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -25,10 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); //!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); -//!< MEKF was not able to compute a solution during safe or detumble mode. -static constexpr Event MEKF_INVALID_SAFE_MODE = MAKE_EVENT(3, severity::INFO); -//!< MEKF was not able to compute a solution during any pointing ACS mode. -static constexpr Event MEKF_INVALID_PTG_MODE = MAKE_EVENT(4, severity::HIGH); +//!< MEKF was not able to compute a solution. +static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); +//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time. +static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c6448a82..81e5b1ca 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -14,8 +14,6 @@ AcsController::AcsController(object_id_t objectId) safeCtrl(&acsParameters), detumble(&acsParameters), ptgCtrl(&acsParameters), - detumbleCounter{0}, - multipleRwUnavailableCounter{0}, parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), @@ -129,10 +127,9 @@ void AcsController::performSafe() { if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_SAFE_MODE); + triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } - return; } else { mekfInvalidFlag = false; } @@ -157,10 +154,9 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); - // Detumble check and switch + // detumble check and switch if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { @@ -196,10 +192,9 @@ void AcsController::performDetumble() { if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_SAFE_MODE); + triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } - return; } else { mekfInvalidFlag = false; } @@ -207,7 +202,6 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); if (mekfData.satRotRateMekf.isValid() && @@ -245,21 +239,29 @@ void AcsController::performPointingCtrl() { if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_PTG_MODE); + triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } + if (mekfInvalidCounter > 4) { + triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION); + } + mekfInvalidCounter++; + commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); return; } else { mekfInvalidFlag = false; + mekfInvalidCounter = 0; } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { - multipleRwUnavailableCounter++; if (multipleRwUnavailableCounter > 4) { triggerEvent(acs::MULTIPLE_RW_INVALID); } + multipleRwUnavailableCounter++; return; } else { multipleRwUnavailableCounter = 0; @@ -387,12 +389,10 @@ void AcsController::performPointingCtrl() { ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); } - int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); - int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); updateCtrlValData(targetQuat, errorQuat, errorAngle); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 9e6c2c4e..a36f99ee 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -50,12 +50,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes Detumble detumble; PtgCtrl ptgCtrl; - uint8_t detumbleCounter; - uint8_t multipleRwUnavailableCounter; - ParameterHelper parameterHelper; + uint8_t detumbleCounter = 0; + uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = true; + uint8_t mekfInvalidCounter = 0; + int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; + int16_t cmdDipolMtqs[3] = {0, 0, 0}; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; From 67a14ca6f61963fd350e84853aaedef2bf8bd481 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Feb 2023 15:02:15 +0100 Subject: [PATCH 44/60] handle ACS event in AcsSubsystem --- mission/system/objects/AcsSubsystem.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index edb1c6d3..c812394c 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -47,6 +47,11 @@ ReturnValue_t AcsSubsystem::initialize() { if (result != returnvalue::OK) { sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; } + result = manager->subscribeToEvent(eventQueue->getId(), + event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION)); + if (result != returnvalue::OK) { + sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; + } return Subsystem::initialize(); } @@ -71,7 +76,8 @@ void AcsSubsystem::handleEventMessages() { } } if (event.getEvent() == acs::SAFE_RATE_RECOVERY || - event.getEvent() == acs::MULTIPLE_RW_INVALID) { + event.getEvent() == acs::MULTIPLE_RW_INVALID || + event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { CommandMessage msg; ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); status = commandQueue->sendMessage(commandQueue->getId(), &msg); From 01d8eccdd2958a9961306d9ad0ed092913b33b1f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Feb 2023 16:00:28 +0100 Subject: [PATCH 45/60] changelog --- CHANGELOG.md | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0f01acdc..b942a74d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,30 @@ change warranting a new major release: # [unreleased] +## Changed + +- All `targetQuat` functions in `Guidance` now return the target quaternion (target + in ECI frame), which is passed on to `CtrlValData`. + +## Added + +- `MEKF` now returns an unique returnvalue depending on why the function terminates. These + returnvalues are used in the `AcsController` to determine on how to procede with its + perform functions. In case the `MEKF` did terminate before estimating the quaternion + and rotational rate, an info event will be triggered. Another info event can only be + triggered after the `MEKF` has run successfully again. If the `AcsController` tries to + perform any pointing mode and the `MEKF` fails, the `performPointingCtrl` function will + set the RWs to the last RW speeds and set a zero dipole vector. If the `MEKF` does not + recover within 5 cycles (2 mins) the `AcsController` triggers another event, resulting in + the `AcsSubsystem` being commanded to `SAFE`. +- `MekfData` now includes `mekfStatus` +- `CtrlValData` now includes `tgtRotRate` + +## Fixed + +- Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter` +- Removed unused variables in the `AcsController` + # [v1.29.1] ## Fixed From 0d8cb295d9ca4acfeb2120e68d9f91b6e36ea013 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 22 Feb 2023 16:55:12 +0100 Subject: [PATCH 46/60] removed duplicate code --- mission/tmtc/CcsdsIpCoreHandler.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mission/tmtc/CcsdsIpCoreHandler.cpp b/mission/tmtc/CcsdsIpCoreHandler.cpp index 0b6a210d..b462f236 100644 --- a/mission/tmtc/CcsdsIpCoreHandler.cpp +++ b/mission/tmtc/CcsdsIpCoreHandler.cpp @@ -97,10 +97,6 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } - if (result != returnvalue::OK) { - return ObjectManagerIF::CHILD_INIT_FAILED; - } - #if OBSW_SYRLINKS_SIMULATED == 1 // Update data on rising edge ptmeConfig->invertTxClock(false); From 0466622a16c8eec8478384f895143027e0d9d6ed Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 Feb 2023 18:07:15 +0100 Subject: [PATCH 47/60] tweaks for ACS scheduling --- bsp_q7s/core/scheduling.cpp | 2 +- common/config/eive/definitions.h | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 3c3695dd..db6f3c91 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -404,7 +404,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction #ifdef RELEASE_BUILD static constexpr float acsPstPeriod = 0.4; #else - static constexpr float acsPstPeriod = 0.8; + static constexpr float acsPstPeriod = 0.4; #endif FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask( "ACS_TCS_PST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index c87c89ff..edeb3765 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -62,7 +62,8 @@ static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 50; -static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 75; +static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 90; +static constexpr uint32_t SCHED_BLOCK_RTD = 150; static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300; // 15 ms for FM @@ -76,6 +77,7 @@ static constexpr float SCHED_BLOCK_5_PERIOD = static_cast(SCHED_BLOCK_5_A static constexpr float SCHED_BLOCK_6_PERIOD = static_cast(SCHED_BLOCK_6_IMTQ_BLOCK_2_MS) / 400.0; static constexpr float SCHED_BLOCK_7_PERIOD = static_cast(SCHED_BLOCK_7_RW_READ_MS) / 400.0; +static constexpr float SCHED_BLOCK_RTD_PERIOD = static_cast(SCHED_BLOCK_RTD) / 400.0; } // namespace acs From 5357de8da88b11b8c5068d58fcb3343654827ec3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 Feb 2023 18:13:52 +0100 Subject: [PATCH 48/60] bump fsfe --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 2efff4d2..bd208038 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a +Subproject commit bd208038dd85a94dce8c763397ad5ac7eae76402 From a190d99f6d4e174f456948ded64baec1540bdc78 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 Feb 2023 19:45:41 +0100 Subject: [PATCH 49/60] auto-formatter --- bsp_q7s/core/ObjectFactory.cpp | 6 +-- linux/devices/CMakeLists.txt | 5 ++- mission/devices/ImtqHandler.cpp | 2 +- mission/system/objects/ComSubsystem.cpp | 23 +++++----- mission/system/objects/ComSubsystem.h | 30 ++++++------- mission/system/tree/comModeTree.cpp | 60 ++++++++++++++----------- mission/system/tree/comModeTree.h | 2 +- mission/system/tree/system.cpp | 3 +- mission/tmtc/CfdpTmFunnel.cpp | 2 +- mission/tmtc/PusTmFunnel.cpp | 2 +- mission/tmtc/VirtualChannel.cpp | 2 +- 11 files changed, 74 insertions(+), 63 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 63539cc3..59a17207 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -756,9 +756,9 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, new AxiPtmeConfig(objects::AXI_PTME_CONFIG, q7s::UIO_PTME, q7s::uiomapids::PTME_CONFIG); PtmeConfig* ptmeConfig = new PtmeConfig(objects::PTME_CONFIG, axiPtmeConfig); - *ipCoreHandler = new CcsdsIpCoreHandler( - objects::CCSDS_HANDLER, objects::PTME, objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, - gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA); + *ipCoreHandler = new CcsdsIpCoreHandler(objects::CCSDS_HANDLER, objects::PTME, + objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, gpioComIF, + gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA); VirtualChannel* vc = nullptr; vc = new VirtualChannel(ccsds::VC0, config::VC0_QUEUE_SIZE, objects::CCSDS_HANDLER); (*ipCoreHandler)->addVirtualChannel(ccsds::VC0, vc); diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 17d842ea..22b39840 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -3,8 +3,9 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER) endif() target_sources( - ${OBSW_NAME} PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp - ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) + ${OBSW_NAME} + PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp + ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) add_subdirectory(ploc) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 18c002c4..aa935ce5 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -256,7 +256,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result; ReturnValue_t status = returnvalue::OK; - if(getMode() != MODE_NORMAL) { + if (getMode() != MODE_NORMAL) { // Ignore replies during transitions. return returnvalue::OK; } diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp index 3a79ac15..a30f441b 100644 --- a/mission/system/objects/ComSubsystem.cpp +++ b/mission/system/objects/ComSubsystem.cpp @@ -10,7 +10,6 @@ #include - ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, uint32_t transmitterTimeout) : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) { @@ -116,11 +115,11 @@ void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) { transmitterCountdown.timeOut(); countdownActive = false; } else if (isTxMode(mode)) { - // Only start transmitter countdown if transmitter is not already on - if (not isTxMode(this->mode)) { - transmitterCountdown.resetTimer(); - countdownActive = true; - } + // Only start transmitter countdown if transmitter is not already on + if (not isTxMode(this->mode)) { + transmitterCountdown.resetTimer(); + countdownActive = true; + } } Subsystem::startTransition(mode, submode); } @@ -190,10 +189,10 @@ void ComSubsystem::checkTransmitterCountdown() { } bool ComSubsystem::isTxMode(Mode_t mode) { - if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) || - (mode == com::Submode::RX_AND_TX_LOW_DATARATE) || - (mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) { - return true; - } - return false; + if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) || + (mode == com::Submode::RX_AND_TX_LOW_DATARATE) || + (mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) { + return true; + } + return false; } diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 6b5b41dc..50f34b0d 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -1,24 +1,25 @@ #ifndef MISSION_SYSTEM_COMSUBSYSTEM_H_ #define MISSION_SYSTEM_COMSUBSYSTEM_H_ +#include #include #include #include -#include #include "mission/comDefs.h" class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { public: - /** - * @brief Constructor - * - * @param setObjectId - * @param maxNumberOfSequences - * @param maxNumberOfTables - * @param transmitterTimeout Maximum time the transmitter of the syrlinks - * will be enabled - */ + /** + * @brief Constructor + * + * @param setObjectId + * @param maxNumberOfSequences + * @param maxNumberOfTables + * @param transmitterTimeout Maximum time the transmitter of the syrlinks + * will be + * enabled + */ ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, uint32_t transmitterTimeout); virtual ~ComSubsystem() = default; @@ -30,7 +31,6 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { virtual void performChildOperation() override; private: - static const Mode_t INITIAL_MODE = 0; ReturnValue_t handleCommandMessage(CommandMessage *message) override; @@ -40,7 +40,7 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { void startTransition(Mode_t mode, Submode_t submode) override; void readEventQueue(); - void handleEventMessage(EventMessage* eventMessage); + void handleEventMessage(EventMessage *eventMessage); void handleBitLockEvent(); void handleCarrierLockEvent(); void checkTransmitterCountdown(); @@ -56,11 +56,11 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { uint8_t datarateCfg = static_cast(com::Datarate::LOW_RATE_MODULATION_BPSK); // Maximum time after which the transmitter will be turned of. This is a - // protection mechanism due prevent the syrlinks from overheating - uint32_t transmitterTimeout = 0; + // protection mechanism due prevent the syrlinks from overheating + uint32_t transmitterTimeout = 0; ParameterHelper paramHelper; - MessageQueueIF* eventQueue = nullptr; + MessageQueueIF *eventQueue = nullptr; bool enableTxWhenCarrierLock = false; diff --git a/mission/system/tree/comModeTree.cpp b/mission/system/tree/comModeTree.cpp index 222d1f7d..9dc85a78 100644 --- a/mission/system/tree/comModeTree.cpp +++ b/mission/system/tree/comModeTree.cpp @@ -10,7 +10,8 @@ const auto check = subsystem::checkInsert; -ComSubsystem satsystem::com::SUBSYSTEM = ComSubsystem(objects::COM_SUBSYSTEM, 12, 24, TRANSMITTER_TIMEOUT); +ComSubsystem satsystem::com::SUBSYSTEM = + ComSubsystem(objects::COM_SUBSYSTEM, 12, 24, TRANSMITTER_TIMEOUT); static const auto OFF = HasModesIF::MODE_OFF; static const auto ON = HasModesIF::MODE_ON; @@ -18,39 +19,48 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL; auto COM_SEQUENCE_RX_ONLY = std::make_pair(::com::Submode::RX_ONLY, FixedArrayList()); -auto COM_TABLE_RX_ONLY_TGT = - std::make_pair(static_cast(::com::Submode::RX_ONLY << 24) | 1, FixedArrayList()); -auto COM_TABLE_RX_ONLY_TRANS_0 = - std::make_pair(static_cast(::com::Submode::RX_ONLY << 24) | 2, FixedArrayList()); -auto COM_TABLE_RX_ONLY_TRANS_1 = - std::make_pair(static_cast(::com::Submode::RX_ONLY << 24) | 3, FixedArrayList()); +auto COM_TABLE_RX_ONLY_TGT = std::make_pair( + static_cast(::com::Submode::RX_ONLY << 24) | 1, FixedArrayList()); +auto COM_TABLE_RX_ONLY_TRANS_0 = std::make_pair( + static_cast(::com::Submode::RX_ONLY << 24) | 2, FixedArrayList()); +auto COM_TABLE_RX_ONLY_TRANS_1 = std::make_pair( + static_cast(::com::Submode::RX_ONLY << 24) | 3, FixedArrayList()); auto COM_SEQUENCE_RX_AND_TX_LOW_RATE = std::make_pair(::com::Submode::RX_AND_TX_LOW_DATARATE, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3, FixedArrayList()); +auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1, + FixedArrayList()); +auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2, + FixedArrayList()); +auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3, + FixedArrayList()); auto COM_SEQUENCE_RX_AND_TX_HIGH_RATE = std::make_pair(::com::Submode::RX_AND_TX_HIGH_DATARATE, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3, FixedArrayList()); +auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1, + FixedArrayList()); +auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2, + FixedArrayList()); +auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3, + FixedArrayList()); auto COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE = std::make_pair(::com::Submode::RX_AND_TX_DEFAULT_DATARATE, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2, FixedArrayList()); -auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 = std::make_pair( - static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3, FixedArrayList()); +auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1, + FixedArrayList()); +auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2, + FixedArrayList()); +auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 = + std::make_pair(static_cast(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3, + FixedArrayList()); namespace { diff --git a/mission/system/tree/comModeTree.h b/mission/system/tree/comModeTree.h index 8641874e..9260e3ea 100644 --- a/mission/system/tree/comModeTree.h +++ b/mission/system/tree/comModeTree.h @@ -1,8 +1,8 @@ #ifndef MISSION_SYSTEM_TREE_COMMODETREE_H_ #define MISSION_SYSTEM_TREE_COMMODETREE_H_ -#include #include +#include namespace satsystem { diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp index cb6202f1..5ec10a8c 100644 --- a/mission/system/tree/system.cpp +++ b/mission/system/tree/system.cpp @@ -3,10 +3,11 @@ #include #include #include -#include "mission/comDefs.h" + #include "acsModeTree.h" #include "comModeTree.h" #include "eive/objects.h" +#include "mission/comDefs.h" #include "payloadModeTree.h" #include "tcsModeTree.h" #include "util.h" diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index 779e9aa8..32dba7fe 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -21,7 +21,7 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) { break; } count++; - if(count == 500) { + if (count == 500) { sif::error << "CfdpTmFunnel: Possible message storm detected" << std::endl; break; } diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index e239afc9..974cdca0 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -21,7 +21,7 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { break; } count++; - if(count == 500) { + if (count == 500) { sif::error << "PusTmFunnel: Possible message storm detected" << std::endl; break; } diff --git a/mission/tmtc/VirtualChannel.cpp b/mission/tmtc/VirtualChannel.cpp index 64c7b006..6a24cc09 100644 --- a/mission/tmtc/VirtualChannel.cpp +++ b/mission/tmtc/VirtualChannel.cpp @@ -50,7 +50,7 @@ ReturnValue_t VirtualChannel::performOperation() { } count++; - if(count == 500) { + if (count == 500) { sif::error << "VirtualChannel: Possible message storm detected" << std::endl; break; } From d32ce3c5631f0c682e1c60563be1b569b801059b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 Feb 2023 19:48:44 +0100 Subject: [PATCH 50/60] changelog --- CHANGELOG.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 02840492..e075c5f3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,18 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +COM PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/364 + +* Moved transmitter timer and handling of carrier and bitlock event from CCSDS handler to COM + subsystem +* Added parameter command to be able to change the transmitter timeout +* Solves [#362](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/362) +* Solves [#360](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/360) +* Solves [#361](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/361) +* Solves [#386](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/386) + # [v1.30.0] eive-tmtc: v2.14.0 From 9f83a4969084e483030da420476fca391be73e1b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 11:46:30 +0100 Subject: [PATCH 51/60] remove shadowed member variables --- fsfw | 2 +- mission/system/objects/AcsBoardAssembly.h | 2 -- mission/system/objects/SusAssembly.h | 2 -- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/fsfw b/fsfw index 2efff4d2..bd208038 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a +Subproject commit bd208038dd85a94dce8c763397ad5ac7eae76402 diff --git a/mission/system/objects/AcsBoardAssembly.h b/mission/system/objects/AcsBoardAssembly.h index 1fe3336d..0c25396f 100644 --- a/mission/system/objects/AcsBoardAssembly.h +++ b/mission/system/objects/AcsBoardAssembly.h @@ -108,8 +108,6 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { static constexpr pcdu::Switches SWITCH_A = pcdu::Switches::PDU1_CH7_ACS_A_SIDE_3V3; static constexpr pcdu::Switches SWITCH_B = pcdu::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3; - bool tryingOtherSide = false; - bool dualModeErrorSwitch = true; AcsBoardHelper helper; GpioIF* gpioIF = nullptr; diff --git a/mission/system/objects/SusAssembly.h b/mission/system/objects/SusAssembly.h index 673e91f5..e95803df 100644 --- a/mission/system/objects/SusAssembly.h +++ b/mission/system/objects/SusAssembly.h @@ -50,8 +50,6 @@ class SusAssembly : public DualLaneAssemblyBase { SusAssHelper helper; PowerSwitchIF* pwrSwitcher = nullptr; - bool tryingOtherSide = false; - bool dualModeErrorSwitch = true; ReturnValue_t initialize() override; // AssemblyBase overrides From ba9268aae6facfe45256c00702ba4d29cc763e78 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 11:49:13 +0100 Subject: [PATCH 52/60] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3a29477a..7433bb7c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -39,6 +39,8 @@ will consitute of a breaking change warranting a new major release: - Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter` - Removed unused variables in the `AcsController` +- Remove shadowing variables inside ACS assembly classes. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/385 # [v1.30.0] From bd60255220ab958a67fa971bf1329dab5d2b6ac4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 11:57:12 +0100 Subject: [PATCH 53/60] interior structure changes, use new sched block --- CHANGELOG.md | 2 ++ bsp_q7s/core/scheduling.cpp | 2 +- bsp_q7s/core/scheduling.h | 3 +-- fsfw | 2 +- linux/devices/CMakeLists.txt | 5 +++-- linux/fsfwconfig/CMakeLists.txt | 3 +-- mission/controller/acs/Guidance.cpp | 3 +-- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 7 +++---- mission/core/CMakeLists.txt | 3 ++- .../core/pollingSeqTables.cpp | 4 ++-- .../core/pollingSeqTables.h | 0 mission/devices/ImtqHandler.cpp | 2 +- mission/tmtc/CfdpTmFunnel.cpp | 2 +- mission/tmtc/PusTmFunnel.cpp | 2 +- mission/tmtc/VirtualChannel.cpp | 2 +- 15 files changed, 21 insertions(+), 21 deletions(-) rename linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp => mission/core/pollingSeqTables.cpp (99%) rename linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h => mission/core/pollingSeqTables.h (100%) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3a29477a..2fdfa306 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ will consitute of a breaking change warranting a new major release: - All `targetQuat` functions in `Guidance` now return the target quaternion (target in ECI frame), which is passed on to `CtrlValData`. +- Moved polling sequence table definitions and source code to `mission/core` folder. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/395 ## Added diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index db6f3c91..6dc7fceb 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -17,10 +17,10 @@ #include "fsfw/tasks/FixedTimeslotTaskIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" #include "fsfw/tasks/TaskFactory.h" +#include "mission/core/pollingSeqTables.h" #include "mission/core/scheduling.h" #include "mission/devices/devicedefinitions/Max31865Definitions.h" #include "mission/utility/InitMission.h" -#include "pollingsequence/pollingSequenceFactory.h" /* This is configured for linux without CR */ #ifdef PLATFORM_UNIX diff --git a/bsp_q7s/core/scheduling.h b/bsp_q7s/core/scheduling.h index 2241e683..e49ce036 100644 --- a/bsp_q7s/core/scheduling.h +++ b/bsp_q7s/core/scheduling.h @@ -1,11 +1,10 @@ #ifndef BSP_Q7S_INITMISSION_H_ #define BSP_Q7S_INITMISSION_H_ -#include - #include #include "fsfw/tasks/definitions.h" +#include "mission/core/pollingSeqTables.h" using pst::AcsPstCfg; diff --git a/fsfw b/fsfw index 2efff4d2..bd208038 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a +Subproject commit bd208038dd85a94dce8c763397ad5ac7eae76402 diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 17d842ea..22b39840 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -3,8 +3,9 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER) endif() target_sources( - ${OBSW_NAME} PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp - ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) + ${OBSW_NAME} + PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp + ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) add_subdirectory(ploc) diff --git a/linux/fsfwconfig/CMakeLists.txt b/linux/fsfwconfig/CMakeLists.txt index bfede452..d5b5e7e5 100644 --- a/linux/fsfwconfig/CMakeLists.txt +++ b/linux/fsfwconfig/CMakeLists.txt @@ -1,5 +1,4 @@ -target_sources(${OBSW_NAME} PRIVATE ipc/MissionMessageTypes.cpp - pollingsequence/pollingSequenceFactory.cpp) +target_sources(${OBSW_NAME} PRIVATE ipc/MissionMessageTypes.cpp) target_include_directories(${OBSW_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 2346d18f..031cd384 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -111,8 +111,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve if (sightAngleSun < critSightAngle) { strBlindAvoidFlag = true; } - } - else { + } else { if (sightAngleSun < blindEnd * exclAngle) { double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; double blindRefRate[3] = {0, 0, 0}; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index d90f3f51..5bae4624 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1098,7 +1098,7 @@ void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { } void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, - MekfStatus mekfStatus) { + MekfStatus mekfStatus) { { PoolReadGuard pg(mekfData); if (pg.getReadResult() == returnvalue::OK) { @@ -1114,9 +1114,8 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek } } -void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, - MekfStatus mekfStatus, double quat[4], - double satRotRate[3]) { +void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, + double quat[4], double satRotRate[3]) { { PoolReadGuard pg(mekfData); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/core/CMakeLists.txt b/mission/core/CMakeLists.txt index fc6e1cff..ce731219 100644 --- a/mission/core/CMakeLists.txt +++ b/mission/core/CMakeLists.txt @@ -1 +1,2 @@ -target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp) +target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp + pollingSeqTables.cpp) diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/mission/core/pollingSeqTables.cpp similarity index 99% rename from linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp rename to mission/core/pollingSeqTables.cpp index 1f3e65de..0b134d87 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -1,4 +1,4 @@ -#include "pollingSequenceFactory.h" +#include "pollingSeqTables.h" #include #include @@ -659,7 +659,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::GET_READ); } - thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0); + thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::SCHED_BLOCK_RTD_PERIOD, 0); return returnvalue::OK; } diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/mission/core/pollingSeqTables.h similarity index 100% rename from linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h rename to mission/core/pollingSeqTables.h diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 18c002c4..aa935ce5 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -256,7 +256,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t result; ReturnValue_t status = returnvalue::OK; - if(getMode() != MODE_NORMAL) { + if (getMode() != MODE_NORMAL) { // Ignore replies during transitions. return returnvalue::OK; } diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index 779e9aa8..32dba7fe 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -21,7 +21,7 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) { break; } count++; - if(count == 500) { + if (count == 500) { sif::error << "CfdpTmFunnel: Possible message storm detected" << std::endl; break; } diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index e239afc9..974cdca0 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -21,7 +21,7 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { break; } count++; - if(count == 500) { + if (count == 500) { sif::error << "PusTmFunnel: Possible message storm detected" << std::endl; break; } diff --git a/mission/tmtc/VirtualChannel.cpp b/mission/tmtc/VirtualChannel.cpp index 64c7b006..6a24cc09 100644 --- a/mission/tmtc/VirtualChannel.cpp +++ b/mission/tmtc/VirtualChannel.cpp @@ -50,7 +50,7 @@ ReturnValue_t VirtualChannel::performOperation() { } count++; - if(count == 500) { + if (count == 500) { sif::error << "VirtualChannel: Possible message storm detected" << std::endl; break; } From cd988753ae8e3a0b5fa2d0d2e1f759e8fee8236f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:14:40 +0100 Subject: [PATCH 54/60] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index be032ab9..4eb470c7 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit be032ab925cab58bc443dd877b8260d291894802 +Subproject commit 4eb470c72447fb90f74ee33681d1e9e75c932d0f From b60891ddd169ca737c172627d3f97ae656006393 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:14:56 +0100 Subject: [PATCH 55/60] tmtc bump --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7a0061dd..8de42fd1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +eive-tmtc: v2.15.0 + ## Fixed - Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter` From 9fc6386ac5abfa0b5077a1737b96a272c7b96aa3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:19:56 +0100 Subject: [PATCH 56/60] bump tmtc again --- CHANGELOG.md | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8de42fd1..5da3c93e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,7 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -eive-tmtc: v2.15.0 +eive-tmtc: v2.15.1 ## Fixed diff --git a/tmtc b/tmtc index 4eb470c7..7e06043d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 4eb470c72447fb90f74ee33681d1e9e75c932d0f +Subproject commit 7e06043df736e167323f7df701291ee25071eba9 From ca743ccd6c80c1df6dc01d3b2c04c6efb8f98c87 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:22:11 +0100 Subject: [PATCH 57/60] re-run generators --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 10 ++++++++-- bsp_hosted/fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_events.csv | 2 ++ generators/bsp_hosted_returnvalues.csv | 10 +++++++--- generators/bsp_q7s_events.csv | 2 ++ generators/bsp_q7s_returnvalues.csv | 10 +++++++--- generators/events/translateEvents.cpp | 10 ++++++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 10 ++++++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 11 files changed, 46 insertions(+), 16 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 39d4c89a..5e660026 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 258 translations. + * @brief Auto-generated event translation file. Contains 260 translations. * @details - * Generated on: 2023-02-22 15:00:34 + * Generated on: 2023-02-23 15:20:41 */ #include "translateEvents.h" @@ -93,6 +93,8 @@ const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION"; const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; +const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; +const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -437,6 +439,10 @@ const char *translateEvents(Event event) { return SAFE_RATE_RECOVERY_STRING; case (11202): return MULTIPLE_RW_INVALID_STRING; + case (11203): + return MEKF_INVALID_INFO_STRING; + case (11204): + return MEKF_INVALID_MODE_VIOLATION_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 4a9083e3..3d4c4d42 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2023-02-22 15:00:34 + * Generated on: 2023-02-23 15:20:41 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index f1188b4e..4542daa6 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -87,6 +87,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h 11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h +11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h +11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 0f1de940..2b045d89 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -53,9 +53,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h 0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h 0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h -0x6901;ACSKAL_KalmanNoGyrMeas;No description;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6902;ACSKAL_KalmanNoModel;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6903;ACSKAL_KalmanInversionFailed;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6902;ACSKAL_KalmanUninitialized;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6903;ACSKAL_KalmanNoGyrData;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6904;ACSKAL_KalmanNoModelVectors;No description;4;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6905;ACSKAL_KalmanNoSusMgmStrData;No description;5;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6906;ACSKAL_KalmanCovarianceInversionFailed;No description;6;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6907;ACSKAL_KalmanInitialized;No description;7;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6908;ACSKAL_KalmanRunning;No description;8;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h 0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index f1188b4e..4542daa6 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -87,6 +87,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h 11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h +11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h +11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 5b1dd409..449307cd 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -53,9 +53,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h 0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h 0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h -0x6901;ACSKAL_KalmanNoGyrMeas;No description;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6902;ACSKAL_KalmanNoModel;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6903;ACSKAL_KalmanInversionFailed;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6902;ACSKAL_KalmanUninitialized;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6903;ACSKAL_KalmanNoGyrData;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6904;ACSKAL_KalmanNoModelVectors;No description;4;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6905;ACSKAL_KalmanNoSusMgmStrData;No description;5;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6906;ACSKAL_KalmanCovarianceInversionFailed;No description;6;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6907;ACSKAL_KalmanInitialized;No description;7;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6908;ACSKAL_KalmanRunning;No description;8;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h 0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 39d4c89a..5e660026 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 258 translations. + * @brief Auto-generated event translation file. Contains 260 translations. * @details - * Generated on: 2023-02-22 15:00:34 + * Generated on: 2023-02-23 15:20:41 */ #include "translateEvents.h" @@ -93,6 +93,8 @@ const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION"; const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; +const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; +const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -437,6 +439,10 @@ const char *translateEvents(Event event) { return SAFE_RATE_RECOVERY_STRING; case (11202): return MULTIPLE_RW_INVALID_STRING; + case (11203): + return MEKF_INVALID_INFO_STRING; + case (11204): + return MEKF_INVALID_MODE_VIOLATION_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index a5100ae1..68273aa5 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 153 translations. - * Generated on: 2023-02-22 15:00:34 + * Generated on: 2023-02-23 15:20:41 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 39d4c89a..5e660026 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 258 translations. + * @brief Auto-generated event translation file. Contains 260 translations. * @details - * Generated on: 2023-02-22 15:00:34 + * Generated on: 2023-02-23 15:20:41 */ #include "translateEvents.h" @@ -93,6 +93,8 @@ const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION"; const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; +const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; +const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -437,6 +439,10 @@ const char *translateEvents(Event event) { return SAFE_RATE_RECOVERY_STRING; case (11202): return MULTIPLE_RW_INVALID_STRING; + case (11203): + return MEKF_INVALID_INFO_STRING; + case (11204): + return MEKF_INVALID_MODE_VIOLATION_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index a5100ae1..68273aa5 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 153 translations. - * Generated on: 2023-02-22 15:00:34 + * Generated on: 2023-02-23 15:20:41 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 7e06043d..a04bd8aa 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 7e06043df736e167323f7df701291ee25071eba9 +Subproject commit a04bd8aa738ca14cb3d5badffd00e91047730d5e From 89f6314a183b87bf421d2698ec0704606688de18 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:32:40 +0100 Subject: [PATCH 58/60] prep v1.31.0 --- CHANGELOG.md | 4 +++- CMakeLists.txt | 2 +- tmtc | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fb5ea9d0..32b638d0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,9 @@ will consitute of a breaking change warranting a new major release: # [unreleased] -eive-tmtc: v2.15.1 +# [v1.31.0] + +eive-tmtc: v2.15.2 ## Fixed diff --git a/CMakeLists.txt b/CMakeLists.txt index 44dce9b9..1c698dfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 1) -set(OBSW_VERSION_MINOR 30) +set(OBSW_VERSION_MINOR 31) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) diff --git a/tmtc b/tmtc index a04bd8aa..0bb1f575 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a04bd8aa738ca14cb3d5badffd00e91047730d5e +Subproject commit 0bb1f57599bfa0120e527fb24ffddae425ef1910 From 91af3ac497b520932aa9adb71c937c4fbbbcc5d2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:40:06 +0100 Subject: [PATCH 59/60] skip private retval --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 2 +- bsp_hosted/fsfwconfig/objects/translateObjects.cpp | 2 +- generators/bsp_hosted_returnvalues.csv | 1 - generators/bsp_q7s_returnvalues.csv | 1 - generators/events/translateEvents.cpp | 2 +- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 2 +- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- mission/system/objects/Stack5VHandler.h | 1 + tmtc | 2 +- 10 files changed, 8 insertions(+), 9 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 5e660026..e35a31ef 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 260 translations. * @details - * Generated on: 2023-02-23 15:20:41 + * Generated on: 2023-02-23 15:39:20 */ #include "translateEvents.h" diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 3d4c4d42..63e56ef2 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2023-02-23 15:20:41 + * Generated on: 2023-02-23 15:39:20 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 2b045d89..4c6d37e8 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -2,7 +2,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h -0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h 0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 449307cd..6f215a94 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -2,7 +2,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h -0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h 0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h 0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 5e660026..e35a31ef 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 260 translations. * @details - * Generated on: 2023-02-23 15:20:41 + * Generated on: 2023-02-23 15:39:20 */ #include "translateEvents.h" diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 68273aa5..70bcf3ad 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 153 translations. - * Generated on: 2023-02-23 15:20:41 + * Generated on: 2023-02-23 15:39:20 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 5e660026..e35a31ef 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 260 translations. * @details - * Generated on: 2023-02-23 15:20:41 + * Generated on: 2023-02-23 15:39:20 */ #include "translateEvents.h" diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 68273aa5..70bcf3ad 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 153 translations. - * Generated on: 2023-02-23 15:20:41 + * Generated on: 2023-02-23 15:39:20 */ #include "translateObjects.h" diff --git a/mission/system/objects/Stack5VHandler.h b/mission/system/objects/Stack5VHandler.h index 7ade81e5..46fc963a 100644 --- a/mission/system/objects/Stack5VHandler.h +++ b/mission/system/objects/Stack5VHandler.h @@ -10,6 +10,7 @@ enum class HandlerState { SWITCH_PENDING, IDLE }; class Stack5VHandler { public: + //! [EXPORT] : [SKIP] static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0); Stack5VHandler(PowerSwitchIF& switcher); diff --git a/tmtc b/tmtc index 0bb1f575..75a8712f 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0bb1f57599bfa0120e527fb24ffddae425ef1910 +Subproject commit 75a8712f9124ca1ea51f12aed4d66f6946d8231c From 152a9b2dce8953144d897dbf592fc4eb0a9645de Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 15:46:53 +0100 Subject: [PATCH 60/60] bump eive-tmtc again --- CHANGELOG.md | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 32b638d0..210e8bc4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,7 +18,7 @@ will consitute of a breaking change warranting a new major release: # [v1.31.0] -eive-tmtc: v2.15.2 +eive-tmtc: v2.16.0 ## Fixed diff --git a/tmtc b/tmtc index 75a8712f..24f0d8e1 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 75a8712f9124ca1ea51f12aed4d66f6946d8231c +Subproject commit 24f0d8e1a6a8ea1323623932e699326214c78159