diff --git a/CHANGELOG.md b/CHANGELOG.md index 2ec1aa03..c642b897 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,20 +18,67 @@ will consitute of a breaking change warranting a new major release: # [v4.0.0] to be released -- eive-tmtc: v4.0.0 (to be released) -TODO: New firmware package version. +- `eive-tmtc` version v4.0.0 +- `q7s-package` version v3.0.0 ## Fixed -- CFDP low level protocol bugfix. Requires fsfw update and tmtc update. - Important bugfixes for PTME. See `q7s-package` CHANGELOG. +- Fixed H parameter in SUS converter from 1 mm to 2.5 mm. ## Changed - Removed PTME busy/ready signals. Those were not used anyway because register reads are used now. - APB bus access busy checking is not done anymore as this is performed by the bus itself now. -# [v3.0.0] to be released +## Added + +- Added PL I2C reset pin. It is not used for now but could be used for FDIR procedures to restore + the PL I2C. + +# [v3.3.0] 2023-06-21 + +Like v3.2.0 but without the custom FM changes related to VC0. + +# [v3.2.0] 2023-06-21 + +## Fixed + +- Fix sun vector calculation +- SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy + length. + +## Changed + +- Reverted all changes related to VC0 handling to avoid FM bug possibly related to FPGA bug. + +# [v3.1.1] 2023-06-14 + +## Fixed + +- TMP1075 bugfix where negative temperatures could not be measured because of a two's-complement + conversion bug. + +# [v3.1.0] 2023-06-14 + +- `eive-tmtc` version v4.1.0 + +## Fixed + +- TCS heater switch enumeration naming was old/wrong and was not updated in sync with the object ID + update. This lead to the TCS controller commanding the wrong heaters. + +## Changed + +- Increase number of allowed parallel HK commands to 16 + +## Added + +- Added `CONFIG_SET`, `MAN_HEATER_ON` and `MAN_HEATER_OFF` support for the BPX battery handler + +# [v3.0.0] 2023-06-11 + +- `eive-tmtc` version v4.0.0 ## Changed @@ -66,6 +113,13 @@ TODO: New firmware package version. only be used to cancel a transfer. - Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter commands. +- The Directory Listing direct dumper now has a state machine to stagger the directory listing dump. + This is required because very large dumps will overload the queue capacities in the framework. +- The PUS Service 8 now has larger queue sizes to handle more action replies. The PUS Service 1 + also has a larger queue size to handle a lot of step replies now. +- Moved PDU `vcc` and `vbat` variable from auxiliary dataset to core dataset. +- Tweak TCP/IP configuration: Only the TCP server will be included on the EM. For the FM, no + TCP/IP servers will be included by default. ## Added @@ -75,9 +129,13 @@ TODO: New firmware package version. - ACU dummy HK sets - IMTQ HK sets - IMTQ dummy now handles power switch +- Added some new ACS parameters +- Enabled decimation filter for the ADIS GYRs +- Enabled second low-pass filter for L3GD20H GYRs ## Fixed +- CFDP low level protocol bugfix. Requires `fsfw` update and `tmtc` update. - Compile fix if SCEX is compiled for the EM. - Set up Rad Sensor chip select even for EM to avoid SPI bus issues. - Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the @@ -106,8 +164,13 @@ TODO: New firmware package version. - Prevent spam of TCS controller heater unavailability event if all heaters are in external control. - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS controller. There is not crash risk but the heater states were invalid. +- STR datasets were not set to invalid on shutdown. - Fixed usage of quaternion valid flag, which does not actually represent the validity of the quaternion. +- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as + intended. +- The variance for the ADIS GYRs now represents the used `-3` version and not the `-1` version +- CFDP funnel did not route packets to live channel VC0 # [v2.0.5] 2023-05-11 diff --git a/CMakeLists.txt b/CMakeLists.txt index 26bcbd80..3c7e1c1e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,6 +79,13 @@ else() set(INIT_VAL 1) set(OBSW_STAR_TRACKER_GROUND_CONFIG 0) endif() + +set(OBSW_ADD_TMTC_TCP_SERVER + ${OBSW_Q7S_EM} + CACHE STRING "Add TCP TMTC Server") +set(OBSW_ADD_TMTC_UDP_SERVER + 0 + CACHE STRING "Add UDP TMTC Server") set(OBSW_ADD_MGT ${INIT_VAL} CACHE STRING "Add MGT module") diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 9a93fe4c..1500301f 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 295 translations. + * @brief Auto-generated event translation file. Contains 296 translations. * @details - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateEvents.h" @@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -847,6 +848,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 34edfa41..d1d0464c 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 171 translations. - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateObjects.h" diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 1c0a4db8..2555d7cd 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -67,8 +67,8 @@ // because UDP packets are not allowed in the VPN // This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the // CCSDS IP Cores. -#define OBSW_ADD_TMTC_TCP_SERVER 1 -#define OBSW_ADD_TMTC_UDP_SERVER 1 +#define OBSW_ADD_TMTC_TCP_SERVER @OBSW_ADD_TMTC_TCP_SERVER@ +#define OBSW_ADD_TMTC_UDP_SERVER @OBSW_ADD_TMTC_UDP_SERVER@ // Can be used to switch device to NORMAL mode immediately #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 0 diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 304113d2..146386c4 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -23,6 +23,7 @@ static constexpr char UART_SCEX_DEV[] = "/dev/scex"; static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio_pdec_regs"; static constexpr char UIO_PTME[] = "/dev/uio_ptme"; static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio_pdec_cfg_mem"; +static constexpr char UIO_SYS_ROM[] = "/dev/uio_sys_rom"; static constexpr char UIO_PDEC_RAM[] = "/dev/uio_pdec_ram"; static constexpr char UIO_PDEC_IRQ[] = "/dev/uio_pdec_irq"; static constexpr int MAP_ID_PTME_CONFIG = 3; @@ -57,6 +58,7 @@ static constexpr char GYRO_0_ENABLE[] = "enable_gyro_0"; static constexpr char GYRO_2_ENABLE[] = "enable_gyro_2"; static constexpr char GNSS_SELECT[] = "gnss_mux_select"; static constexpr char GNSS_MUX_SELECT[] = "gnss_mux_select"; +static constexpr char PL_I2C_ARESETN[] = "pl_i2c_aresetn"; static constexpr char HEATER_0[] = "heater0"; static constexpr char HEATER_1[] = "heater1"; diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 2aaef9a5..6469750e 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "commonConfig.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -22,6 +23,7 @@ #include #include +#include "bsp_q7s/boardconfig/busConf.h" #include "bsp_q7s/fs/SdCardManager.h" #include "bsp_q7s/memory/scratchApi.h" #include "bsp_q7s/xadc/Xadc.h" @@ -112,6 +114,9 @@ void CoreController::performControlOperation() { sdStateMachine(); performMountedSdCardOperations(); readHkData(); + if (dumpContext.active) { + dirListingDumpHandler(); + } if (shellCmdIsExecuting) { bool replyReceived = false; @@ -182,6 +187,14 @@ ReturnValue_t CoreController::initialize() { if (result != returnvalue::OK) { sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl; } + + UioMapper sysRomMapper(q7s::UIO_SYS_ROM); + result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY); + if (result != returnvalue::OK) { + // TODO: This might be a reason to switch to another image.. + sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl; + return ObjectManager::CHILD_INIT_FAILED; + } return returnvalue::OK; } @@ -220,6 +233,11 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ } triggerEvent(VERSION_INFO, p1, p2); + if (mappedSysRomAddr != nullptr) { + uint32_t p1Firmware = *(reinterpret_cast(mappedSysRomAddr)); + uint32_t p2Firmware = *(reinterpret_cast(mappedSysRomAddr) + 1); + triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware); + } return HasActionsIF::EXECUTION_FINISHED; } case (ANNOUNCE_BOOT_COUNTS): { @@ -1041,7 +1059,6 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI return returnvalue::FAILED; } } - std::array dirListingBuf{}; dirListingBuf[8] = parser.compressionOptionSet(); // First four bytes reserved for segment index. One byte for compression option information std::strcpy(reinterpret_cast(dirListingBuf.data() + 2 * sizeof(uint32_t) + 1), repoName); @@ -1050,38 +1067,47 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI return returnvalue::FAILED; } std::error_code e; - size_t totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); - uint32_t segmentIdx = 0; - size_t dumpedBytes = 0; + dumpContext.totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e); + dumpContext.segmentIdx = 0; + dumpContext.dumpedBytes = 0; size_t nextDumpLen = 0; size_t dummy = 0; - size_t maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1; - size_t listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1; - uint32_t chunks = totalFileSize / maxDumpLen; - if (totalFileSize % maxDumpLen != 0) { + dumpContext.maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1; + dumpContext.listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1; + uint32_t chunks = dumpContext.totalFileSize / dumpContext.maxDumpLen; + if (dumpContext.totalFileSize % dumpContext.maxDumpLen != 0) { chunks++; } SerializeAdapter::serialize(&chunks, dirListingBuf.data() + sizeof(uint32_t), &dummy, dirListingBuf.size() - sizeof(uint32_t), SerializeIF::Endianness::NETWORK); - while (dumpedBytes < totalFileSize) { - ifile.seekg(dumpedBytes, std::ios::beg); - nextDumpLen = maxDumpLen; - if (totalFileSize - dumpedBytes < maxDumpLen) { - nextDumpLen = totalFileSize - dumpedBytes; + while (dumpContext.dumpedBytes < dumpContext.totalFileSize) { + ifile.seekg(dumpContext.dumpedBytes, std::ios::beg); + nextDumpLen = dumpContext.maxDumpLen; + if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) { + nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes; } - SerializeAdapter::serialize(&segmentIdx, dirListingBuf.data(), &dummy, dirListingBuf.size(), - SerializeIF::Endianness::NETWORK); - ifile.read(reinterpret_cast(dirListingBuf.data() + listingDataOffset), nextDumpLen); + SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy, + dirListingBuf.size(), SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + dumpContext.listingDataOffset), + nextDumpLen); result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(), - listingDataOffset + nextDumpLen); + dumpContext.listingDataOffset + nextDumpLen); if (result != returnvalue::OK) { // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); return result; } - segmentIdx++; - dumpedBytes += nextDumpLen; + dumpContext.segmentIdx++; + dumpContext.dumpedBytes += nextDumpLen; + // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. + if (dumpContext.segmentIdx == 10) { + dumpContext.active = true; + dumpContext.firstDump = true; + dumpContext.commander = commandedBy; + dumpContext.actionId = actionId; + return returnvalue::OK; + } } // Remove work file when we are done std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); @@ -2349,6 +2375,54 @@ MessageQueueId_t CoreController::getCommandQueue() const { return ExtendedControllerBase::getCommandQueue(); } +void CoreController::dirListingDumpHandler() { + if (dumpContext.firstDump) { + dumpContext.firstDump = false; + return; + } + size_t nextDumpLen = 0; + size_t dummy = 0; + ReturnValue_t result; + std::error_code e; + std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary); + if (ifile.bad()) { + return; + } + while (dumpContext.dumpedBytes < dumpContext.totalFileSize) { + ifile.seekg(dumpContext.dumpedBytes, std::ios::beg); + nextDumpLen = dumpContext.maxDumpLen; + if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) { + nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes; + } + SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy, + dirListingBuf.size(), SerializeIF::Endianness::NETWORK); + ifile.read(reinterpret_cast(dirListingBuf.data() + dumpContext.listingDataOffset), + nextDumpLen); + result = + actionHelper.reportData(dumpContext.commander, dumpContext.actionId, dirListingBuf.data(), + dumpContext.listingDataOffset + nextDumpLen); + if (result != returnvalue::OK) { + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + dumpContext.active = false; + actionHelper.finish(false, dumpContext.commander, dumpContext.actionId, result); + return; + } + dumpContext.segmentIdx++; + dumpContext.dumpedBytes += nextDumpLen; + // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles. + if (dumpContext.segmentIdx == 10) { + break; + } + } + if (dumpContext.dumpedBytes >= dumpContext.totalFileSize) { + actionHelper.finish(true, dumpContext.commander, dumpContext.actionId, result); + dumpContext.active = false; + // Remove work file when we are done + std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e); + } +} + bool CoreController::isNumber(const std::string &s) { return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index d44907e8..6703e398 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -142,6 +143,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe static constexpr bool BLOCKING_SD_INIT = false; + uint32_t* mappedSysRomAddr = nullptr; SdCardManager* sdcMan = nullptr; MessageQueueIF* eventQueue = nullptr; @@ -177,6 +179,20 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe DeviceCommandId_t actionId; } sdCommandingInfo; + struct DirListingDumpContext { + bool active; + bool firstDump; + size_t dumpedBytes; + size_t totalFileSize; + size_t listingDataOffset; + size_t maxDumpLen; + uint32_t segmentIdx; + MessageQueueId_t commander = MessageQueueIF::NO_QUEUE; + DeviceCommandId_t actionId; + }; + std::array dirListingBuf{}; + DirListingDumpContext dumpContext{}; + RebootFile rebootFile = {}; CommandExecutor cmdExecutor; @@ -274,6 +290,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe void rewriteRebootFile(RebootFile file); void announceBootCounts(); void readHkData(); + void dirListingDumpHandler(); bool isNumber(const std::string& s); }; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 6e1bffdf..54084089 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -1013,3 +1014,19 @@ void ObjectFactory::createRadSensorChipSelect(LinuxLibgpioIF* gpioIF) { gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio); gpioChecker(gpioIF->addGpios(gpioCookieRadSensor), "RAD sensor"); } + +void ObjectFactory::createPlI2cResetGpio(LinuxLibgpioIF* gpioIF) { + using namespace gpio; + if (gpioIF == nullptr) { + return; + } + GpioCookie* gpioI2cResetnCookie = new GpioCookie; + GpiodRegularByLineName* gpioI2cResetn = new GpiodRegularByLineName( + q7s::gpioNames::PL_I2C_ARESETN, "PL_I2C_ARESETN", Direction::OUT, Levels::HIGH); + gpioI2cResetnCookie->addGpio(gpioIds::PL_I2C_ARESETN, gpioI2cResetn); + gpioChecker(gpioIF->addGpios(gpioI2cResetnCookie), "PL I2C ARESETN"); + // Reset I2C explicitely again. + gpioIF->pullLow(gpioIds::PL_I2C_ARESETN); + TaskFactory::delayTask(1); + gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN); +} diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index df65e1ae..a2edec4c 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -78,6 +78,7 @@ ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args); void createMiscComponents(); void createTestComponents(LinuxLibgpioIF* gpioComIF); +void createPlI2cResetGpio(LinuxLibgpioIF* gpioComIF); void testAcsBrdAss(AcsBoardAssembly* assAss); diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 5b2935b7..464b3398 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -47,6 +47,7 @@ void ObjectFactory::produce(void* args) { /* Adding gpios for chip select decoding to the gpioComIf */ q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + createPlI2cResetGpio(gpioComIF); // Hardware is usually not connected to EM, so we need to create dummies which replace lower // level components. @@ -140,6 +141,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index df0bbc91..413e5648 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -45,6 +45,7 @@ void ObjectFactory::produce(void* args) { /* Adding gpios for chip select decoding to the gpioComIf */ q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF); gpioCallbacks::disableAllDecoder(gpioComIF); + createPlI2cResetGpio(gpioComIF); new CoreController(objects::CORE_CONTROLLER, enableHkSets); createPcduComponents(gpioComIF, &pwrSwitcher, enableHkSets); @@ -97,6 +98,7 @@ void ObjectFactory::produce(void* args) { #if OBSW_TM_TO_PTME == 1 if (ccsdsArgs.liveDestination != nullptr) { pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); + cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0); } #endif #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */ diff --git a/common/config/devices/gpioIds.h b/common/config/devices/gpioIds.h index 573327fa..bed82142 100644 --- a/common/config/devices/gpioIds.h +++ b/common/config/devices/gpioIds.h @@ -77,6 +77,8 @@ enum gpioId_t { CS_RAD_SENSOR, ENABLE_RADFET, + PL_I2C_ARESETN, + PAPB_BUSY_N, PAPB_EMPTY, diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index 78ba52e4..2083962a 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -58,7 +58,9 @@ static constexpr uint32_t CFDP_STORE_QUEUE_SIZE = 300; static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100; static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 80; +static constexpr uint32_t VERIFICATION_SERVICE_QUEUE_DEPTH = 120; static constexpr uint32_t HK_SERVICE_QUEUE_DEPTH = 60; +static constexpr uint32_t ACTION_SERVICE_QUEUE_DEPTH = 60; static constexpr uint32_t MAX_STORED_CMDS_UDP = 150; static constexpr uint32_t MAX_STORED_CMDS_TCP = 180; diff --git a/dummies/helperFactory.h b/dummies/helperFactory.h index 467cb172..bab9d8d8 100644 --- a/dummies/helperFactory.h +++ b/dummies/helperFactory.h @@ -19,7 +19,7 @@ struct DummyCfg { bool addTempSensorDummies = true; bool addRtdComIFDummy = true; bool addPlocDummies = true; - bool addCamSwitcherDummy = true; + bool addCamSwitcherDummy = false; }; void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF, bool enableHkSets); diff --git a/fsfw b/fsfw index 4391823f..0f76cdb3 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 4391823f01d792bcc078d47b60f7df7624f8cbe4 +Subproject commit 0f76cdb3ba54f5e90a8eee4316c49cf0f581f996 diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 9dc26e07..17592d1d 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -271,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 9dc26e07..17592d1d 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -271,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 9a93fe4c..1500301f 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 295 translations. + * @brief Auto-generated event translation file. Contains 296 translations. * @details - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateEvents.h" @@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -847,6 +848,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index eb2125b4..1ecd5e71 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateObjects.h" diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index e525bf53..1ba55357 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send if (req->mode != adis.mode) { if (req->mode == acs::SimpleSensorMode::NORMAL) { adis.type = req->type; + adis.decRate = req->cfg.decRateReg; // The initial countdown is handled by the device handler now. // adis.countdown.setTimeout(adis1650x::START_UP_TIME); if (adis.type == adis1650x::Type::ADIS16507) { @@ -376,6 +377,80 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { } } +ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { + ReturnValue_t result = returnvalue::OK; + int retval = 0; + // Prepare transfer + int fileDescriptor = 0; + std::string device = spiComIF.getSpiDev(); + UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return spi::OPENING_FILE_FAILED; + } + spi::SpiModes spiMode = spi::SpiModes::MODE_0; + uint32_t spiSpeed = 0; + cookie.getSpiParameters(spiMode, spiSpeed, nullptr); + spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + cookie.assignWriteBuffer(cmdBuf.data()); + cookie.setTransferSize(2); + + gpioId_t gpioId = cookie.getChipSelectPin(); + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 0; + MutexIF* mutex = spiComIF.getCsMutex(); + cookie.getMutexParams(timeoutType, timeoutMs); + if (mutex == nullptr) { + sif::warning << "GyroADIS16507Handler::spiSendCallback: " + "Mutex or GPIO interface invalid" + << std::endl; + return returnvalue::FAILED; + } + + size_t idx = 0; + spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); + uint64_t origTx = transferStruct->tx_buf; + uint64_t origRx = transferStruct->rx_buf; + for (idx = 0; idx < 4; idx += 2) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl; +#endif + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullLow(gpioId); + } + + // Execute transfer + // Initiate a full duplex SPI transfer. + retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle()); + if (retval < 0) { + utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); + result = spi::FULL_DUPLEX_TRANSFER_FAILED; + } +#if FSFW_HAL_SPI_WIRETAPPING == 1 + comIf->performSpiWiretapping(cookie); +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ + + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullHigh(gpioId); + } + mutex->unlockMutex(); + + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < 4) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(0); + return returnvalue::OK; +} + ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { ReturnValue_t result = returnvalue::OK; int retval = 0; @@ -455,15 +530,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { ReturnValue_t result; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; bool mustPerformStartup = false; + uint16_t decRate = 0; { MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); mode = gyro.mode; + decRate = gyro.decRate; mustPerformStartup = gyro.performStartup; } if (mode == acs::SimpleSensorMode::OFF) { return; } if (mustPerformStartup) { + adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(), + cmdBuf.size()); + writeAdisReg(*gyro.cookie); uint8_t regList[6]; // Read configuration regList[0] = adis1650x::DIAG_STAT_REG; @@ -491,13 +571,19 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.replyResult = returnvalue::FAILED; return; } + uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11]; + if (decRateReadBack != decRate) { + sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack + << ", expected " << decRate << std::endl; + gyro.replyResult = returnvalue::FAILED; + } MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); gyro.ownReply.cfgWasSet = true; gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3]; gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5]; gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7]; gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9]; - gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.cfg.decRateReg = decRateReadBack; gyro.ownReply.cfg.prodId = prodId; gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); gyro.performStartup = false; diff --git a/linux/acs/AcsBoardPolling.h b/linux/acs/AcsBoardPolling.h index 794c9c47..5b4d0390 100644 --- a/linux/acs/AcsBoardPolling.h +++ b/linux/acs/AcsBoardPolling.h @@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject, struct GyroAdis : public DevBase { adis1650x::Type type; + uint16_t decRate; Countdown countdown; acs::Adis1650XReply ownReply; acs::Adis1650XReply readerReply; @@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject, void gyroAdisHandler(GyroAdis& gyro); void mgmLis3Handler(MgmLis3& mgm); void mgmRm3100Handler(MgmRm3100& mgm); + // This fumction configures the register as specified on p.21 of the datasheet. + ReturnValue_t writeAdisReg(SpiCookie& cookie); // Special readout: 16us stall time between small 2 byte transfers. ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); }; diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 9a93fe4c..1500301f 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 295 translations. + * @brief Auto-generated event translation file. Contains 296 translations. * @details - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateEvents.h" @@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS"; const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY"; const char *I2C_REBOOT_STRING = "I2C_REBOOT"; const char *PDEC_REBOOT_STRING = "PDEC_REBOOT"; +const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO"; const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE"; const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE"; const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING"; @@ -847,6 +848,8 @@ const char *translateEvents(Event event) { return I2C_REBOOT_STRING; case (14012): return PDEC_REBOOT_STRING; + case (14013): + return FIRMWARE_INFO_STRING; case (14100): return NO_VALID_SENSOR_TEMPERATURE_STRING; case (14101): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index eb2125b4..1ecd5e71 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 175 translations. - * Generated on: 2023-05-17 17:15:34 + * Generated on: 2023-06-21 19:01:02 */ #include "translateObjects.h" diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index c61187ce..fe10c214 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -15,7 +15,7 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic } void GyrAdis1650XHandler::doStartUp() { - if (internalState != InternalState::STARTUP) { + if (internalState == InternalState::NONE) { internalState = InternalState::STARTUP; commandExecuted = false; } @@ -24,13 +24,14 @@ void GyrAdis1650XHandler::doStartUp() { if (not commandExecuted) { warningSwitch = true; breakCountdown.setTimeout(adis1650x::START_UP_TIME); + updatePeriodicReply(true, adis1650x::REPLY); commandExecuted = true; } - if (breakCountdown.hasTimedOut()) { - updatePeriodicReply(true, adis1650x::REPLY); - setMode(MODE_ON); - internalState = InternalState::NONE; - } + } + if (internalState == InternalState::STARTUP_DONE) { + setMode(MODE_ON); + commandExecuted = false; + internalState = InternalState::NONE; } } @@ -61,6 +62,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ return NOTHING_TO_SEND; } *id = adis1650x::REQUEST; + adisRequest.cfg.decRateReg = adis1650x::DEC_RATE; return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); } case (InternalState::SHUTDOWN): { @@ -91,6 +93,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem getMode() == _MODE_POWER_DOWN) { return IGNORE_FULL_PACKET; } + if (internalState == InternalState::STARTUP) { + internalState = InternalState::STARTUP_DONE; + } *foundLen = remainingSize; if (remainingSize != sizeof(acs::Adis1650XReply)) { return returnvalue::FAILED; diff --git a/mission/acs/GyrAdis1650XHandler.h b/mission/acs/GyrAdis1650XHandler.h index 5d906f61..308d472b 100644 --- a/mission/acs/GyrAdis1650XHandler.h +++ b/mission/acs/GyrAdis1650XHandler.h @@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase { bool warningSwitch = true; - enum class InternalState { NONE, STARTUP, SHUTDOWN }; + enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; diff --git a/mission/acs/acsBoardPolling.h b/mission/acs/acsBoardPolling.h index 90f0f19c..9f794fb0 100644 --- a/mission/acs/acsBoardPolling.h +++ b/mission/acs/acsBoardPolling.h @@ -8,11 +8,6 @@ namespace acs { -struct Adis1650XRequest { - SimpleSensorMode mode; - adis1650x::Type type; -}; - struct Adis1650XConfig { uint16_t diagStat; uint16_t filterSetting; @@ -22,6 +17,12 @@ struct Adis1650XConfig { uint16_t prodId; }; +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; + Adis1650XConfig cfg; +}; + struct Adis1650XData { double sensitivity = 0.0; // Angular velocities in all axes (X, Y and Z) diff --git a/mission/acs/gyroAdisHelpers.cpp b/mission/acs/gyroAdisHelpers.cpp index 0f41b058..5c1446cc 100644 --- a/mission/acs/gyroAdisHelpers.cpp +++ b/mission/acs/gyroAdisHelpers.cpp @@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { } } } + +void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, + size_t maxLen) { + if (maxLen < 4) { + return; + } + outBuf[0] = regStart | adis1650x::WRITE_MASK; + outBuf[1] = val & 0xff; + outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK; + outBuf[3] = (val >> 8) & 0xff; +} diff --git a/mission/acs/gyroAdisHelpers.h b/mission/acs/gyroAdisHelpers.h index 09f0aa1e..f360db76 100644 --- a/mission/acs/gyroAdisHelpers.h +++ b/mission/acs/gyroAdisHelpers.h @@ -16,6 +16,8 @@ enum class BurstModes { }; size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen); + BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); double rangMdlToSensitivity(uint16_t rangMdl); @@ -92,6 +94,9 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2; static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; +static constexpr uint16_t FILT_CTRL = 0x0000; +static constexpr uint16_t DEC_RATE = 0x00C7; + enum GlobCmds : uint8_t { FACTORY_CALIBRATION = 0b0000'0010, SENSOR_SELF_TEST = 0b0000'0100, diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 0942164a..1ba73181 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -100,6 +100,19 @@ void StarTrackerHandler::doShutDown() { startupState = StartupState::IDLE; bootState = FwBootState::NONE; solutionSet.setReportingEnabled(false); + { + PoolReadGuard pg(&solutionSet); + solutionSet.caliQw.value = 0.0; + solutionSet.caliQx.value = 0.0; + solutionSet.caliQy.value = 0.0; + solutionSet.caliQz.value = 0.0; + solutionSet.isTrustWorthy.value = 0; + solutionSet.setValidity(false, true); + } + { + PoolReadGuard pg(&temperatureSet); + temperatureSet.setValidity(false, true); + } reinitNextSetParam = false; setMode(_MODE_POWER_DOWN); } diff --git a/mission/cfdp/CfdpHandler.cpp b/mission/cfdp/CfdpHandler.cpp index 902097b6..fa35535c 100644 --- a/mission/cfdp/CfdpHandler.cpp +++ b/mission/cfdp/CfdpHandler.cpp @@ -93,6 +93,8 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { return INVALID_PDU_FORMAT; } if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { + sif::error << "CfdpHandler: Invalid PDU directive field " << static_cast(pduDataField[0]) + << std::endl; return INVALID_DIRECTIVE_FIELD; } auto directive = static_cast(pduDataField[0]); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 15e9a840..04009cb2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,7 +169,7 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, @@ -205,11 +205,13 @@ void AcsController::performSafe() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl; + break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && @@ -231,8 +233,8 @@ void AcsController::performSafe() { } updateCtrlValData(errAng, safeCtrlStrat); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -259,7 +261,7 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } - uint8_t safeCtrlStrat = detumble.detumbleStrategy( + acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); @@ -279,11 +281,13 @@ void AcsController::performDetumble() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl; + break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -304,8 +308,8 @@ void AcsController::performDetumble() { } disableCtrlValData(); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -366,24 +370,26 @@ void AcsController::performPointingCtrl() { // Variables required for setting actuators double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, mgtDpDes[3] = {0, 0, 0}; + switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat, + targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -397,17 +403,17 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; @@ -418,17 +424,17 @@ void AcsController::performPointingCtrl() { targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, 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); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -442,63 +448,61 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + 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: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); + sizeof(targetQuat)); 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), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: - sif::error << "AcsController: Invalid mode for performPointingCtrl"; + sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; break; } actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, - cmdSpeedRws, acsParameters.onBoardParams.sampleTime, - acsParameters.rwHandlingParameters.maxRwSpeed, - acsParameters.rwHandlingParameters.inertiaWheel); + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, + acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 6ec26c57..0c8b94bb 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool mekfLost = false; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; - int16_t cmdDipolMtqs[3] = {0, 0, 0}; + int16_t cmdDipoleMtqs[3] = {0, 0, 0}; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 6205ce44..53babbd5 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -1002,7 +1002,7 @@ void ThermalController::copyDevices() { void ThermalController::ctrlAcsBoard() { heater::Switch switchNr = heater::HEATER_2_ACS_BRD; - heater::Switch redSwitchNr = heater::HEATER_0_OBC_BRD; + heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD; // A side thermalComponent = ACS_BOARD; @@ -1067,7 +1067,7 @@ void ThermalController::ctrlMgt() { sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, mgtLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not mgtTooHotFlag) { triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32()); @@ -1206,7 +1206,7 @@ void ThermalController::ctrlIfBoard() { sensors[2].first = deviceTemperatures.mgm2SideB.isValid(); sensors[2].second = deviceTemperatures.mgm2SideB.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits); + HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } @@ -1220,7 +1220,7 @@ void ThermalController::ctrlTcsBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits); ctrlComponentTemperature(htrCtx); // TODO: special event overheating + could go back to safe mode } @@ -1234,7 +1234,7 @@ void ThermalController::ctrlObc() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); @@ -1253,7 +1253,7 @@ void ThermalController::ctrlObcIfBoard() { sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs1.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); + HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not obcTooHotFlag) { triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32()); @@ -1288,7 +1288,7 @@ void ThermalController::ctrlPcduP60Board() { sensors[1].first = deviceTemperatures.temp2P60dock.isValid(); sensors[1].second = deviceTemperatures.temp2P60dock.value; numSensors = 2; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); @@ -1300,7 +1300,7 @@ void ThermalController::ctrlPcduP60Board() { void ThermalController::ctrlPcduAcu() { thermalComponent = PCDUACU; - heater::Switch switchNr = heater::HEATER_3_PCDU_PDU; + heater::Switch switchNr = heater::HEATER_1_PCDU_PDU; heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD; if (chooseHeater(switchNr, redSwitchNr)) { @@ -1340,7 +1340,7 @@ void ThermalController::ctrlPcduPdu() { sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid(); sensors[2].second = sensorTemperatures.tmp1075Tcs0.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits); ctrlComponentTemperature(htrCtx); if (componentAboveUpperLimit and not pcduSystemTooHotFlag) { triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32()); @@ -1361,7 +1361,7 @@ void ThermalController::ctrlPlPcduBoard() { sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid(); sensors[3].second = sensorTemperatures.plpcduHeatspreader.value; numSensors = 4; - HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); + HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); } @@ -1375,7 +1375,7 @@ void ThermalController::ctrlPlocMissionBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocMissionBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); @@ -1390,7 +1390,7 @@ void ThermalController::ctrlPlocProcessingBoard() { sensors[2].first = sensorTemperatures.dacHeatspreader.isValid(); sensors[2].second = sensorTemperatures.dacHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, plocProcessingBoardLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag); @@ -1405,7 +1405,7 @@ void ThermalController::ctrlDac() { sensors[2].first = sensorTemperatures.plocHeatspreader.isValid(); sensors[2].second = sensorTemperatures.plocHeatspreader.value; numSensors = 3; - HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits); + HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, dacLimits); ctrlComponentTemperature(htrCtx); tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag); } diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index d516aebd..eea30389 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -221,6 +221,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x23: parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; + case 0x24: + parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); + break; default: return INVALID_IDENTIFIER_ID; } @@ -318,7 +321,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->setVector(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspaceVector); break; default: return INVALID_IDENTIFIER_ID; @@ -378,15 +381,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); + parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(idleModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x8: + parameterWrapper->set(idleModeControllerParameters.desatOn); + break; + case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: @@ -411,48 +417,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(targetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x13: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x14: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -477,30 +486,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(gsTargetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xD: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xE: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -525,27 +537,30 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(nadirModeControllerParameters.desatOn); + parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); + parameterWrapper->set(nadirModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; + case 0xD: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -570,21 +585,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(inertialModeControllerParameters.desatOn); + parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); + parameterWrapper->set(inertialModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + break; + case 0xB: parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: @@ -696,7 +714,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquerParameter.dipolMax); + parameterWrapper->set(magnetorquerParameter.dipoleMax); break; case 0x6: parameterWrapper->set(magnetorquerParameter.torqueDuration); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 7034c9ae..9e13070f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -766,6 +766,7 @@ class AcsParameters : public HasParametersIF { {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; + float susBrightnessThreshold = 0.7; } susHandlingParameters; struct GyrHandlingParameters { @@ -781,9 +782,9 @@ class AcsParameters : public HasParametersIF { /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ - float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms - pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms - pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms + float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms + pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms + pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = false; float gyrFilterWeight = 0.6; @@ -816,7 +817,7 @@ class AcsParameters : public HasParametersIF { {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; double without4[4][3] = { {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; - double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; + double nullspaceVector[4] = {-1, 1, -1, 1}; } rwMatrices; struct SafeModeControllerParameters { @@ -840,7 +841,9 @@ class AcsParameters : public HasParametersIF { double om = 0.3; double omMax = 1 * M_PI / 180; double qiMin = 0.1; + double gainNullspace = 0.01; + double nullspaceSpeed = 32500; // 0.1 RPM double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; @@ -933,7 +936,7 @@ class AcsParameters : public HasParametersIF { double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; - double dipolMax = 0.2; // [Am^2] + double dipoleMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] } magnetorquerParameter; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index a8fab6a4..0fb8bdf5 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,11 +5,6 @@ #include #include -#include - -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} @@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { } } -void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, - int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, - double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { - using namespace Math; - - // Calculating the commanded speed in RPM for every reaction wheel +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, + int32_t *rwCmdSpeed) { + // group RW speed values (in 0.1 [RPM]) in vector int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; + + // calculate required RW speed as sum of current RW speed and RW speed delta + // delta w_rw = delta t / I_RW * torque_RW [rad/s] double deltaSpeed[4] = {0, 0, 0, 0}; - double radToRpm = 60 / (2 * PI); // factor for conversion to RPM - // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = sampleTime / inertiaWheel * radToRpm; - int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; + const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); + + // convert double to int32 + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; for (int i = 0; i < 4; i++) { deltaSpeedInt[i] = std::round(deltaSpeed[i]); } + + // sum of current RW speed and RW speed delta VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); - VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); + + // crop values which would exceed the maximum possible RPM for (uint8_t i = 0; i < 4; i++) { if (rwCmdSpeed[i] > maxRwSpeed) { rwCmdSpeed[i] = maxRwSpeed; @@ -52,24 +53,25 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee } } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol) { - // Convert to actuator frame - double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, - 1); - // Scaling along largest element if dipol exceeds maximum +void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator) { + // convert to actuator frame + double dipoleMomentActuatorDouble[3] = {0, 0, 0}; + MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, + 3, 1); + // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; - VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); - double maxAbsValue = std::abs(dipolMomentActuatorDouble[maxIdx]); - if (maxAbsValue > maxDipol) { - double scalingFactor = maxDipol / maxAbsValue; - VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, - dipolMomentActuatorDouble, 3); + VectorOperations::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipole) { + double scalingFactor = maxDipole / maxAbsValue; + VectorOperations::mulScalar(dipoleMomentActuatorDouble, scalingFactor, + dipoleMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, + 3); for (int i = 0; i < 3; i++) { - dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 5d5d47f3..b14a4a25 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,9 +1,7 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include class ActuatorCmd { public: @@ -19,29 +17,30 @@ class ActuatorCmd { void scalingTorqueRws(double *rwTrq, double maxTorque); /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction - * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque + * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration, + * given the required torque calculated by the controller. Will also scale down the RPM of the + * wheels if they exceed the maximum possible RPM + * @param: rwTrq given torque from pointing controller * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, - int32_t maxRwSpeed, double inertiaWheel); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* - * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * @brief: cmdDipoleMtq() gives the commanded dipole moment for the + * magnetorquer * - * @param: dipolMoment given dipol moment in spacecraft frame - * dipolMomentActuator resulting dipol moment in actuator reference frame + * @param: dipoleMoment given dipole moment in spacecraft frame + * dipoleMomentActuator resulting dipole moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol); + void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator); protected: private: + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACTUATORCMD_H_ */ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 79c6b416..1d82d317 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], @@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl 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) { + 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 @@ -424,26 +424,17 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // 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 ?? + // Calculate error satellite rotational rate + // First combine the target and reference satellite rotational rates + double combinedRefSatRotRate[3] = {0, 0, 0}; + VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); + // Then subtract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], - double errorSatRotRate[3], double errorAngle) { + 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 @@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // 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 ?? + // Calculate error satellite rotational rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); } void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], @@ -471,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua //------------------------------------------------------------------------------------- // Calculation of target 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)); + double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 - + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6); + if (VectorOperations::norm(savedQuaternion, 4) == 0) { + std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + } if (timeElapsed < timeElapsedMax) { + double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(quatInertialTarget, q); + QuaternionOperations::inverse(savedQuaternion, qS); double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::subtract(q, qS, 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 tgtQuatVec[3] = {q[0], q[1], q[2]}; + double 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::cross(tgtQuatVec, qDiffVec, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; @@ -531,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { - // @note: This one takes the normal pseudoInverse of all four raction wheels valid. - // Does not make sense, but is implemented that way in MATLAB ?! - // Thought: It does not really play a role, because in case there are more then one - // reaction wheel invalid the pointing control is destined to fail. return returnvalue::FAILED; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a52c476a..7b81e411 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -15,7 +15,7 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); - // Function to get the target quaternion and refence rotation rate from gps position and + // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], double refDirB[3], double quatBI[4], double targetQuat[4], @@ -25,9 +25,10 @@ class Guidance { 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 + // Function to get the target quaternion and reference rotation rate for sun pointing after ground // station - void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing @@ -37,15 +38,15 @@ class Guidance { double targetQuat[4], double refSatRate[3]); // @note: Calculates the error quaternion between the current orientation and the target - // quaternion, considering a reference quaternion. Additionally the difference between the actual + // 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); + 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); + 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 b106baaa..0279215f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -30,10 +30,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { - // Should be existing class object which will be called and modified here. Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs - // stored in acsParameters ? igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeOfMgmMeasurement); // maybe put a condition here, to only update after a full day, this @@ -45,14 +42,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); mgmDataProcessed->setValidity(false, true); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); @@ -210,63 +206,68 @@ void SensorProcessing::processSus( sunIjkModel[0] = cos(eclipticLongitude); sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); + + uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; if (sus0valid) { - sus0valid = susConverter.checkSunSensorData(sus0Value); + susBrightness[0] = susConverter.checkSunSensorData(sus0Value); } if (sus1valid) { - sus1valid = susConverter.checkSunSensorData(sus1Value); + susBrightness[1] = susConverter.checkSunSensorData(sus1Value); } if (sus2valid) { - sus2valid = susConverter.checkSunSensorData(sus2Value); + susBrightness[2] = susConverter.checkSunSensorData(sus2Value); } if (sus3valid) { - sus3valid = susConverter.checkSunSensorData(sus3Value); + susBrightness[3] = susConverter.checkSunSensorData(sus3Value); } if (sus4valid) { - sus4valid = susConverter.checkSunSensorData(sus4Value); + susBrightness[4] = susConverter.checkSunSensorData(sus4Value); } if (sus5valid) { - sus5valid = susConverter.checkSunSensorData(sus5Value); + susBrightness[5] = susConverter.checkSunSensorData(sus5Value); } if (sus6valid) { - sus6valid = susConverter.checkSunSensorData(sus6Value); + susBrightness[6] = susConverter.checkSunSensorData(sus6Value); } if (sus7valid) { - sus7valid = susConverter.checkSunSensorData(sus7Value); + susBrightness[7] = susConverter.checkSunSensorData(sus7Value); } if (sus8valid) { - sus8valid = susConverter.checkSunSensorData(sus8Value); + susBrightness[8] = susConverter.checkSunSensorData(sus8Value); } if (sus9valid) { - sus9valid = susConverter.checkSunSensorData(sus9Value); + susBrightness[9] = susConverter.checkSunSensorData(sus9Value); } if (sus10valid) { - sus10valid = susConverter.checkSunSensorData(sus10Value); + susBrightness[10] = susConverter.checkSunSensorData(sus10Value); } if (sus11valid) { - sus11valid = susConverter.checkSunSensorData(sus11Value); + susBrightness[11] = susConverter.checkSunSensorData(sus11Value); } - if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && - !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { + bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; + bool allInvalid = + susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold); + + if (allInvalid) { { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - float zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double)); susDataProcessed->setValidity(false, true); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -274,118 +275,78 @@ void SensorProcessing::processSus( } return; } - // WARNING: NOT TRANSFORMED IN BODY FRAME YET - // Transformation into Geomtry Frame - float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, - sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, - sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, - sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - if (sus0valid) { - MatrixOperations::multiply( - susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, - susParameters->sus0coeffBeta), - sus0VecBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply( - susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, - susParameters->sus1coeffBeta), - sus1VecBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply( - susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, - susParameters->sus2coeffBeta), - sus2VecBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply( - susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, - susParameters->sus3coeffBeta), - sus3VecBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply( - susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, - susParameters->sus4coeffBeta), - sus4VecBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply( - susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, - susParameters->sus5coeffBeta), - sus5VecBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply( - susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, - susParameters->sus6coeffBeta), - sus6VecBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply( - susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, - susParameters->sus7coeffBeta), - sus7VecBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply( - susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, - susParameters->sus8coeffBeta), - sus8VecBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply( - susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, - susParameters->sus9coeffBeta), - sus9VecBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply( - susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, - susParameters->sus10coeffBeta), - sus10VecBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply( - susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, - susParameters->sus11coeffBeta), - sus11VecBody, 3, 3, 1); - } + float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - /* ------ Mean Value: susDirEst ------ */ - bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, - sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; - float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], - sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], - sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, - {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], - sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], - sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, - {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], - sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], - sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; + if (susValid[0]) { + susConverter.calculateSunVector(susVecSensor[0], sus0Value); + MatrixOperations::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0], + susVecBody[0], 3, 3, 1); + } + if (susValid[1]) { + susConverter.calculateSunVector(susVecSensor[1], sus1Value); + MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1], + susVecBody[1], 3, 3, 1); + } + if (susValid[2]) { + susConverter.calculateSunVector(susVecSensor[2], sus2Value); + MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2], + susVecBody[2], 3, 3, 1); + } + if (susValid[3]) { + susConverter.calculateSunVector(susVecSensor[3], sus3Value); + MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3], + susVecBody[3], 3, 3, 1); + } + if (susValid[4]) { + susConverter.calculateSunVector(susVecSensor[4], sus4Value); + MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4], + susVecBody[4], 3, 3, 1); + } + if (susValid[5]) { + susConverter.calculateSunVector(susVecSensor[5], sus5Value); + MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5], + susVecBody[5], 3, 3, 1); + } + if (susValid[6]) { + susConverter.calculateSunVector(susVecSensor[6], sus6Value); + MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6], + susVecBody[6], 3, 3, 1); + } + if (susValid[7]) { + susConverter.calculateSunVector(susVecSensor[7], sus7Value); + MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7], + susVecBody[7], 3, 3, 1); + } + if (susValid[8]) { + susConverter.calculateSunVector(susVecSensor[8], sus8Value); + MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8], + susVecBody[8], 3, 3, 1); + } + if (susValid[9]) { + susConverter.calculateSunVector(susVecSensor[9], sus9Value); + MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9], + susVecBody[9], 3, 3, 1); + } + if (susValid[10]) { + susConverter.calculateSunVector(susVecSensor[10], sus10Value); + MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10], + susVecBody[10], 3, 3, 1); + } + if (susValid[11]) { + susConverter.calculateSunVector(susVecSensor[11], sus11Value); + MatrixOperations::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11], + susVecBody[11], 3, 3, 1); + } double susMeanValue[3] = {0, 0, 0}; for (uint8_t i = 0; i < 12; i++) { - if (validIds[i]) { - susMeanValue[0] += susVecBody[0][i]; - susMeanValue[1] += susVecBody[1][i]; - susMeanValue[2] += susVecBody[2][i]; - } + susMeanValue[0] += susVecBody[i][0]; + susMeanValue[1] += susVecBody[i][1]; + susMeanValue[2] += susVecBody[i][2]; } double susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); @@ -406,29 +367,29 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float)); susDataProcessed->sus0vec.setValid(sus0valid); - std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float)); susDataProcessed->sus1vec.setValid(sus1valid); - std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float)); susDataProcessed->sus2vec.setValid(sus2valid); - std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float)); susDataProcessed->sus3vec.setValid(sus3valid); - std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float)); susDataProcessed->sus4vec.setValid(sus4valid); - std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float)); susDataProcessed->sus5vec.setValid(sus5valid); - std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float)); susDataProcessed->sus6vec.setValid(sus6valid); - std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float)); susDataProcessed->sus7vec.setValid(sus7valid); - std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float)); susDataProcessed->sus8vec.setValid(sus8valid); - std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float)); susDataProcessed->sus9vec.setValid(sus9valid); - std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float)); susDataProcessed->sus10vec.setValid(sus10valid); - std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float)); susDataProcessed->sus11vec.setValid(sus11valid); std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double)); susDataProcessed->susVecTot.setValid(true); @@ -459,12 +420,11 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - double zeroVector[3] = {0.0, 0.0, 0.0}; - std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double)); - std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr0vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 3 * sizeof(double)); gyrDataProcessed->setValidity(false, true); } } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 47449a75..6dbc5d58 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,6 +23,9 @@ class SensorProcessing { acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); // Will call protected functions private: + static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; + static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; + protected: // short description needed for every function void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 1a645270..31cc0371 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -1,121 +1,64 @@ #include "SusConverter.h" -#include -#include -#include #include -#include - -bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { - if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || +uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { + if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH || susChannel[0] > susChannel[GNDREF]) { - return false; + return 0; } - if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || + if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH || susChannel[1] > susChannel[GNDREF]) { - return false; + return 0; }; - if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || + if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH || susChannel[2] > susChannel[GNDREF]) { - return false; + return 0; }; - if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || + if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH || susChannel[3] > susChannel[GNDREF]) { - return false; + return 0; }; - susChannelValueSum = + uint64_t susChannelValueSum = 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); - if ((susChannelValueSum < susChannelValueSumHigh) && - (susChannelValueSum > susChannelValueSumLow)) { - return false; + if (susChannelValueSum < SUS_ALBEDO_CHECK) { + return 0; }; - return true; + return susChannelValueSum; } -void SusConverter::calcAngle(const uint16_t susChannel[6]) { - float xout, yout; - float s = 0.03; // s=[mm] gap between diodes - uint8_t d = 5; // d=[mm] edge length of the quadratic aperture - uint8_t h = 1; // h=[mm] distance between diodes and aperture - int ch0, ch1, ch2, ch3; +bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12], + const float threshold) { + uint8_t maxBrightness = 0; + VectorOperations::maxValue(brightness, 12, &maxBrightness); + if (brightness[maxBrightness] == 0) { + return true; + } + for (uint8_t idx = 0; idx < 12; idx++) { + if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) { + susValid[idx] = false; + continue; + } + susValid[idx] = true; + } + return false; +} + +void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) { // Substract measurement values from GNDREF zero current threshold - ch0 = susChannel[GNDREF] - susChannel[0]; - ch1 = susChannel[GNDREF] - susChannel[1]; - ch2 = susChannel[GNDREF] - susChannel[2]; - ch3 = susChannel[GNDREF] - susChannel[3]; + float ch0 = susChannel[GNDREF] - susChannel[0]; + float ch1 = susChannel[GNDREF] - susChannel[1]; + float ch2 = susChannel[GNDREF] - susChannel[2]; + float ch3 = susChannel[GNDREF] - susChannel[3]; // Calculation of x and y - xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] - yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float xout = ((D - S) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + float yout = ((D - S) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] - alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] -} - -void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { - uint8_t index, k, l; - - // while loop iterates above all calibration cells to use the different calibration functions in - // each cell - k = 0; - while (k < 3) { - k++; - l = 0; - while (l < 3) { - l++; - // if-condition to check in which cell the data point has to be - if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) && - alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) && - (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) && - alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) { - index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell - alphaBetaCalibrated[0] = - coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + - coeffAlpha[index][2] * alphaBetaRaw[1] + - coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] - alphaBetaCalibrated[1] = - coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] + - coeffBeta[index][2] * alphaBetaRaw[1] + - coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + - coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + - coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + - coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] - } - } - } -} - -float* SusConverter::calculateSunVector() { - // Calculate the normalized Sun Vector - sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / - (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / - (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorSensorFrame[2] = - -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - - return sunVectorSensorFrame; -} - -float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], - const float coeffAlpha[9][10], - const float coeffBeta[9][10]) { - calcAngle(susChannel); - calibration(coeffAlpha, coeffBeta); - return calculateSunVector(); + sunVectorSensorFrame[0] = -xout; + sunVectorSensorFrame[1] = -yout; + sunVectorSensorFrame[2] = H; + VectorOperations::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3); } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 046b0ca8..8a6c279b 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -1,8 +1,4 @@ -#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ -#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ - -#include -#include +#include #include "AcsParameters.h" @@ -10,41 +6,26 @@ class SusConverter { public: SusConverter() {} - bool checkSunSensorData(const uint16_t susChannel[6]); - - void calcAngle(const uint16_t susChannel[6]); - void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); - float* calculateSunVector(); - - float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], - const float coeffBeta[9][10]); + uint64_t checkSunSensorData(const uint16_t susChannel[6]); + bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold); + void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]); private: - float alphaBetaRaw[2]; //[°] - float alphaBetaCalibrated[2]; //[°] - float sunVectorSensorFrame[3]; //[-] - - bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, - returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, - returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; - static const uint8_t GNDREF = 4; - uint16_t susChannelValueCheckHigh = - 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t susChannelValueCheckLow = - 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t susChannelValueSumHigh = - 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by - // the reflection of sunlight from the moon/earth - uint8_t susChannelValueSumLow = - 0; //[Bit]low borderline for check if the sun sensor is illuminated - // by the sun or by the reflection of sunlight from the moon/earth - uint8_t completeCellWidth = 140, - halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in - // which cell a data point should be - uint16_t susChannelValueSum = 0; + // =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check + static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096; + // [Bit]low borderline for the channel values of one sun sensor for validity Check + static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0; + // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the + // reflection of sunlight from the moon/earth + static constexpr uint16_t SUS_ALBEDO_CHECK = 1000; + // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection + // of sunlight from the moon/earth + static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0; + + static constexpr float S = 0.03; // S=[mm] gap between diodes + static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture + static constexpr float H = 2.5; // H=[mm] distance between diodes and aperture AcsParameters acsParameters; }; - -#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */ diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 9ca20862..8f422ec1 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,8 +7,10 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw) { +acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, + const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 4424896e..9fca77e6 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,8 +11,9 @@ class Detumble { Detumble(); virtual ~Detumble(); - uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw); + acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw); void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, double gain); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 1b20efb9..2f5847cc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,9 +5,6 @@ #include #include #include -#include - -#include "../util/MathOperations.h" PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } @@ -32,12 +29,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorLaw[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(qError[i]) < qErrorMin) { + if (std::abs(qError[i]) < qErrorMin) { qErrorLaw[i] = qErrorMin; } else { - qErrorLaw[i] = abs(qError[i]); + qErrorLaw[i] = std::abs(qError[i]); } } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); double gain1 = cInt * omMax / qErrorLawNorm; @@ -73,7 +71,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double pErrorSign[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(pError[i]) > 1) { + if (std::abs(pError[i]) > 1) { pErrorSign[i] = sign(pError[i]); } else { pErrorSign[i] = pError[i]; @@ -98,61 +96,92 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs) { + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); + + // calculate RPM offset utilizing the nullspace + double rpmOffset[4] = {0, 0, 0, 0}; + double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed, + rpmOffset, 4); + + // calculate resulting angular momentum + double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; + VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, + rwAngMomentum, 4); + + // calculate resulting torque + double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(acsParameters->rwMatrices.nullspaceVector, + acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4, + 1, 4); + MatrixOperations::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs, + 4); +} + void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, - int32_t *speedRw3, double *mgtDpDes) { - if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { - mgtDpDes[0] = 0; - mgtDpDes[1] = 0; - mgtDpDes[2] = 0; + const double *magFieldB, const bool magFieldBValid, + const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { + if (not magFieldBValid or not pointingLawParameters->desatOn) { return; } - // calculating momentum of satellite and momentum of reaction wheels - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; - double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, - momentumRwU, 4); - MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, - momentumRw, 3, 4, 1); - double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, - momentumSat, 3, 3, 1); - VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error - double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, - deltaMomentum, 3); - // resulting magnetic dipole command - double crossMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); - double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingLawParameters->deSatGainFactor) / normMag; - VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); -} + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; -void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; - double wheelMomentum[4] = {0, 0, 0, 0}; - double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; - // conversion to [rad/s] for further calculations - VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); - VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); - double diffRwSpeed[4] = {0, 0, 0, 0}; - VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, - wheelMomentum, 4); - double gainNs = pointingLawParameters->gainNullspace; - double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, - acsParameters->rwMatrices.nullspace, - *nullSpaceMatrix, 4); - MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); - VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); - VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); + // convert magFieldB from uT to T + double magFieldBT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate angular momentum of the satellite + double angMomentumSat[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, + angMomentumSat, 3, 3, 1); + + // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed + // relocate RW speed zero to nullspace RW speed + double refSpeedRws[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert speed from 10 RPM to 1 RPM + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + // convert to rad/s + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); + // calculate angular momentum of each RW + double angMomentumRwU[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + angMomentumRwU, 4); + // convert RW angular momentum to body RF + double angMomentumRw[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU, + angMomentumRw, 3, 4, 1); + + // calculate total angular momentum + double angMomentumTotal[3] = {0, 0, 0}; + VectorOperations::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); + + // calculating momentum error + double deltaAngMomentum[3] = {0, 0, 0}; + VectorOperations::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef, + deltaAngMomentum, 3); + + // resulting magnetic dipole command + double crossAngMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + double factor = + pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); + VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); } void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { @@ -169,15 +198,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee if (rwCmdSpeeds[i] != 0) { if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { - if (currRwSpeed[i] == 0) { - if (rwCmdSpeeds[i] > 0) { - rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (rwCmdSpeeds[i] < 0) { - rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; - } - } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) { + if (rwCmdSpeeds[i] > currRwSpeed[i]) { rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + } else if (rwCmdSpeeds[i] < currRwSpeed[i]) { rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fad72e6b..5f731e6b 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,13 +1,10 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ +#include +#include +#include #include -#include -#include - -#include "../AcsParameters.h" -#include "../SensorValues.h" -#include "eive/resultClassIds.h" class PtgCtrl { /* @@ -29,14 +26,14 @@ class PtgCtrl { void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, - double *mgtDpDes); - void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, double *rwTrqNs); + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs); + + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, const double *satRate, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after antistiction @@ -45,6 +42,7 @@ class PtgCtrl { private: const AcsParameters *acsParameters; + static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index f0ebd5a0..43677ccf 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, + const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 91625360..12f9ddb0 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,9 +12,9 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp index 2dd00ee6..cbe78c7f 100644 --- a/mission/genericFactory.cpp +++ b/mission/genericFactory.cpp @@ -49,6 +49,7 @@ #include "mission/system/acs/acsModeTree.h" #include "mission/system/tcs/tcsModeTree.h" #include "mission/tcs/defs.h" +#include "mission/tmtc/Service15TmStorage.h" #include "mission/tmtc/tmFilters.h" #include "objects/systemObjectList.h" #include "tmtc/pusIds.h" @@ -58,15 +59,13 @@ using persTmStore::PersistentTmStores; #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 // UDP server includes -#include "devices/gpioIds.h" -#include "fsfw/osal/common/UdpTcPollingTask.h" -#include "fsfw/osal/common/UdpTmTcBridge.h" +#include +#include #endif #if OBSW_ADD_TMTC_TCP_SERVER == 1 // TCP server includes -#include "fsfw/osal/common/TcpTmTcBridge.h" -#include "fsfw/osal/common/TcpTmTcServer.h" -#include "mission/tmtc/Service15TmStorage.h" +#include +#include #endif #endif @@ -234,16 +233,17 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun // PUS service stack new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID, - pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 40); + pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, + config::VERIFICATION_SERVICE_QUEUE_DEPTH); new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID, pus::PUS_SERVICE_2, 3, 10); new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID, - pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH); + pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16); new Service5EventReporting( PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5), 80, 160); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID, - pus::PUS_SERVICE_8, 16, 60); + pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9)); diff --git a/mission/power/BpxBatteryHandler.cpp b/mission/power/BpxBatteryHandler.cpp index b4aece40..96253705 100644 --- a/mission/power/BpxBatteryHandler.cpp +++ b/mission/power/BpxBatteryHandler.cpp @@ -1,4 +1,5 @@ #include +#include #include BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, @@ -51,6 +52,9 @@ void BpxBatteryHandler::fillCommandAndReplyMap() { insertInCommandAndReplyMap(bpxBat::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN); insertInCommandAndReplyMap(bpxBat::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::CONFIG_SET, 1, nullptr, EMPTY_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_ON, 1, nullptr, MAN_HEAT_REPLY_LEN); + insertInCommandAndReplyMap(bpxBat::MAN_HEAT_OFF, 1, nullptr, MAN_HEAT_REPLY_LEN); } ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -155,7 +159,7 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai case (bpxBat::PING): case (bpxBat::MAN_HEAT_ON): case (bpxBat::MAN_HEAT_OFF): { - if (remainingSize != PING_REPLY_LEN) { + if (remainingSize != MAN_HEAT_REPLY_LEN) { return DeviceHandlerIF::LENGTH_MISSMATCH; } break; diff --git a/mission/power/GomspaceDeviceHandler.cpp b/mission/power/GomspaceDeviceHandler.cpp index 89fca8ee..c20ac4e9 100644 --- a/mission/power/GomspaceDeviceHandler.cpp +++ b/mission/power/GomspaceDeviceHandler.cpp @@ -595,8 +595,8 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { coreHk.voltages[idx] = as(packet + 0x12 + (idx * 2)); } - auxHk.vcc.value = as(packet + 0x24); - auxHk.vbat.value = as(packet + 0x26); + coreHk.vcc.value = as(packet + 0x24); + coreHk.vbat.value = as(packet + 0x26); coreHk.temperature = as(packet + 0x28) * 0.1; for (uint8_t idx = 0; idx < 3; idx++) { diff --git a/mission/power/P60DockHandler.cpp b/mission/power/P60DockHandler.cpp index d643aef1..77fd9aaf 100644 --- a/mission/power/P60DockHandler.cpp +++ b/mission/power/P60DockHandler.cpp @@ -66,7 +66,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.battMode = newBattMode; - auxHk.heaterOn = *(packet + 0x57); + auxHk.heaterForBp4PackOn = *(packet + 0x57); auxHk.converter5VStatus = *(packet + 0x58); for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { @@ -111,6 +111,8 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.setValidity(true, true); auxHk.setValidity(true, true); + // No BP4 pack, no this is always invalid. + auxHk.heaterForBp4PackOn.setValid(false); } ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/power/bpxBattDefs.h b/mission/power/bpxBattDefs.h index 6df87efd..0e167f07 100644 --- a/mission/power/bpxBattDefs.h +++ b/mission/power/bpxBattDefs.h @@ -48,6 +48,7 @@ static constexpr uint32_t CFG_SET_ID = CONFIG_GET; static constexpr size_t GET_HK_REPLY_LEN = 23; static constexpr size_t PING_REPLY_LEN = 3; static constexpr size_t EMPTY_REPLY_LEN = 2; +static constexpr size_t MAN_HEAT_REPLY_LEN = 3; static constexpr size_t CONFIG_GET_REPLY_LEN = 5; static constexpr uint8_t PORT_PING = 1; @@ -219,6 +220,7 @@ class BpxBatteryCfg : public StaticLocalDataSet { if (size < 3) { return SerializeIF::STREAM_TOO_SHORT; } + battheatermode.value = data[0]; battheaterLow.value = data[1]; battheaterHigh.value = data[2]; diff --git a/mission/power/gsDefs.h b/mission/power/gsDefs.h index d42cabd2..1ad000d1 100644 --- a/mission/power/gsDefs.h +++ b/mission/power/gsDefs.h @@ -260,7 +260,8 @@ class HkTableDataset : public StaticLocalDataSet<32> { lp_var_t resetcause = lp_var_t(sid.objectId, pool::P60DOCK_RESETCAUSE, this); /** Battery heater control only possible on BP4 packs */ - lp_var_t heaterOn = lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); + lp_var_t heaterForBp4PackOn = + lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); lp_var_t converter5VStatus = lp_var_t(sid.objectId, pool::P60DOCK_CONV_5V_ENABLE_STATUS, this); @@ -404,6 +405,11 @@ class PduCoreHk : public StaticLocalDataSet<9> { /** Battery mode: 1 = Critical, 2 = Safe, 3 = Normal, 4 = Full */ lp_var_t battMode = lp_var_t(sid.objectId, pool::PDU_BATT_MODE, this); lp_var_t temperature = lp_var_t(sid.objectId, pool::PDU_TEMPERATURE, this); + + /** Measured VCC */ + lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); + /** Measured VBAT */ + lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); }; class PduConfig : public StaticLocalDataSet<32> { @@ -451,11 +457,6 @@ class PduAuxHk : public StaticLocalDataSet<36> { PduAuxHk(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} - /** Measured VCC */ - lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); - /** Measured VBAT */ - lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); - /** Output converter enable status */ lp_vec_t converterEnable = lp_vec_t(sid.objectId, pool::PDU_CONV_EN, this); diff --git a/mission/sysDefs.h b/mission/sysDefs.h index 9f402226..f906453b 100644 --- a/mission/sysDefs.h +++ b/mission/sysDefs.h @@ -44,6 +44,8 @@ static constexpr char VERSION_FILE_NAME[] = "version.txt"; static constexpr char REBOOT_FILE_NAME[] = "reboot.txt"; static constexpr char TIME_FILE_NAME[] = "time_backup.txt"; +static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000; + static constexpr ActionId_t ANNOUNCE_VERSION = 1; static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2; static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3; @@ -113,6 +115,10 @@ static constexpr Event TRYING_I2C_RECOVERY = event::makeEvent(SUBSYSTEM_ID, 10, static constexpr Event I2C_REBOOT = event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH); //! [EXPORT] : [COMMENT] PDEC recovery through reset was not possible, performing full reboot. static constexpr Event PDEC_REBOOT = event::makeEvent(SUBSYSTEM_ID, 12, severity::HIGH); +//! [EXPORT] : [COMMENT] Version information of the firmware (not OBSW). +//! P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash +//! P2: First four letters of Git SHA is the last byte of P1 is set. +static constexpr Event FIRMWARE_INFO = event::makeEvent(SUBSYSTEM_ID, 13, severity::INFO); class ListDirectoryCmdBase { public: // TODO: Packet definition for clean deserialization diff --git a/mission/tcs/Tmp1075Handler.cpp b/mission/tcs/Tmp1075Handler.cpp index df57aa8a..30331e1f 100644 --- a/mission/tcs/Tmp1075Handler.cpp +++ b/mission/tcs/Tmp1075Handler.cpp @@ -86,8 +86,11 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { case TMP1075::GET_TEMP: { - int16_t tempValueRaw = 0; - tempValueRaw = packet[0] << 4 | packet[1] >> 4; + // Convert 12 bit MSB first raw temperature to 16 bit first. + int16_t tempValueRaw = static_cast((packet[0] << 8) | packet[1]) >> 4; + // Sign extension to 16 bits: If the sign bit is set, fill up with ones on the left. + tempValueRaw = (packet[0] & 0x80) ? (tempValueRaw | 0xF000) : tempValueRaw; + // 0.0625 is the sensor sensitivity. float tempValue = ((static_cast(tempValueRaw)) * 0.0625); #if OBSW_DEBUG_TMP1075 == 1 sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId() diff --git a/mission/tcs/defs.h b/mission/tcs/defs.h index cb1a73e4..747dbc6d 100644 --- a/mission/tcs/defs.h +++ b/mission/tcs/defs.h @@ -5,10 +5,10 @@ namespace heater { enum Switch : uint8_t { - HEATER_0_OBC_BRD, - HEATER_1_PLOC_PROC_BRD, + HEATER_0_PLOC_PROC_BRD, + HEATER_1_PCDU_PDU, HEATER_2_ACS_BRD, - HEATER_3_PCDU_PDU, + HEATER_3_OBC_BRD, HEATER_4_CAMERA, HEATER_5_STR, HEATER_6_DRO, diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp index 8174f8a0..08a7ea45 100644 --- a/mission/tmtc/PersistentTmStore.cpp +++ b/mission/tmtc/PersistentTmStore.cpp @@ -268,6 +268,10 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi // restore the file dump, but for now do not trust the file. std::error_code e; std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e); + if (dumpParams.dirEntry.path() == activeFile) { + activeFile == std::nullopt; + assignAndOrCreateMostRecentFile(); + } fileHasSwapped = true; return loadNextDumpFile(); } diff --git a/release_checklist.md b/release_checklist.md index 4c8be066..04a0233c 100644 --- a/release_checklist.md +++ b/release_checklist.md @@ -7,7 +7,9 @@ OBSW Release Checklist 2. Re-run the generators with `generators/gen.py all` 3. Re-run the auto-formatters with the `scripts/auto-formatter.sh` script 4. Verify that the Q7S, Q7S EM and Host build are working -5. Wait for CI/CD results +5. Update `CHANGELOG.md`: Add new `unreleased` section, convert old unreleased section to + header containing version number and release date. +6. Wait for CI/CD results # Post-Release diff --git a/tmtc b/tmtc index 6182369e..1bb8bea8 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6182369e4f40872c5c26e59be25d5fa79339176a +Subproject commit 1bb8bea8d92fef2c9ec58ea657b04b56635c16dd