diff --git a/CHANGELOG.md b/CHANGELOG.md index b772640d..ecbb7302 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,9 +19,39 @@ change warranting a new major release: ## Changed +- Igrf13 model vector now outputs as uT instead of nT + +# [v1.27.2] 2023-02-14 + +Reaction Wheel handling was determined to be (quasi) broken and needs to be fixed in future release +to be usable by ACS controller. + +eive-tmtc: v2.15.6 + +## Added + +- Function for the ACS controller to command MTQ and RWs called by all subroutines +- RwHandler now handles commanding of RW speeds via RwSpeedActuationSet +- Tracing supports which allows checking whether threads are running as usual. + +## Changed + - Remove 2 TCS threads. - Move low level polling into ACS PST, move high level device handlers into TCS system task. -- Igrf13 model vector now outputs as uT instead of nT +- ActCmds now returns command vectors as integers as required by the actuators + and scales them to the appropriate range +- All RwHandler are now polled five times per ACS cycle +- Remove 2 TCS threads. Move low level polling into ACS PST, move high level device handlers into + TCS system task. +- Further reduce number of threads: + 1. Remove PUS low priority task, move assigned threads to the generic system task + 2. Group events and verification tasks into PUS high priority task + 3. Group all other components into PUS medium priority task + 4. Add SCEX device handler to PL task, remove dedicated thread + +## Removed + +- lwgps dependency not compiled anymore, is not used # [v1.27.1] 2023-02-13 diff --git a/CMakeLists.txt b/CMakeLists.txt index 782993be..4f62c199 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 1) set(OBSW_VERSION_MINOR 27) -set(OBSW_VERSION_REVISION 1) +set(OBSW_VERSION_REVISION 2) # set(CMAKE_VERBOSE TRUE) @@ -424,7 +424,6 @@ add_subdirectory(${BSP_PATH}) add_subdirectory(${COMMON_PATH}) add_subdirectory(${DUMMY_PATH}) -add_subdirectory(${LIB_LWGPS_PATH}) add_subdirectory(${FSFW_PATH}) add_subdirectory(${LIB_EIVE_MISSION_PATH}) add_subdirectory(${TEST_PATH}) @@ -491,8 +490,8 @@ endif() # ############################################################################## # Add libraries -target_link_libraries(${LIB_EIVE_MISSION} - PUBLIC ${LIB_FSFW_NAME} ${LIB_LWGPS_NAME} ${LIB_OS_NAME}) +target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_FSFW_NAME} + ${LIB_OS_NAME}) target_link_libraries(${LIB_DUMMIES} PUBLIC ${LIB_FSFW_NAME} ${LIB_JSON_NAME}) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 116b4fc9..4ff4965f 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -63,6 +63,9 @@ ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) { } void CoreController::performControlOperation() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "CORE CTRL"); +#endif EventMessage event; for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 9cbc6340..65ee20ef 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -12,6 +12,7 @@ #include "events/subsystemIdRanges.h" #include "fsfw/controller/ExtendedControllerBase.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" +#include "mission/trace.h" class Timer; class SdCardManager; @@ -222,6 +223,9 @@ class CoreController : public ExtendedControllerBase { std::string currMntPrefix; bool performOneShotSdCardOpsSwitch = false; uint8_t shortSdCardCdCounter = 0; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter; +#endif Countdown sdCardCheckCd = Countdown(INIT_SD_CARD_CHECK_TIMEOUT); /** diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2bf36743..2631a628 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -683,8 +683,8 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback::spiCallback, nullptr); auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, - rwGpioIds[idx]); - rwCookies[idx]->setCallbackArgs(rws[idx]); + rwGpioIds[idx], idx); + rwCookies[idx]->setCallbackArgs(rwHandler); #if OBSW_TEST_RW == 1 rws[idx]->setStartUpImmediately(); #endif diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index ad76d308..2fa8f015 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -154,6 +154,14 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); } + result = genericSysTask->addComponent(objects::INTERNAL_ERROR_REPORTER); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); + } + result = genericSysTask->addComponent(objects::PUS_SERVICE_17_TEST); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); + } #if OBSW_ADD_CCSDS_IP_CORES == 1 result = genericSysTask->addComponent(objects::CCSDS_HANDLER); @@ -266,14 +274,16 @@ void scheduling::initTasks() { #endif /* OBSW_ADD_PLOC_SUPERVISOR */ PeriodicTaskIF* plTask = factory->createPeriodicTask( - "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); - scheduling::addMpsocSupvHandlers(plTask); + "PL_TASK", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); plTask->addComponent(objects::CAM_SWITCHER); + scheduling::addMpsocSupvHandlers(plTask); +#if OBSW_ADD_SCEX_DEVICE == 1 + scheduling::scheduleScexDev(plTask); +#endif #if OBSW_ADD_SCEX_DEVICE == 1 - PeriodicTaskIF* scexDevHandler; PeriodicTaskIF* scexReaderTask; - scheduling::schedulingScex(*factory, scexDevHandler, scexReaderTask); + scheduling::scheduleScexReader(*factory, scexReaderTask); #endif std::vector pusTasks; @@ -330,7 +340,6 @@ void scheduling::initTasks() { taskStarter(pstTasks, "PST task vector"); taskStarter(pusTasks, "PUS task vector"); #if OBSW_ADD_SCEX_DEVICE == 1 - scexDevHandler->startTask(); scexReaderTask->startTask(); #endif @@ -374,9 +383,9 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction #else static constexpr float acsPstPeriod = 0.8; #endif - FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask( - "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); - result = pst::pstTcsAndAcs(acsPst, cfg); + FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask( + "ACS_TCS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); + result = pst::pstTcsAndAcs(acsTcsPst, cfg); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "scheduling::initTasks: ACS PST is empty" << std::endl; @@ -384,7 +393,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction sif::error << "scheduling::initTasks: Creating ACS PST failed!" << std::endl; } } else { - taskVec.push_back(acsPst); + taskVec.push_back(acsTcsPst); } /* Polling Sequence Table Default */ @@ -435,42 +444,28 @@ void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction std::vector& taskVec) { ReturnValue_t result = returnvalue::OK; /* PUS Services */ - PeriodicTaskIF* pusVerification = factory.createPeriodicTask( - "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); - result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); + PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( + "PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); + result = pusHighPrio->addComponent(objects::PUS_SERVICE_1_VERIFICATION); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION); } - taskVec.push_back(pusVerification); - - PeriodicTaskIF* pusEvents = factory.createPeriodicTask( - "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); - result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); + result = pusHighPrio->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } - result = pusEvents->addComponent(objects::EVENT_MANAGER); + result = pusHighPrio->addComponent(objects::EVENT_MANAGER); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } - taskVec.push_back(pusEvents); - - PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( - "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); - result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); - } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT); } - taskVec.push_back(pusHighPrio); PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); - result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_3", objects::PUS_SERVICE_3_HOUSEKEEPING); @@ -495,20 +490,11 @@ void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_201", objects::PUS_SERVICE_201_HEALTH); } - // Used for connection tests, therefore use higher priority - result = pusMedPrio->addComponent(objects::PUS_SERVICE_17_TEST); + result = pusMedPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); if (result != returnvalue::OK) { - scheduling::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); + scheduling::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } taskVec.push_back(pusMedPrio); - - PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( - "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); - result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); - } - taskVec.push_back(pusLowPrio); } void scheduling::createTestTasks(TaskFactory& factory, diff --git a/fsfw b/fsfw index d256ede8..9de6c4b3 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d256ede8c1d8e7a746d3a56d45313d2b863e0b28 +Subproject commit 9de6c4b3aa20ee63c28051d486be8a12df147f22 diff --git a/linux/boardtest/UartTestClass.h b/linux/boardtest/UartTestClass.h index b206de53..fd20e621 100644 --- a/linux/boardtest/UartTestClass.h +++ b/linux/boardtest/UartTestClass.h @@ -10,7 +10,7 @@ #include -#include "lwgps/lwgps.h" +//#include "lwgps/lwgps.h" #include "mission/devices/devicedefinitions/ScexDefinitions.h" #include "test/TestTask.h" @@ -59,7 +59,7 @@ class UartTestClass : public TestTask { DleEncoder dleEncoder = DleEncoder(); SerialCookie* uartCookie = nullptr; size_t encodedLen = 0; - lwgps_t gpsData = {}; + // lwgps_t gpsData = {}; struct termios tty = {}; int serialPort = 0; bool startFound = false; diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index afc76fcf..a6e0a1a3 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -102,6 +102,9 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) { handleQueue(); poolManager.performHkOperation(); while (true) { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "GPS CTRL"); +#endif bool callAgainImmediately = readGpsDataFromGpsd(); if (not callAgainImmediately) { handleQueue(); diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index 5d4a35ff..1e97bc29 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -6,6 +6,7 @@ #include "fsfw/controller/ExtendedControllerBase.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" +#include "mission/trace.h" #ifdef FSFW_OSAL_LINUX #include @@ -60,6 +61,9 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); bool modeCommanded = false; bool timeInit = false; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif struct OneShotSwitches { void reset() { diff --git a/linux/devices/ploc/PlocMPSoCHelper.cpp b/linux/devices/ploc/PlocMPSoCHelper.cpp index fcd5178b..fca2e51c 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.cpp +++ b/linux/devices/ploc/PlocMPSoCHelper.cpp @@ -3,7 +3,6 @@ #include #include -#include "OBSWConfig.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/FilesystemHelper.h" #endif @@ -34,6 +33,9 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { ReturnValue_t result = returnvalue::OK; semaphore.acquire(); while (true) { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "PLOC MPSOC Helper"); +#endif switch (internalState) { case InternalState::IDLE: { semaphore.acquire(); diff --git a/linux/devices/ploc/PlocMPSoCHelper.h b/linux/devices/ploc/PlocMPSoCHelper.h index c39cc758..dea35a82 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.h +++ b/linux/devices/ploc/PlocMPSoCHelper.h @@ -11,6 +11,7 @@ #include "fsfw/tmtcservices/SourceSequenceCounter.h" #include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" +#include "mission/trace.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" #endif @@ -116,6 +117,10 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { struct FlashWrite flashWrite; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + enum class InternalState { IDLE, FLASH_WRITE, FLASH_READ }; InternalState internalState = InternalState::IDLE; diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 1fd2b841..5113de40 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -101,6 +101,9 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { lock->unlockMutex(); semaphore->acquire(); putTaskToSleep = false; +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "PLOC SUPV Helper PST"); +#endif while (true) { if (putTaskToSleep) { performUartShutdown(); diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index e1539c97..02bfb6c7 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -15,6 +15,7 @@ #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" +#include "mission/trace.h" #include "tas/crc.h" #ifdef XIPHOS_Q7S @@ -211,6 +212,9 @@ class PlocSupvUartManager : public DeviceCommunicationIF, supv::TmBase tmReader; int serialPort = 0; struct termios tty = {}; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif struct EventBufferRequest { std::string path = ""; diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index b91fcd7a..e017021e 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -5,6 +5,7 @@ #include #include +#include "OBSWConfig.h" #include "eive/definitions.h" #include "mission/devices/devicedefinitions/Max31865Definitions.h" @@ -240,7 +241,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); } - if (enableBside) { // B side thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, @@ -293,7 +293,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg } } // SUS: 16 ms - bool addSus0 = true; bool addSus1 = true; bool addSus2 = true; @@ -441,7 +440,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus6) { thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -464,7 +462,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus7) { thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -487,7 +484,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus8) { thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -510,7 +506,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus9) { thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -522,6 +517,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_WRITE); @@ -532,7 +528,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus10) { thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -555,7 +550,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus11) { thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -639,6 +633,129 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg } if (cfg.scheduleRws) { + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + } + + if (cfg.scheduleRws) { + // this is the torquing cycle + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_WRITE); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, @@ -684,6 +801,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_READ); } + thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0); + return returnvalue::OK; } diff --git a/linux/scheduling.cpp b/linux/scheduling.cpp index f2e6ddec..85394dea 100644 --- a/linux/scheduling.cpp +++ b/linux/scheduling.cpp @@ -8,8 +8,7 @@ #include "ObjectFactory.h" #include "eive/objects.h" -void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, - PeriodicTaskIF*& scexReaderTask) { +void scheduling::scheduleScexReader(TaskFactory& factory, PeriodicTaskIF*& scexReaderTask) { using namespace scheduling; ReturnValue_t result = returnvalue::OK; #if OBSW_PRINT_MISSED_DEADLINES == 1 @@ -17,37 +16,6 @@ void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHa #else void (*missedDeadlineFunc)(void) = nullptr; #endif - scexDevHandler = factory.createPeriodicTask( - "SCEX_DEV", 35, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc); - - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::PERFORM_OPERATION); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_WRITE); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_WRITE); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } result = returnvalue::OK; scexReaderTask = factory.createPeriodicTask( @@ -79,3 +47,35 @@ void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) { plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); #endif } + +void scheduling::scheduleScexDev(PeriodicTaskIF*& scexDevHandler) { + ReturnValue_t result = + scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::PERFORM_OPERATION); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_WRITE); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_WRITE); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } +} diff --git a/linux/scheduling.h b/linux/scheduling.h index d33e9d1f..b5ec8ef2 100644 --- a/linux/scheduling.h +++ b/linux/scheduling.h @@ -3,7 +3,7 @@ #include namespace scheduling { -void schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, - PeriodicTaskIF*& scexReaderTask); +void scheduleScexDev(PeriodicTaskIF*& scexDevHandler); +void scheduleScexReader(TaskFactory& factory, PeriodicTaskIF*& scexReaderTask); void addMpsocSupvHandlers(PeriodicTaskIF* task); } // namespace scheduling diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index aaecb53e..0dfd812a 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,7 @@ - + @@ -119,7 +119,7 @@ - + @@ -187,7 +187,7 @@ - + @@ -255,7 +255,7 @@ - + @@ -418,7 +418,7 @@ - + @@ -580,7 +580,7 @@ - + @@ -750,7 +750,7 @@ - + @@ -917,7 +917,7 @@ - + @@ -1084,7 +1084,7 @@ - + @@ -1149,7 +1149,7 @@ - + @@ -1317,7 +1317,7 @@ - + @@ -1386,7 +1386,7 @@ - + @@ -1451,18 +1451,9 @@ - - - - - - - - - @@ -1478,6 +1469,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1490,25 +1523,16 @@ - - - - + - + - - - - - - - + diff --git a/mission/CMakeLists.txt b/mission/CMakeLists.txt index f284a675..37c4a2e2 100644 --- a/mission/CMakeLists.txt +++ b/mission/CMakeLists.txt @@ -9,4 +9,4 @@ add_subdirectory(csp) add_subdirectory(cfdp) add_subdirectory(config) -target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp) +target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp trace.cpp) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ef590b8c..220c477e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -50,6 +50,9 @@ ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, } void AcsController::performControlOperation() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "ACS & TCS PST"); +#endif switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); @@ -116,10 +119,10 @@ void AcsController::performSafe() { navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); - // Give desired satellite rate and sun direction to align + // give desired satellite rate and sun direction to align double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // IF MEKF is working + // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; if (validMekf == returnvalue::OK) { @@ -137,8 +140,8 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); { PoolReadGuard pg(&ctrlValData); @@ -180,18 +183,15 @@ void AcsController::performSafe() { actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], - // torqueDuration); - // } + + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + // acsParameters.rwHandlingParameters.rampTime); } void AcsController::performDetumble() { @@ -208,8 +208,8 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -228,10 +228,6 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); - } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { @@ -239,18 +235,15 @@ void AcsController::performDetumble() { actuatorCmdData.rwTargetTorque.setValid(false); std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } + + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + // acsParameters.rwHandlingParameters.rampTime); } void AcsController::performPointingCtrl() { @@ -273,7 +266,7 @@ void AcsController::performPointingCtrl() { guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + double mgtDpDes[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: @@ -393,44 +386,62 @@ void AcsController::performPointingCtrl() { if (enableAntiStiction) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = { - (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value}; ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } - double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); - } - int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; - for (int i = 0; i < 4; ++i) { - cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); - } + 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); { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.setValidity(true, true); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } + + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], + // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + // acsParameters.rwHandlingParameters.rampTime); +} + +ReturnValue_t AcsController::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) { + { + PoolReadGuard pg(&dipoleSet); + MutexGuard mg(torquer::lazyLock()); + torquer::NEW_ACTUATION_FLAG = true; + dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration); + } + { + PoolReadGuard pg(&rw1SpeedSet); + rw1SpeedSet.setRwSpeed(rw1Speed, rampTime); + } + { + PoolReadGuard pg(&rw2SpeedSet); + rw2SpeedSet.setRwSpeed(rw2Speed, rampTime); + } + { + PoolReadGuard pg(&rw3SpeedSet); + rw3SpeedSet.setRwSpeed(rw3Speed, rampTime); + } + { + PoolReadGuard pg(&rw4SpeedSet); + rw4SpeedSet.setRwSpeed(rw4Speed, rampTime); + } + return returnvalue::OK; } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1e017099..c0e719f2 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -17,8 +17,10 @@ #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" +#include "mission/trace.h" class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: @@ -51,6 +53,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ParameterHelper parameterHelper; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; @@ -69,11 +75,20 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + 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); + /* ACS Sensor Values */ ACS::SensorValues sensorValues; - /* ACS Datasets */ + /* ACS Actuation Datasets */ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); + RwDefinitions::RwSpeedActuationSet rw1SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW1); + RwDefinitions::RwSpeedActuationSet rw2SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW2); + RwDefinitions::RwSpeedActuationSet rw3SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW3); + RwDefinitions::RwSpeedActuationSet rw4SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW4); + /* ACS Datasets */ // MGMs acsctrl::MgmDataRaw mgmDataRaw; PoolEntry mgm0VecRaw = PoolEntry(3); diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 0521d9aa..53b7f13d 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -72,6 +72,9 @@ ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) { } void ThermalController::performControlOperation() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "TCS Task"); +#endif switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 03c7954c..6297b175 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -11,7 +11,8 @@ #include -#include "../devices/HeaterHandler.h" +#include "mission/devices/HeaterHandler.h" +#include "mission/trace.h" /** * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit @@ -152,6 +153,10 @@ class ThermalController : public ExtendedControllerBase { // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(DELAY); +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + std::array, 5> sensors; uint8_t numSensors = 0; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 13642fe9..44bed15f 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -278,6 +278,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x4: parameterWrapper->set(rwHandlingParameters.stictionTorque); break; + case 0x5: + parameterWrapper->set(rwHandlingParameters.rampTime); + break; default: return INVALID_IDENTIFIER_ID; } @@ -584,6 +587,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x5: parameterWrapper->set(magnetorquesParameter.DipolMax); break; + case 0x6: + parameterWrapper->set(magnetorquesParameter.torqueDuration); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 60efe01f..b237a0f2 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -792,6 +792,8 @@ class AcsParameters : public HasParametersIF { double stictionSpeed = 100; // 80; // RPM double stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; + + uint16_t rampTime = 10; } rwHandlingParameters; struct RwMatrices { @@ -910,6 +912,7 @@ class AcsParameters : public HasParametersIF { double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double DipolMax = 0.2; // [Am^2] + uint16_t torqueDuration = 300; // [ms] } magnetorquesParameter; struct DetumbleParameter { diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 01befd93..8cda84d6 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -31,27 +31,33 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { } } -void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, - const double *rwTorque, double *rwCmdSpeed) { +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double *rwTorque, int32_t *rwCmdSpeed) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; double deltaSpeed[4] = {0, 0, 0, 0}; double commandTime = acsParameters.onBoardParams.sampleTime, inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] double factor = commandTime / inertiaWheel * radToRpm; + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); - VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); + for (int i = 0; i < 4; i++) { + deltaSpeedInt[i] = std::round(deltaSpeed[i]); + } + VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); + VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) { +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { // Convert to actuator frame + double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, - dipolMoment, dipolMomentActuator, 3, 3, 1); + dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); // Scaling along largest element if dipol exceeds maximum double maxDipol = acsParameters.magnetorquesParameter.DipolMax; double maxValue = 0; @@ -62,8 +68,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu } if (maxValue > maxDipol) { double scalingFactor = maxDipol / maxValue; - VectorOperations::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3); + VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, + dipolMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3); + VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + for (int i = 0; i < 3; i++) { + dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 04e2b61f..969bd782 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -28,8 +28,8 @@ class ActuatorCmd { * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -37,7 +37,7 @@ class ActuatorCmd { * @param: dipolMoment given dipol moment in spacecraft frame * dipolMomentActuator resulting dipol moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator); + void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); protected: private: diff --git a/mission/devices/BpxBatteryHandler.cpp b/mission/devices/BpxBatteryHandler.cpp index 4e49bebe..922bcd3b 100644 --- a/mission/devices/BpxBatteryHandler.cpp +++ b/mission/devices/BpxBatteryHandler.cpp @@ -2,8 +2,6 @@ #include -#include "OBSWConfig.h" - BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), hkSet(this), cfgSet(this) {} @@ -280,3 +278,9 @@ void BpxBatteryHandler::setToGoToNormalMode(bool enable) { } void BpxBatteryHandler::setDebugMode(bool enable) { this->debugMode = enable; } + +void BpxBatteryHandler::performOperationHook() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "BPX BATT"); +#endif +} diff --git a/mission/devices/BpxBatteryHandler.h b/mission/devices/BpxBatteryHandler.h index ceb0ff8d..c42ed7fd 100644 --- a/mission/devices/BpxBatteryHandler.h +++ b/mission/devices/BpxBatteryHandler.h @@ -4,6 +4,7 @@ #include #include "devicedefinitions/BpxBatteryDefinitions.h" +#include "mission/trace.h" class BpxBatteryHandler : public DeviceHandlerBase { public: @@ -24,6 +25,10 @@ class BpxBatteryHandler : public DeviceHandlerBase { bool debugMode = false; bool goToNormalModeImmediately = false; uint8_t sentPingByte = BpxBattery::DEFAULT_PING_SENT_BYTE; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + BpxBatteryHk hkSet; DeviceCommandId_t lastCmd = DeviceHandlerIF::NO_COMMAND_ID; BpxBatteryCfg cfgSet; @@ -47,6 +52,7 @@ class BpxBatteryHandler : public DeviceHandlerBase { ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override; void fillCommandAndReplyMap() override; + void performOperationHook() override; ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 0e75c06d..93e9dae8 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -183,18 +183,20 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } ReturnValue_t result; - // Commands override anything which was set in the software - if (commandData != nullptr) { - dipoleSet.setValidityBufferGeneration(false); - result = - dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); - dipoleSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - } else { + { // Read set dipole values from local pool PoolReadGuard pg(&dipoleSet); + + // Commands override anything which was set in the software + if (commandData != nullptr) { + dipoleSet.setValidityBufferGeneration(false); + result = dipoleSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } } if (ACTUATION_WIRETAPPING) { sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 732bb9fe..9d4a7255 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -7,13 +7,15 @@ #include "OBSWConfig.h" RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - GpioIF* gpioComIF, gpioId_t enableGpio) + GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx) : DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio), statusSet(this), lastResetStatusSet(this), - tmDataset(this) { + tmDataset(this), + rwSpeedActuationSet(*this), + rwIdx(rwIdx) { if (comCookie == nullptr) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } @@ -42,6 +44,10 @@ void RwHandler::doShutDown() { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { + case InternalState::SET_SPEED: + *id = RwDefinitions::SET_SPEED; + internalState = InternalState::GET_RESET_STATUS; + break; case InternalState::GET_RESET_STATUS: *id = RwDefinitions::GET_LAST_RESET_STATUS; internalState = InternalState::READ_TEMPERATURE; @@ -52,7 +58,7 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { break; case InternalState::GET_RW_SATUS: *id = RwDefinitions::GET_RW_STATUS; - internalState = InternalState::GET_RESET_STATUS; + internalState = InternalState::CLEAR_RESET_STATUS; break; case InternalState::CLEAR_RESET_STATUS: *id = RwDefinitions::CLEAR_LAST_RESET_STATUS; @@ -97,16 +103,37 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand return returnvalue::OK; } case (RwDefinitions::SET_SPEED): { - if (commandDataLen != 6) { + if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; return SET_SPEED_COMMAND_INVALID_LENGTH; } - result = checkSpeedAndRampTime(commandData, commandDataLen); + + { + PoolReadGuard pg(&rwSpeedActuationSet); + // Commands override anything which was set in the software + if (commandData != nullptr) { + rwSpeedActuationSet.setValidityBufferGeneration(false); + result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + rwSpeedActuationSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } + } + if (ACTUATION_WIRETAPPING) { + int32_t speed = 0; + uint16_t rampTime = 0; + rwSpeedActuationSet.getRwSpeed(speed, rampTime); + sif::debug << "Actuating RW " << static_cast(rwIdx) << " with speed = " << speed + << " and rampTime = " << rampTime << std::endl; + } + result = checkSpeedAndRampTime(); if (result != returnvalue::OK) { return result; } - prepareSetSpeedCmd(commandData, commandDataLen); + result = prepareSetSpeedCmd(); return result; } case (RwDefinitions::GET_TEMPERATURE): { @@ -243,6 +270,9 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime); + localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry({0})); @@ -295,17 +325,15 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { rawPacketLen = 3; } -ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) { - int32_t speed = - *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3); - +ReturnValue_t RwHandler::checkSpeedAndRampTime() { + int32_t speed = 0; + uint16_t rampTime = 0; + rwSpeedActuationSet.getRwSpeed(speed, rampTime); if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) { sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl; return INVALID_SPEED; } - uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5); - if (rampTime < 10 || rampTime > 20000) { sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl; return INVALID_RAMP_TIME; @@ -314,23 +342,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_ return returnvalue::OK; } -void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) { +ReturnValue_t RwHandler::prepareSetSpeedCmd() { commandBuffer[0] = static_cast(RwDefinitions::SET_SPEED); - - /** Speed (0.1 RPM) */ - commandBuffer[1] = *(commandData + 3); - commandBuffer[2] = *(commandData + 2); - commandBuffer[3] = *(commandData + 1); - commandBuffer[4] = *commandData; - /** Ramp time (ms) */ - commandBuffer[5] = *(commandData + 5); - commandBuffer[6] = *(commandData + 4); + uint8_t* serPtr = commandBuffer + 1; + size_t serSize = 1; + rwSpeedActuationSet.setValidityBufferGeneration(false); + ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), + SerializeIF::Endianness::LITTLE); + rwSpeedActuationSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF); commandBuffer[7] = static_cast(crc & 0xFF); - commandBuffer[8] = static_cast(crc >> 8 & 0xFF); + commandBuffer[8] = static_cast((crc >> 8) & 0xFF); rawPacket = commandBuffer; rawPacketLen = 9; + return result; } void RwHandler::handleResetStatusReply(const uint8_t* packet) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index f9f66bd5..ee18960d 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -9,6 +9,8 @@ #include "events/subsystemIdRanges.h" #include "returnvalues/classIds.h" +static constexpr bool ACTUATION_WIRETAPPING = false; + class GpioIF; /** @@ -34,7 +36,7 @@ class RwHandler : public DeviceHandlerBase { * to high to enable the device. */ RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF, - gpioId_t enableGpio); + gpioId_t enableGpio, uint8_t rwIdx); void setDebugMode(bool enable); @@ -93,10 +95,21 @@ class RwHandler : public DeviceHandlerBase { RwDefinitions::StatusSet statusSet; RwDefinitions::LastResetSatus lastResetStatusSet; RwDefinitions::TmDataset tmDataset; + RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet; uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; + uint8_t rwIdx; - enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS }; + PoolEntry rwSpeed = PoolEntry({0}); + PoolEntry rampTime = PoolEntry({10}); + + enum class InternalState { + GET_RESET_STATUS, + CLEAR_RESET_STATUS, + READ_TEMPERATURE, + SET_SPEED, + GET_RW_SATUS + }; InternalState internalState = InternalState::GET_RESET_STATUS; @@ -114,13 +127,14 @@ class RwHandler : public DeviceHandlerBase { * range. * @return returnvalue::OK if successful, otherwise error code. */ - ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen); + ReturnValue_t checkSpeedAndRampTime(); /** - * @brief This function prepares the set speed command from the commandData received with - * an action message. + * @brief This function prepares the set speed command from the dataSet received with + * an action message or set in the software. + * @return returnvalue::OK if successful, otherwise error code. */ - void prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen); + ReturnValue_t prepareSetSpeedCmd(); /** * @brief This function writes the last reset status retrieved with the get last reset status diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index 2b4e22ac..77c826fd 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -11,6 +11,7 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw_hal/common/gpio/GpioCookie.h" +#include "mission/trace.h" static constexpr bool DEBUG_MODE = true; @@ -37,6 +38,9 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default; ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { using namespace std::filesystem; +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "SA DEPL"); +#endif if (opDivider.checkAndIncrement()) { auto activeSdc = sdcMan.getActiveSdCard(); if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h index 63f1d3de..61211ca3 100644 --- a/mission/devices/SolarArrayDeploymentHandler.h +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -19,6 +19,7 @@ #include "fsfw/timemanager/Countdown.h" #include "fsfw_hal/common/gpio/GpioIF.h" #include "mission/memory/SdCardMountedIF.h" +#include "mission/trace.h" #include "returnvalues/classIds.h" enum DeploymentChannels : uint8_t { SA_1 = 1, SA_2 = 2 }; @@ -172,6 +173,9 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, bool firstAutonomousCycle = true; ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID; std::optional initUptime; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif PeriodicOperationDivider opDivider = PeriodicOperationDivider(5); uint8_t retryCounter = 3; diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index 2923b41b..b3cb0fe4 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -51,7 +51,10 @@ enum PoolIds : lp_id_t { SPI_BYTES_WRITTEN, SPI_BYTES_READ, SPI_REG_OVERRUN_ERRORS, - SPI_TOTAL_ERRORS + SPI_TOTAL_ERRORS, + + RW_SPEED, + RAMP_TIME, }; enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING }; @@ -75,10 +78,13 @@ static const DeviceCommandId_t SET_SPEED = 6; static const DeviceCommandId_t GET_TEMPERATURE = 8; static const DeviceCommandId_t GET_TM = 9; -static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE; -static const uint32_t STATUS_SET_ID = GET_RW_STATUS; -static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS; -static const uint32_t TM_SET_ID = GET_TM; +enum SetIds : uint32_t { + TEMPERATURE_SET_ID = GET_TEMPERATURE, + STATUS_SET_ID = GET_RW_STATUS, + LAST_RESET_ID = GET_LAST_RESET_STATUS, + TM_SET_ID = GET_TM, + SPEED_CMD_SET = 10, +}; static const size_t SIZE_GET_RESET_STATUS = 5; static const size_t SIZE_CLEAR_RESET_STATUS = 4; @@ -106,9 +112,11 @@ static const uint8_t TM_SET_ENTRIES = 24; */ class StatusSet : public StaticLocalDataSet { public: - StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, STATUS_SET_ID) {} + StatusSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, RwDefinitions::SetIds::STATUS_SET_ID) {} - StatusSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {} + StatusSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::STATUS_SET_ID)) {} lp_var_t temperatureCelcius = lp_var_t(sid.objectId, PoolIds::TEMPERATURE_C, this); @@ -124,9 +132,11 @@ class StatusSet : public StaticLocalDataSet { */ class LastResetSatus : public StaticLocalDataSet { public: - LastResetSatus(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LAST_RESET_ID) {} + LastResetSatus(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, RwDefinitions::SetIds::LAST_RESET_ID) {} - LastResetSatus(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {} + LastResetSatus(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::LAST_RESET_ID)) {} // If a reset occurs, the status code will be cached into this variable lp_var_t lastNonClearedResetStatus = @@ -143,9 +153,11 @@ class LastResetSatus : public StaticLocalDataSet { */ class TmDataset : public StaticLocalDataSet { public: - TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TM_SET_ID) {} + TmDataset(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, RwDefinitions::SetIds::TM_SET_ID) {} - TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TM_SET_ID)) {} + TmDataset(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::TM_SET_ID)) {} lp_var_t lastResetStatus = lp_var_t(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this); @@ -192,6 +204,36 @@ class TmDataset : public StaticLocalDataSet { lp_var_t(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this); }; +class RwSpeedActuationSet : public StaticLocalDataSet<2> { + friend class RwHandler; + + public: + RwSpeedActuationSet(HasLocalDataPoolIF& owner) + : StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {} + RwSpeedActuationSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {} + + void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) { + if (rwSpeed.value != rwSpeed_) { + rwSpeed = rwSpeed_; + } + if (rampTime.value != rampTime_) { + rampTime = rampTime_; + } + } + + void getRwSpeed(int32_t& rwSpeed_, uint16_t& rampTime_) { + rwSpeed_ = rwSpeed.value; + rampTime_ = rampTime.value; + } + + private: + lp_var_t rwSpeed = + lp_var_t(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this); + lp_var_t rampTime = + lp_var_t(sid.objectId, RwDefinitions::PoolIds::RAMP_TIME, this); +}; + } // namespace RwDefinitions #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h index 2abf1c21..c3bc3d36 100644 --- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h @@ -492,14 +492,14 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_, uint16_t currentTorqueDurationMs_) { if (xDipole.value != xDipole_) { + xDipole = xDipole_; } - xDipole = xDipole_; if (yDipole.value != yDipole_) { + yDipole = yDipole_; } - yDipole = yDipole_; if (zDipole.value != zDipole_) { + zDipole = zDipole_; } - zDipole = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; } diff --git a/mission/trace.cpp b/mission/trace.cpp new file mode 100644 index 00000000..6c9af0af --- /dev/null +++ b/mission/trace.cpp @@ -0,0 +1,10 @@ +#include "trace.h" + +#include "fsfw/serviceinterface.h" + +void trace::threadTrace(uint32_t& counter, const char* name, unsigned div) { + counter++; + if (counter % div == 0) { + sif::debug << name << " running" << std::endl; + } +} diff --git a/mission/trace.h b/mission/trace.h new file mode 100644 index 00000000..fbc99d37 --- /dev/null +++ b/mission/trace.h @@ -0,0 +1,14 @@ +#ifndef MISSION_TRACE_H_ +#define MISSION_TRACE_H_ + +#include + +#define OBSW_THREAD_TRACING 0 + +namespace trace { + +void threadTrace(uint32_t& counter, const char* name, unsigned div = 5); + +} + +#endif /* MISSION_TRACE_H_ */ diff --git a/scripts/q7s-em-udp-forwarding.sh b/scripts/q7s-em-udp-forwarding.sh new file mode 100755 index 00000000..284e7889 --- /dev/null +++ b/scripts/q7s-em-udp-forwarding.sh @@ -0,0 +1,7 @@ +#!/bin/bash +echo "Q7S UDP connection from local port 18000 -> TCP ssh tunnel -> EM port 7301" + + +socat udp4-listen:18000,reuseaddr,fork tcp:localhost:18002 & +ssh -L 18002:localhost:18123 eive@flatsat.eive.absatvirt.lw \ + 'socat tcp4-listen:18123,reuseaddr udp:192.168.133.10:7301' diff --git a/scripts/q7s-fm-udp-forwarding.sh b/scripts/q7s-fm-udp-forwarding.sh new file mode 100755 index 00000000..34c8421f --- /dev/null +++ b/scripts/q7s-fm-udp-forwarding.sh @@ -0,0 +1,7 @@ +#!/bin/bash +echo "Q7S UDP connection from local port 18000 -> TCP ssh tunnel -> FM port 7301" + + +socat udp4-listen:18000,reuseaddr,fork tcp:localhost:18002 & +ssh -L 18002:localhost:18123 eive@flatsat.eive.absatvirt.lw \ + 'socat tcp4-listen:18123,reuseaddr udp:192.168.155.55:7301' diff --git a/scripts/q7s-ssh-udp-forwarding.sh b/scripts/q7s-ssh-udp-forwarding.sh deleted file mode 100755 index 35303246..00000000 --- a/scripts/q7s-ssh-udp-forwarding.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -echo "Setting up all Q7S ports" -echo "upd connection from local port 18000 -> tcp ssh tunnel -> EM port 7301" - - -socat udp4-listen:18000,reuseaddr,fork tcp:localhost:18002 & -ssh -L 18002:localhost:18123 eive@flatsat.eive.absatvirt.lw 'socat tcp4-listen:18123,reuseaddr udp:192.168.133.10:7301' - diff --git a/test/TestTask.cpp b/test/TestTask.cpp index 634fdd0d..2c6cb015 100644 --- a/test/TestTask.cpp +++ b/test/TestTask.cpp @@ -41,11 +41,13 @@ ReturnValue_t EiveTestTask::performOperation(uint8_t operationCode) { } #include -#include + +// #include /** * @brief Dummy data from GPS receiver. Will be replaced witgh hyperion data later. */ + const char gps_rx_data[] = "" "$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n" @@ -96,20 +98,22 @@ ReturnValue_t EiveTestTask::performActionB() { return result; } +/* void EiveTestTask::performLwgpsTest() { - /* Everything here will only be performed once. */ + // Everything here will only be performed once. sif::info << "Processing sample GPS output.." << std::endl; lwgps_t gpsStruct; sif::info << "Size of GPS struct: " << sizeof(gpsStruct) << std::endl; lwgps_init(&gpsStruct); - /* Process all input data */ + // Process all input data lwgps_process(&gpsStruct, hyperion_gps_data, strlen(hyperion_gps_data)); - /* Print messages */ + // Print messages printf("Valid status: %d\n", gpsStruct.is_valid); printf("Latitude: %f degrees\n", gpsStruct.latitude); printf("Longitude: %f degrees\n", gpsStruct.longitude); printf("Altitude: %f meters\n", gpsStruct.altitude); } +*/ diff --git a/tmtc b/tmtc index 9b7471e9..1e143ea6 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 9b7471e9097edd410995ba0c76125b626440d9be +Subproject commit 1e143ea6faa608baf3118512416f5a495dbd606c