diff --git a/.gitmodules b/.gitmodules index 7af82082..6f538287 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,3 +13,9 @@ [submodule "thirdparty/lwgps"] path = thirdparty/lwgps url = https://github.com/rmspacefish/lwgps.git +[submodule "fsfw_hal"] + path = fsfw_hal + url = https://egit.irs.uni-stuttgart.de/fsfw/fsfw_hal.git +[submodule "generators/modgen"] + path = generators/modgen + url = https://git.ksat-stuttgart.de/source/modgen.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 53689123..6e4eb527 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,6 +40,7 @@ set(TARGET_NAME ${CMAKE_PROJECT_NAME}) set(LIB_FSFW_NAME fsfw) set(LIB_ETL_NAME etl) set(LIB_CSP_NAME libcsp) +set(LIB_FSFW_HAL_NAME fsfw_hal) set(LIB_LWGPS_NAME lwgps) set(THIRD_PARTY_FOLDER thirdparty) @@ -50,12 +51,13 @@ set(TEST_PATH test/testtasks) set(LINUX_PATH linux) set(COMMON_PATH common) +set(FSFW_HAL_LIB_PATH fsfw_hal) set(CSP_LIB_PATH ${THIRD_PARTY_FOLDER}/libcsp) set(ETL_LIB_PATH ${THIRD_PARTY_FOLDER}/etl) set(LWGPS_LIB_PATH ${THIRD_PARTY_FOLDER}/lwgps) set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF) -set(ADD_LINUX_FILES FALSE) +set(ADD_LINUX_FILES TRUE) # Analyse different OS and architecture/target options, determine BSP_PATH, # display information about compiler etc. @@ -68,10 +70,12 @@ if(TGT_BSP) set(FSFW_CONFIG_PATH "fsfwconfig") set(ADD_LINUX_FILES TRUE) set(ADD_CSP_LIB TRUE) + set(FSFW_HAL_ADD_LINUX ON) endif() if(${TGT_BSP} MATCHES "arm/raspberrypi") add_definitions(-DRASPBERRY_PI) + set(FSFW_HAL_ADD_RASPBERRY_PI ON) endif() if(${TGT_BSP} MATCHES "arm/q7s") @@ -113,6 +117,7 @@ add_subdirectory(${BSP_PATH}) add_subdirectory(${FSFW_PATH}) add_subdirectory(${MISSION_PATH}) add_subdirectory(${TEST_PATH}) +add_subdirectory(${FSFW_HAL_LIB_PATH}) add_subdirectory(${COMMON_PATH}) ################################################################################ @@ -126,6 +131,7 @@ target_link_libraries(${TARGET_NAME} PRIVATE ${LIB_FSFW_NAME} ${LIB_OS_NAME} ${LIB_LWGPS_NAME} + ${LIB_FSFW_HAL_NAME} ) if(ADD_ETL_LIB) diff --git a/README.md b/README.md index 67a2f9b6..38df4443 100644 --- a/README.md +++ b/README.md @@ -198,6 +198,12 @@ IP address and path settings differ from machine to machine. Open SSH connection to flatsat PC: +```sh +ssh eive@flatsat.eive.absatvirt.lw +``` + +or + ```sh ssh eive@2001:7c0:2018:1099:babe:0:e1fe:f1a5 ``` @@ -266,6 +272,16 @@ Then you can copy an `example` file like this scp -P 1535 example root@localhost:/tmp ``` +Copying a file from Q7S to flatsat PC +```` +scp -P 22 root@192.168.133.10:/tmp/kernel-config /tmp +```` + +From a windows machine files can be copied with putty tools +```` +pscp -scp -P 22 eive@192.168.199.227:/example-file +```` + ## Launching an application at start-up Load the root partiton from the flash memory (there are to nor-flash memories and each flash holds two xdi images). diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 798fa7ec..f3d7591d 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -11,13 +11,8 @@ #include #include -#ifdef LINUX -#include -#include -#elif WIN32 -#include -#include -#endif +#include +#include #include @@ -45,17 +40,9 @@ void ObjectFactory::produce(){ Factory::setStaticFrameworkObjectIds(); ObjectFactory::produceGenericObjects(); -#ifdef LINUX - new TmTcUnixUdpBridge(objects::UDP_BRIDGE, + new UdpTmTcBridge(objects::UDP_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR, objects::TM_STORE, objects::TC_STORE); - new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); -#elif WIN32 - new TmTcWinUdpBridge(objects::UDP_BRIDGE, - objects::CCSDS_PACKET_DISTRIBUTOR, objects::TM_STORE, - objects::TC_STORE); - new TcWinUdpPollingTask(objects::UDP_POLLING_TASK, - objects::UDP_BRIDGE); -#endif + new UdpTcPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); } diff --git a/bsp_hosted/fsfwconfig/devices/gpioIds.h b/bsp_hosted/fsfwconfig/devices/gpioIds.h index 8963bd20..df49c0b7 100644 --- a/bsp_hosted/fsfwconfig/devices/gpioIds.h +++ b/bsp_hosted/fsfwconfig/devices/gpioIds.h @@ -25,7 +25,31 @@ namespace gpioIds { MGM_3_RM3100_CS, TEST_ID_0, - TEST_ID_1 + TEST_ID_1, + + RTD_IC3, + RTD_IC4, + RTD_IC5, + RTD_IC6, + RTD_IC7, + RTD_IC8, + RTD_IC9, + RTD_IC10, + RTD_IC11, + RTD_IC12, + RTD_IC13, + RTD_IC14, + RTD_IC15, + RTD_IC16, + RTD_IC17, + RTD_IC18, + + SPI_MUX_BIT_1, + SPI_MUX_BIT_2, + SPI_MUX_BIT_3, + SPI_MUX_BIT_4, + SPI_MUX_BIT_5, + SPI_MUX_BIT_6 }; } diff --git a/bsp_q7s/CMakeLists.txt b/bsp_q7s/CMakeLists.txt index 7b985558..e567628e 100644 --- a/bsp_q7s/CMakeLists.txt +++ b/bsp_q7s/CMakeLists.txt @@ -8,6 +8,7 @@ add_subdirectory(boardconfig) add_subdirectory(comIF) add_subdirectory(devices) add_subdirectory(boardtest) +add_subdirectory(gpio) diff --git a/bsp_q7s/InitMission.cpp b/bsp_q7s/InitMission.cpp index 4fda5f85..3fe0ba0c 100644 --- a/bsp_q7s/InitMission.cpp +++ b/bsp_q7s/InitMission.cpp @@ -115,6 +115,10 @@ void initmission::initTasks() { if(result != HasReturnvaluesIF::RETURN_OK) { initmission::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } + result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); + if(result!=HasReturnvaluesIF::RETURN_OK){ + sif::error << "Object add component failed" << std::endl; + } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); if(result != HasReturnvaluesIF::RETURN_OK) { initmission::printAddObjectError("PUS_200", objects::PUS_SERVICE_200_MODE_MGMT); @@ -133,6 +137,7 @@ void initmission::initTasks() { //TODO: Add handling of missed deadlines /* Polling Sequence Table Default */ +#if Q7S_ADD_SPI_TEST == 0 FixedTimeslotTaskIF * pollingSequenceTableTaskDefault = factory->createFixedTimeslotTask( "PST_TASK_DEFAULT", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc); @@ -140,6 +145,7 @@ void initmission::initTasks() { if (result != HasReturnvaluesIF::RETURN_OK) { sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl; } +#endif #if TE0720 == 0 FixedTimeslotTaskIF* gomSpacePstTask = factory-> @@ -151,13 +157,20 @@ void initmission::initTasks() { } #endif - PeriodicTaskIF* testTask = factory->createPeriodicTask( - "GPIOD_TEST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, missedDeadlineFunc); #if OBSW_ADD_TEST_CODE == 1 + PeriodicTaskIF* testTask = factory->createPeriodicTask( + "TEST_TASK", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, missedDeadlineFunc); result = testTask->addComponent(objects::TEST_TASK); if(result != HasReturnvaluesIF::RETURN_OK) { initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK); } + +#if Q7S_ADD_SPI_TEST == 1 + result = testTask->addComponent(objects::SPI_TEST); + if(result != HasReturnvaluesIF::RETURN_OK) { + initmission::printAddObjectError("SPI_TEST", objects::SPI_TEST); + } +#endif #endif /* OBSW_ADD_TEST_CODE == 1 */ #if TE0720 == 1 && TEST_LIBGPIOD == 1 result = testTask->addComponent(objects::LIBGPIOD_TEST); @@ -174,7 +187,10 @@ void initmission::initTasks() { #if TE0720 == 0 gomSpacePstTask->startTask(); #endif + +#if Q7S_ADD_SPI_TEST == 0 pollingSequenceTableTaskDefault->startTask(); +#endif pusVerification->startTask(); pusEvents->startTask(); @@ -182,6 +198,8 @@ void initmission::initTasks() { pusMedPrio->startTask(); pusLowPrio->startTask(); +#if OBSW_ADD_TEST_CODE == 1 testTask->startTask(); +#endif sif::info << "Tasks started.." << std::endl; } diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp index f81fc173..5519b84b 100644 --- a/bsp_q7s/ObjectFactory.cpp +++ b/bsp_q7s/ObjectFactory.cpp @@ -5,9 +5,11 @@ #include #include #include +#include #include #include +#include #include #include @@ -16,23 +18,40 @@ #include #include #include -#include +#include +#include +#include +#include +#include +#include +#include + #include +#include #include #include #include -#include -#include -#include -#include + +#include +#include + +#include +#include +#include +#include +#include +#include #include #include #include -#include #include -#include + +/* UDP server includes */ +#include +#include +#include #if TEST_LIBGPIOD == 1 #include @@ -49,10 +68,10 @@ void Factory::setStaticFrameworkObjectIds() { // No storage object for now. TmFunnel::storageDestination = objects::NO_OBJECT; - LocalDataPoolManager::defaultHkDestination = objects::NO_OBJECT; + LocalDataPoolManager::defaultHkDestination = objects::PUS_SERVICE_3_HOUSEKEEPING; VerificationReporter::messageReceiver = objects::PUS_SERVICE_1_VERIFICATION; - TmPacketStored::timeStamperId = objects::TIME_STAMPER; + TmPacketBase::timeStamperId = objects::TIME_STAMPER; } @@ -72,9 +91,27 @@ void ObjectFactory::produce(){ I2cCookie* i2cCookieTmp1075tcs2 = new I2cCookie(addresses::TMP1075_TCS_2, TMP1075::MAX_REPLY_LENGTH, std::string("/dev/i2c-1")); #endif + LinuxLibgpioIF* gpioComIF = new LinuxLibgpioIF(objects::GPIO_IF); + /* Communication interfaces */ new CspComIF(objects::CSP_COM_IF); new I2cComIF(objects::I2C_COM_IF); + new UartComIF(objects::UART_COM_IF); +#if Q7S_ADD_SPI_TEST == 0 + new SpiComIF(objects::SPI_COM_IF, gpioComIF); +#endif + + /* Temperature sensors */ + Tmp1075Handler* tmp1075Handler_1 = new Tmp1075Handler( + objects::TMP1075_HANDLER_1, objects::I2C_COM_IF, + i2cCookieTmp1075tcs1); + tmp1075Handler_1->setStartUpImmediately(); + Tmp1075Handler* tmp1075Handler_2 = new Tmp1075Handler( + objects::TMP1075_HANDLER_2, objects::I2C_COM_IF, + i2cCookieTmp1075tcs2); + tmp1075Handler_2->setStartUpImmediately(); + + GpioCookie* heaterGpiosCookie = new GpioCookie; #if TE0720 == 0 CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_LENGTH, @@ -100,94 +137,303 @@ void ObjectFactory::produce(){ * Setting PCDU devices to mode normal immediately after start up because PCDU is always * running. */ - //p60dockhandler->setModeNormal(); - //pdu1handler->setModeNormal(); - //pdu2handler->setModeNormal(); - //acuhandler->setModeNormal(); -#endif - /* Temperature sensors */ - Tmp1075Handler* tmp1075Handler_1 = new Tmp1075Handler( - objects::TMP1075_HANDLER_1, objects::I2C_COM_IF, - i2cCookieTmp1075tcs1); - tmp1075Handler_1->setStartUpImmediately(); - Tmp1075Handler* tmp1075Handler_2 = new Tmp1075Handler( - objects::TMP1075_HANDLER_2, objects::I2C_COM_IF, - i2cCookieTmp1075tcs2); - tmp1075Handler_2->setStartUpImmediately(); + /** For now this needs to be commented out because there is no PCDU connected to the OBC */ +// p60dockhandler->setModeNormal(); +// pdu1handler->setModeNormal(); +// pdu2handler->setModeNormal(); +// acuhandler->setModeNormal(); + (void) p60dockhandler; + (void) pdu1handler; + (void) pdu2handler; + (void) acuhandler; + +#if OBSW_ADD_ACS_BOARD == 1 + GpioCookie* gpioCookieAcsBoard = new GpioCookie(); + GpiodRegular* gpio = nullptr; + gpio = new GpiodRegular(std::string("gpiochip5"), 1, std::string("CS_GYRO_1_ADIS"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio); + gpio = new GpiodRegular(std::string("gpiochip5"), 7, std::string("CS_GYRO_2_L3G"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::GYRO_1_L3G_CS, gpio); + gpio = new GpiodRegular(std::string("gpiochip5"), 3, std::string("CS_GYRO_3_L3G"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::GYRO_2_L3G_CS, gpio); + + gpio = new GpiodRegular(std::string("gpiochip5"), 5, std::string("CS_MGM_0_LIS3_A"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::MGM_0_LIS3_CS, gpio); + + gpio = new GpiodRegular(std::string("gpiochip5"), 17, std::string("CS_MGM_1_RM3100_A"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::MGM_1_RM3100_CS, gpio); + + gpio = new GpiodRegular(std::string("gpiochip6"), 0, std::string("CS_MGM_2_LIS3_B"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::MGM_2_LIS3_CS, gpio); + + gpio = new GpiodRegular(std::string("gpiochip5"), 10, std::string("CS_MGM_3_RM3100_B"), + gpio::OUT, gpio::HIGH); + gpioCookieAcsBoard->addGpio(gpioIds::MGM_3_RM3100_CS, gpio); + + gpioComIF->addGpios(gpioCookieAcsBoard); + + std::string spiDev = "/dev/spidev2.0"; + SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, + MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); + auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, + objects::SPI_COM_IF, spiCookie); + mgmLis3Handler->setStartUpImmediately(); + + spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, spiDev, + MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); + auto mgmLis3Handler2 = new MGMHandlerLIS3MDL(objects::MGM_2_LIS3_HANDLER, + objects::SPI_COM_IF, spiCookie); + mgmLis3Handler2->setStartUpImmediately(); + + spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, + RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); + auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER, + objects::SPI_COM_IF, spiCookie); + mgmRm3100Handler->setStartUpImmediately(); + + spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev, + L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); + auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF, + spiCookie); + gyroL3gHandler->setStartUpImmediately(); +#endif - GpioCookie* heaterGpiosCookie = new GpioCookie; - new LinuxLibgpioIF(objects::GPIO_IF); -#if TE0720 == 0 /* Pin H2-11 on stack connector */ - GpiodRegular gpioConfigHeater0(std::string("gpiochip7"), 18, + GpiodRegular* gpioConfigHeater0 = new GpiodRegular(std::string("gpiochip7"), 6, std::string("Heater0"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpioConfigHeater0); /* Pin H2-12 on stack connector */ - GpiodRegular gpioConfigHeater1(std::string("gpiochip7"), 14, + GpiodRegular* gpioConfigHeater1 = new GpiodRegular(std::string("gpiochip7"), 12, std::string("Heater1"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_1, gpioConfigHeater1); /* Pin H2-13 on stack connector */ - GpiodRegular gpioConfigHeater2(std::string("gpiochip7"), 20, + GpiodRegular* gpioConfigHeater2 = new GpiodRegular(std::string("gpiochip7"), 7, std::string("Heater2"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_2, gpioConfigHeater2); - GpiodRegular gpioConfigHeater3(std::string("gpiochip7"), 16, + GpiodRegular* gpioConfigHeater3 = new GpiodRegular(std::string("gpiochip7"), 5, std::string("Heater3"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_3, gpioConfigHeater3); - GpiodRegular gpioConfigHeater4(std::string("gpiochip7"), 24, + GpiodRegular* gpioConfigHeater4 = new GpiodRegular(std::string("gpiochip7"), 3, std::string("Heater4"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_4, gpioConfigHeater4); - GpiodRegular gpioConfigHeater5(std::string("gpiochip7"), 26, + GpiodRegular* gpioConfigHeater5 = new GpiodRegular(std::string("gpiochip7"), 0, std::string("Heater5"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_5, gpioConfigHeater5); - GpiodRegular gpioConfigHeater6(std::string("gpiochip7"), 22, + GpiodRegular* gpioConfigHeater6 = new GpiodRegular(std::string("gpiochip7"), 1, std::string("Heater6"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_6, gpioConfigHeater6); - GpiodRegular gpioConfigHeater7(std::string("gpiochip7"), 28, + GpiodRegular* gpioConfigHeater7 = new GpiodRegular(std::string("gpiochip7"), 11, std::string("Heater7"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_7, gpioConfigHeater7); - new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie, objects::PCDU_HANDLER, - pcduSwitches::TCS_BOARD_8V_HEATER_IN); + new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie, + objects::PCDU_HANDLER, pcduSwitches::TCS_BOARD_8V_HEATER_IN); GpioCookie* solarArrayDeplCookie = new GpioCookie; - GpiodRegular gpioConfigDeplSA1(std::string("gpiochip7"), 25, - std::string("DeplSA1"), gpio::OUT, 0); - solarArrayDeplCookie->addGpio(gpioIds::DEPLSA1, gpioConfigDeplSA1); - GpiodRegular gpioConfigDeplSA2(std::string("gpiochip7"), 23, - std::string("DeplSA2"), gpio::OUT, 0); - solarArrayDeplCookie->addGpio(gpioIds::DEPLSA2, gpioConfigDeplSA2); + + GpiodRegular* gpioConfigDeplSA0 = new GpiodRegular(std::string("gpiochip7"), 4, + std::string("DeplSA1"), gpio::OUT, 0); + solarArrayDeplCookie->addGpio(gpioIds::DEPLSA1, gpioConfigDeplSA0); + GpiodRegular* gpioConfigDeplSA1 = new GpiodRegular(std::string("gpiochip7"), 2, + std::string("DeplSA2"), gpio::OUT, 0); + solarArrayDeplCookie->addGpio(gpioIds::DEPLSA2, gpioConfigDeplSA1); //TODO: Find out burn time. For now set to 1000 ms. new SolarArrayDeploymentHandler(objects::SOLAR_ARRAY_DEPL_HANDLER, objects::GPIO_IF, solarArrayDeplCookie, objects::PCDU_HANDLER, pcduSwitches::DEPLOYMENT_MECHANISM, gpioIds::DEPLSA1, gpioIds::DEPLSA2, 1000); -#endif - new TmTcUnixUdpBridge(objects::UDP_BRIDGE, - objects::CCSDS_PACKET_DISTRIBUTOR, - objects::TM_STORE, objects::TC_STORE); - new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); + UartCookie* syrlinksUartCookie = new UartCookie( + std::string("/dev/ttyUL0"), 38400, SYRLINKS::MAX_REPLY_SIZE); + syrlinksUartCookie->setParityEven(); + + SyrlinksHkHandler* syrlinksHkHandler = new SyrlinksHkHandler(objects::SYRLINKS_HK_HANDLER, + objects::UART_COM_IF, syrlinksUartCookie); + syrlinksHkHandler->setModeNormal(); + +#if Q7S_ADD_RTD_DEVICES == 1 + GpioCookie* rtdGpioCookie = new GpioCookie; + + gpioCallbacks::initTcsBoardDecoder(gpioComIF); + GpioCallback* gpioRtdIc3 = new GpioCallback(std::string("Chip select RTD IC3"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC3, gpioRtdIc3); + GpioCallback* gpioRtdIc4 = new GpioCallback(std::string("Chip select RTD IC4"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC4, gpioRtdIc4); + GpioCallback* gpioRtdIc5 = new GpioCallback(std::string("Chip select RTD IC5"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC5, gpioRtdIc5); + GpioCallback* gpioRtdIc6 = new GpioCallback(std::string("Chip select RTD IC6"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC6, gpioRtdIc6); + GpioCallback* gpioRtdIc7 = new GpioCallback(std::string("Chip select RTD IC7"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC7, gpioRtdIc7); + GpioCallback* gpioRtdIc8 = new GpioCallback(std::string("Chip select RTD IC8"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC8, gpioRtdIc8); + GpioCallback* gpioRtdIc9 = new GpioCallback(std::string("Chip select RTD IC9"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC9, gpioRtdIc9); + GpioCallback* gpioRtdIc10 = new GpioCallback(std::string("Chip select RTD IC10"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC10, gpioRtdIc10); + GpioCallback* gpioRtdIc11 = new GpioCallback(std::string("Chip select RTD IC11"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC11, gpioRtdIc11); + GpioCallback* gpioRtdIc12 = new GpioCallback(std::string("Chip select RTD IC12"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC12, gpioRtdIc12); + GpioCallback* gpioRtdIc13 = new GpioCallback(std::string("Chip select RTD IC13"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC13, gpioRtdIc13); + GpioCallback* gpioRtdIc14 = new GpioCallback(std::string("Chip select RTD IC14"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC14, gpioRtdIc14); + GpioCallback* gpioRtdIc15 = new GpioCallback(std::string("Chip select RTD IC15"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC15, gpioRtdIc15); + GpioCallback* gpioRtdIc16 = new GpioCallback(std::string("Chip select RTD IC16"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC16, gpioRtdIc16); + GpioCallback* gpioRtdIc17 = new GpioCallback(std::string("Chip select RTD IC17"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC17, gpioRtdIc17); + GpioCallback* gpioRtdIc18 = new GpioCallback(std::string("Chip select RTD IC18"), gpio::OUT, 1, + &gpioCallbacks::tcsBoardDecoderCallback, gpioComIF); + rtdGpioCookie->addGpio(gpioIds::RTD_IC18, gpioRtdIc18); + + gpioComIF->addGpios(rtdGpioCookie); + + SpiCookie* spiRtdIc3 = new SpiCookie(addresses::RTD_IC3, gpioIds::RTD_IC3, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc4 = new SpiCookie(addresses::RTD_IC4, gpioIds::RTD_IC4, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc5 = new SpiCookie(addresses::RTD_IC5, gpioIds::RTD_IC5, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc6 = new SpiCookie(addresses::RTD_IC6, gpioIds::RTD_IC6, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc7 = new SpiCookie(addresses::RTD_IC7, gpioIds::RTD_IC7, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc8 = new SpiCookie(addresses::RTD_IC8, gpioIds::RTD_IC8, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc9 = new SpiCookie(addresses::RTD_IC9, gpioIds::RTD_IC9, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc10 = new SpiCookie(addresses::RTD_IC10, gpioIds::RTD_IC10, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc11 = new SpiCookie(addresses::RTD_IC11, gpioIds::RTD_IC11, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc12 = new SpiCookie(addresses::RTD_IC12, gpioIds::RTD_IC12, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc13 = new SpiCookie(addresses::RTD_IC13, gpioIds::RTD_IC13, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc14 = new SpiCookie(addresses::RTD_IC14, gpioIds::RTD_IC14, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc15 = new SpiCookie(addresses::RTD_IC15, gpioIds::RTD_IC15, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc16 = new SpiCookie(addresses::RTD_IC16, gpioIds::RTD_IC16, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc17 = new SpiCookie(addresses::RTD_IC17, gpioIds::RTD_IC17, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + SpiCookie* spiRtdIc18 = new SpiCookie(addresses::RTD_IC18, gpioIds::RTD_IC18, + std::string("/dev/spidev2.0"), Max31865Definitions::MAX_REPLY_SIZE, + spi::SpiModes::MODE_1, 2000000); + + Max31865PT1000Handler* rtdIc3 = new Max31865PT1000Handler(objects::RTD_IC3, objects::SPI_COM_IF, spiRtdIc3, 0); // 0 is switchId + Max31865PT1000Handler* rtdIc4 = new Max31865PT1000Handler(objects::RTD_IC4, objects::SPI_COM_IF, spiRtdIc4, 0); + Max31865PT1000Handler* rtdIc5 = new Max31865PT1000Handler(objects::RTD_IC5, objects::SPI_COM_IF, spiRtdIc5, 0); + Max31865PT1000Handler* rtdIc6 = new Max31865PT1000Handler(objects::RTD_IC6, objects::SPI_COM_IF, spiRtdIc6, 0); + Max31865PT1000Handler* rtdIc7 = new Max31865PT1000Handler(objects::RTD_IC7, objects::SPI_COM_IF, spiRtdIc7, 0); + Max31865PT1000Handler* rtdIc8 = new Max31865PT1000Handler(objects::RTD_IC8, objects::SPI_COM_IF, spiRtdIc8, 0); + Max31865PT1000Handler* rtdIc9 = new Max31865PT1000Handler(objects::RTD_IC9, objects::SPI_COM_IF, spiRtdIc9, 0); + Max31865PT1000Handler* rtdIc10 = new Max31865PT1000Handler(objects::RTD_IC10, objects::SPI_COM_IF, spiRtdIc10, 0); + Max31865PT1000Handler* rtdIc11 = new Max31865PT1000Handler(objects::RTD_IC11, objects::SPI_COM_IF, spiRtdIc11, 0); + Max31865PT1000Handler* rtdIc12 = new Max31865PT1000Handler(objects::RTD_IC12, objects::SPI_COM_IF, spiRtdIc12, 0); + Max31865PT1000Handler* rtdIc13 = new Max31865PT1000Handler(objects::RTD_IC13, objects::SPI_COM_IF, spiRtdIc13, 0); + Max31865PT1000Handler* rtdIc14 = new Max31865PT1000Handler(objects::RTD_IC14, objects::SPI_COM_IF, spiRtdIc14, 0); + Max31865PT1000Handler* rtdIc15 = new Max31865PT1000Handler(objects::RTD_IC15, objects::SPI_COM_IF, spiRtdIc15, 0); + Max31865PT1000Handler* rtdIc16 = new Max31865PT1000Handler(objects::RTD_IC16, objects::SPI_COM_IF, spiRtdIc16, 0); + Max31865PT1000Handler* rtdIc17 = new Max31865PT1000Handler(objects::RTD_IC17, objects::SPI_COM_IF, spiRtdIc17, 0); + Max31865PT1000Handler* rtdIc18 = new Max31865PT1000Handler(objects::RTD_IC18, objects::SPI_COM_IF, spiRtdIc18, 0); +// rtdIc10->setStartUpImmediately(); +// rtdIc4->setStartUpImmediately(); + + (void) rtdIc3; + (void) rtdIc4; + (void) rtdIc5; + (void) rtdIc6; + (void) rtdIc7; + (void) rtdIc8; + (void) rtdIc9; + (void) rtdIc10; + (void) rtdIc11; + (void) rtdIc12; + (void) rtdIc13; + (void) rtdIc14; + (void) rtdIc15; + (void) rtdIc16; + (void) rtdIc17; + (void) rtdIc18; + +#endif /* Q7S_ADD_RTD_DEVICES == 1 */ + +#endif /* TE0720 == 0 */ + + new UdpTmTcBridge(objects::UDP_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR, objects::TM_STORE, + objects::TC_STORE); + new UdpTcPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); + + I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, + std::string("/dev/i2c-0")); + IMTQHandler* imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie); + imtqHandler->setStartUpImmediately(); #if TE0720 == 1 && TEST_LIBGPIOD == 1 /* Configure MIO0 as input */ - GpioConfig_t gpioConfigMio0(std::string("gpiochip0"), 0, + GpiodRegular gpioConfigMio0(std::string("gpiochip0"), 0, std::string("MIO0"), gpio::IN, 0); GpioCookie* gpioCookie = new GpioCookie; gpioCookie->addGpio(gpioIds::TEST_ID_0, gpioConfigMio0); new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookie); #elif TE0720 == 1 /* Configuration for MIO0 on TE0720-03-1CFA */ - GpioConfig_t gpioConfigForDummyHeater(std::string("gpiochip0"), 0, + GpiodRegular gpioConfigForDummyHeater(std::string("gpiochip0"), 0, std::string("Heater0"), gpio::OUT, 0); heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpioConfigForDummyHeater); new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie, objects::PCDU_HANDLER, pcduSwitches::TCS_BOARD_8V_HEATER_IN); #endif + +#if Q7S_ADD_SPI_TEST == 1 + new SpiTestClass(objects::SPI_TEST, gpioComIF); +#endif } diff --git a/bsp_q7s/boardconfig/q7s_config.h b/bsp_q7s/boardconfig/q7s_config.h new file mode 100644 index 00000000..782ecf4f --- /dev/null +++ b/bsp_q7s/boardconfig/q7s_config.h @@ -0,0 +1,13 @@ +#ifndef BSP_Q7S_BOARDCONFIG_Q7S_CONFIG_H_ +#define BSP_Q7S_BOARDCONFIG_Q7S_CONFIG_H_ + +#define Q7S_ADD_RTD_DEVICES 0 + +/* Only one of those 2 should be enabled! */ +/* Add code for ACS board */ +#define OBSW_ADD_ACS_BOARD 0 +#define Q7S_ADD_SPI_TEST 0 + + + +#endif /* BSP_Q7S_BOARDCONFIG_Q7S_CONFIG_H_ */ diff --git a/bsp_q7s/devices/HeaterHandler.cpp b/bsp_q7s/devices/HeaterHandler.cpp index 9a44c57b..21a4a3f4 100644 --- a/bsp_q7s/devices/HeaterHandler.cpp +++ b/bsp_q7s/devices/HeaterHandler.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_, CookieIF * gpioCookie_, object_id_t mainLineSwitcherObjectId_, uint8_t mainLineSwitch_) : diff --git a/bsp_q7s/devices/HeaterHandler.h b/bsp_q7s/devices/HeaterHandler.h index 01319ce9..8969ebd9 100644 --- a/bsp_q7s/devices/HeaterHandler.h +++ b/bsp_q7s/devices/HeaterHandler.h @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include /** diff --git a/bsp_q7s/devices/SolarArrayDeploymentHandler.cpp b/bsp_q7s/devices/SolarArrayDeploymentHandler.cpp index d307ef15..25be3927 100644 --- a/bsp_q7s/devices/SolarArrayDeploymentHandler.cpp +++ b/bsp_q7s/devices/SolarArrayDeploymentHandler.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include diff --git a/bsp_q7s/devices/SolarArrayDeploymentHandler.h b/bsp_q7s/devices/SolarArrayDeploymentHandler.h index b7e94f23..5e573128 100644 --- a/bsp_q7s/devices/SolarArrayDeploymentHandler.h +++ b/bsp_q7s/devices/SolarArrayDeploymentHandler.h @@ -1,8 +1,6 @@ #ifndef MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ #define MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ -#include - #include #include #include @@ -11,7 +9,7 @@ #include #include #include - +#include #include /** diff --git a/bsp_q7s/gpio/CMakeLists.txt b/bsp_q7s/gpio/CMakeLists.txt new file mode 100644 index 00000000..dd657546 --- /dev/null +++ b/bsp_q7s/gpio/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${TARGET_NAME} PRIVATE + gpioCallbacks.cpp +) diff --git a/bsp_q7s/gpio/gpioCallbacks.cpp b/bsp_q7s/gpio/gpioCallbacks.cpp new file mode 100644 index 00000000..086e6469 --- /dev/null +++ b/bsp_q7s/gpio/gpioCallbacks.cpp @@ -0,0 +1,221 @@ +#include "gpioCallbacks.h" +#include + +#include +#include +#include + + +namespace gpioCallbacks { + +GpioIF* gpioComInterface; + +void initTcsBoardDecoder(GpioIF* gpioComIF) { + + ReturnValue_t result; + + if (gpioComIF == nullptr) { + sif::debug << "initTcsBoardDecoder: Invalid gpioComIF" << std::endl; + return; + } + + gpioComInterface = gpioComIF; + + GpioCookie* spiMuxGpios = new GpioCookie; + /** + * Initial values of the spi mux gpios can all be set to an arbitrary value expect for spi mux + * bit 1. Setting spi mux bit 1 to high will pull all decoder outputs to high voltage level. + */ + GpiodRegular* spiMuxBit1 = new GpiodRegular(std::string("gpiochip7"), 13, + std::string("SPI Mux Bit 1"), gpio::OUT, 1); + spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_1, spiMuxBit1); + GpiodRegular* spiMuxBit2 = new GpiodRegular(std::string("gpiochip7"), 14, + std::string("SPI Mux Bit 2"), gpio::OUT, 0); + spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_2, spiMuxBit2); + GpiodRegular* spiMuxBit3 = new GpiodRegular(std::string("gpiochip7"), 15, + std::string("SPI Mux Bit 3"), gpio::OUT, 0); + spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_3, spiMuxBit3); + GpiodRegular* spiMuxBit4 = new GpiodRegular(std::string("gpiochip7"), 16, + std::string("SPI Mux Bit 4"), gpio::OUT, 0); + spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_4, spiMuxBit4); + GpiodRegular* spiMuxBit5 = new GpiodRegular(std::string("gpiochip7"), 17, + std::string("SPI Mux Bit 5"), gpio::OUT, 0); + spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_5, spiMuxBit5); + GpiodRegular* spiMuxBit6 = new GpiodRegular(std::string("gpiochip7"), 18, + std::string("SPI Mux Bit 6"), gpio::OUT, 0); + spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_6, spiMuxBit6); + + result = gpioComInterface->addGpios(spiMuxGpios); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "initTcsBoardDecoder: Failed to add mux bit gpios to gpioComIF" + << std::endl; + return; + } +} + +void tcsBoardDecoderCallback(gpioId_t gpioId, gpio::GpioOperation gpioOp, int value, + void* args) { + + if (gpioComInterface == nullptr) { + sif::debug << "tcsBoardDecoderCallback: No gpioComIF specified. Call initTcsBoardDecoder " + << "to specify gpioComIF" << std::endl; + return; + } + + /* Read is not supported by the callback function */ + if (gpioOp == gpio::GpioOperation::READ) { + return; + } + + if (value == 1) { + /* This will pull all 16 decoder outputs to high */ + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_1); + } + else if (value == 0) { + switch (gpioId) { + case(gpioIds::RTD_IC3): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC4): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC5): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC6): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC7): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC8): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC9): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC10): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_2); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC11): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC12): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC13): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC14): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC15): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC16): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC17): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + case(gpioIds::RTD_IC18): { + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1); + gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5); + gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_6); + break; + } + default: + sif::debug << "tcsBoardDecoderCallback: Invalid gpioid " << gpioId << std::endl; + } + } + else { + sif::debug << "tcsBoardDecoderCallback: Invalid value. Must be 0 or 1" << std::endl; + } +} + +} diff --git a/bsp_q7s/gpio/gpioCallbacks.h b/bsp_q7s/gpio/gpioCallbacks.h new file mode 100644 index 00000000..845127fb --- /dev/null +++ b/bsp_q7s/gpio/gpioCallbacks.h @@ -0,0 +1,23 @@ +#ifndef LINUX_GPIO_GPIOCALLBACKS_H_ +#define LINUX_GPIO_GPIOCALLBACKS_H_ + +#include +#include + + +namespace gpioCallbacks { + + /** + * @brief This function initializes the GPIOs used to control the SN74LVC138APWR decoders on + * the TCS Board. + */ + void initTcsBoardDecoder(GpioIF* gpioComIF); + + /** + * @brief This function implements the decoding to multiply gpios by using the two decoder + * chips SN74LVC138APWR on the TCS board. + */ + void tcsBoardDecoderCallback(gpioId_t gpioId, gpio::GpioOperation gpioOp, int value, void* args); +} + +#endif /* LINUX_GPIO_GPIOCALLBACKS_H_ */ diff --git a/bsp_rpi/InitMission.cpp b/bsp_rpi/InitMission.cpp index 18e4f01a..40c38e6c 100644 --- a/bsp_rpi/InitMission.cpp +++ b/bsp_rpi/InitMission.cpp @@ -3,8 +3,9 @@ #include #include -#include +#include +#include #include #include #include @@ -12,7 +13,6 @@ #include #include #include -#include #include @@ -127,7 +127,7 @@ void initmission::initTasks() { #if RPI_TEST_ACS_BOARD == 1 FixedTimeslotTaskIF* acsTask = factory->createFixedTimeslotTask( - "ACS_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc); + "ACS_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 2.0, missedDeadlineFunc); result = pst::pollingSequenceAcsTest(acsTask); if(result != HasReturnvaluesIF::RETURN_OK) { sif::warning << "initmission::initTasks: ACS PST initialization failed!" << std::endl; diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp index 13cadfb3..960abfe6 100644 --- a/bsp_rpi/ObjectFactory.cpp +++ b/bsp_rpi/ObjectFactory.cpp @@ -1,5 +1,4 @@ #include "ObjectFactory.h" -#include #include #include @@ -11,11 +10,8 @@ #include #include -#include -#include -#include -#include +#include #include #include #include @@ -24,12 +20,18 @@ #include #include #include -#include #include -#include #include -#include +/* UDP server includes */ +#include +#include + +#include +#include +#include +#include +#include void Factory::setStaticFrameworkObjectIds() { PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR; @@ -54,10 +56,10 @@ void ObjectFactory::produce(){ Factory::setStaticFrameworkObjectIds(); ObjectFactory::produceGenericObjects(); - new TmTcUnixUdpBridge(objects::UDP_BRIDGE, + new UdpTmTcBridge(objects::UDP_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR, objects::TM_STORE, objects::TC_STORE); - new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); + new UdpTcPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); GpioIF* gpioIF = new LinuxLibgpioIF(objects::GPIO_IF); diff --git a/bsp_rpi/gpio/CMakeLists.txt b/bsp_rpi/gpio/CMakeLists.txt index 85bd6aa6..b816684d 100644 --- a/bsp_rpi/gpio/CMakeLists.txt +++ b/bsp_rpi/gpio/CMakeLists.txt @@ -1,5 +1,4 @@ target_sources(${TARGET_NAME} PUBLIC - GPIORPi.cpp ) diff --git a/cmake/scripts/Q7S/unix_path_helper.sh b/cmake/scripts/Q7S/unix_path_helper.sh new file mode 100644 index 00000000..296bfdd8 --- /dev/null +++ b/cmake/scripts/Q7S/unix_path_helper.sh @@ -0,0 +1,6 @@ +#!/bin/sh +export PATH=$PATH:"/opt/Xilinx/SDK/2018.2/gnu/aarch32/lin/gcc-arm-linux-gnueabi/bin" +export CROSS_COMPILE="arm-linux-gnueabihf" + +export Q7S_SYSROOT="$HOME/Xilinx/cortexa9hf-neon-xiphos-linux-gnueabi" + diff --git a/cmake/scripts/Q7S/win_path_helper_xilinx_tools.sh b/cmake/scripts/Q7S/win_path_helper_xilinx_tools.sh index c9bcc54a..a8352331 100644 --- a/cmake/scripts/Q7S/win_path_helper_xilinx_tools.sh +++ b/cmake/scripts/Q7S/win_path_helper_xilinx_tools.sh @@ -2,4 +2,4 @@ export PATH=$PATH:"/c/Xilinx/SDK/2018.2/gnu/aarch32/nt/gcc-arm-linux-gnueabi/bin" export CROSS_COMPILE="arm-linux-gnueabihf" -export Q7S_SYSROOT="/c/Xilinx/SDK/2018.2/gnu/aarch32/nt/gcc-arm-linux-gnueabi/arm-linux-gnueabihf/libc" +export Q7S_SYSROOT="/c/Users/${USER}/Documents/EIVE/cortexa9hf-neon-xiphos-linux-gnueabi" diff --git a/cmake/scripts/cmake_build_config.py b/cmake/scripts/cmake_build_config.py index 0e4b6f25..32b560f6 100644 --- a/cmake/scripts/cmake_build_config.py +++ b/cmake/scripts/cmake_build_config.py @@ -26,6 +26,9 @@ def main(): "Information)", default="debug") parser.add_argument("-l", "--builddir", type=str, help="Specify build directory.") parser.add_argument("-g", "--generator", type=str, help="CMake Generator") + parser.add_argument("-d", "--defines", + help="Additional custom defines passed to CMake (supply without -D prefix!)", + nargs="*", type=str) parser.add_argument("-t", "--target-bsp", type=str, help="Target BSP, combination of architecture and machine") args = parser.parse_args() diff --git a/common/config/OBSWVersion.h b/common/config/OBSWVersion.h new file mode 100644 index 00000000..548363f2 --- /dev/null +++ b/common/config/OBSWVersion.h @@ -0,0 +1,10 @@ +#ifndef COMMON_CONFIG_OBSWVERSION_H_ +#define COMMON_CONFIG_OBSWVERSION_H_ + +const char* const SW_NAME = "eive"; + +#define SW_VERSION 1 +#define SW_SUBVERSION 1 +#define SW_SUBSUBVERSION 0 + +#endif /* COMMON_CONFIG_OBSWVERSION_H_ */ diff --git a/fsfw b/fsfw index 83d0db82..5273ffa7 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 83d0db824289b28dbad81cce0c80276c4fc839c8 +Subproject commit 5273ffa721dd83634ebbcc37c4646752f0bea20f diff --git a/fsfw_hal b/fsfw_hal new file mode 160000 index 00000000..14fe3257 --- /dev/null +++ b/fsfw_hal @@ -0,0 +1 @@ +Subproject commit 14fe32572d62db9d19707dc1f9bb6fecb1993b73 diff --git a/fsfwconfig/FSFWConfig.h b/fsfwconfig/FSFWConfig.h index e3def8fe..097f8aaf 100644 --- a/fsfwconfig/FSFWConfig.h +++ b/fsfwconfig/FSFWConfig.h @@ -17,8 +17,7 @@ #define FSFW_DISABLE_PRINTOUT 0 #endif -//! Can be used to disable the ANSI color sequences for C stdio. -#define FSFW_COLORED_OUTPUT 1 +#define FSFW_USE_PUS_C_TELEMETRY 1 //! Can be used to disable the ANSI color sequences for C stdio. #define FSFW_COLORED_OUTPUT 1 @@ -43,10 +42,17 @@ //! Specify whether a special mode store is used for Subsystem components. #define FSFW_USE_MODESTORE 0 +//! Defines if the real time scheduler for linux should be used. +//! If set to 0, this will also disable priority settings for linux +//! as most systems will not allow to set nice values without privileges +//! For embedded linux system set this to 1. +//! If set to 1 the binary needs "cap_sys_nice=eip" privileges to run +#define FSFW_USE_REALTIME_FOR_LINUX 1 + namespace fsfwconfig { //! Default timestamp size. The default timestamp will be an eight byte CDC //! short timestamp. -static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 8; +static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 7; //! Configure the allocated pool sizes for the event manager. static constexpr size_t FSFW_EVENTMGMR_MATCHTREE_NODES = 240; diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index b8fecaac..f3f7cda3 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -8,28 +8,32 @@ #ifdef RASPBERRY_PI #include +#elif defined(XIPHOS_Q7S) +#include #endif #include "commonConfig.h" #include "OBSWVersion.h" /* These defines should be disabled for mission code but are useful for debugging. */ -#define OBSW_VERBOSE_LEVEL 1 -#define OBSW_PRINT_MISSED_DEADLINES 1 -#define OBSW_ADD_TEST_CODE 1 -#define TEST_LIBGPIOD 0 +#define OBSW_VERBOSE_LEVEL 1 +#define OBSW_PRINT_MISSED_DEADLINES 1 +#define OBSW_ADD_TEST_CODE 1 +#define TEST_LIBGPIOD 0 -#define TE0720 0 +#define TE0720 0 -#define P60DOCK_DEBUG 0 -#define PDU1_DEBUG 0 -#define PDU2_DEBUG 0 -#define ACU_DEBUG 1 +#define P60DOCK_DEBUG 0 +#define PDU1_DEBUG 0 +#define PDU2_DEBUG 0 +#define ACU_DEBUG 0 +#define SYRLINKS_DEBUG 0 +#define IMQT_DEBUG 1 + +#include "OBSWVersion.h" /* Can be used to switch device to NORMAL mode immediately */ #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 1 -/* Can be used for low-level debugging of the SPI bus */ -#define FSFW_LINUX_SPI_WIRETAPPING 0 #ifdef __cplusplus diff --git a/fsfwconfig/OBSWVersion.h b/fsfwconfig/OBSWVersion.h deleted file mode 100644 index fd76332e..00000000 --- a/fsfwconfig/OBSWVersion.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef FSFWCONFIG_OBSWVERSION_H_ -#define FSFWCONFIG_OBSWVERSION_H_ - -//! TODO: Think of a cool name for the software releases. -const char* const SW_NAME = "eive"; - -#define SW_VERSION 1 -#define SW_SUBVERSION 0 -#define SW_SUBSUBVERSION 0 - - - -#endif /* FSFWCONFIG_OBSWVERSION_H_ */ diff --git a/fsfwconfig/devices/addresses.h b/fsfwconfig/devices/addresses.h index cd0b9d29..4974834d 100644 --- a/fsfwconfig/devices/addresses.h +++ b/fsfwconfig/devices/addresses.h @@ -26,10 +26,30 @@ namespace addresses { }; enum i2cAddresses: address_t { + IMTQ = 16, TMP1075_TCS_1 = 72, TMP1075_TCS_2 = 73, }; + enum spiAddresses: address_t { + RTD_IC3, + RTD_IC4, + RTD_IC5, + RTD_IC6, + RTD_IC7, + RTD_IC8, + RTD_IC9, + RTD_IC10, + RTD_IC11, + RTD_IC12, + RTD_IC13, + RTD_IC14, + RTD_IC15, + RTD_IC16, + RTD_IC17, + RTD_IC18 + }; + /* Addresses of devices supporting the CSP protocol */ enum cspAddresses: uint8_t { P60DOCK = 4, diff --git a/fsfwconfig/devices/gpioIds.h b/fsfwconfig/devices/gpioIds.h index 8963bd20..ac999152 100644 --- a/fsfwconfig/devices/gpioIds.h +++ b/fsfwconfig/devices/gpioIds.h @@ -1,7 +1,7 @@ #ifndef FSFWCONFIG_DEVICES_GPIOIDS_H_ #define FSFWCONFIG_DEVICES_GPIOIDS_H_ -#include +#include namespace gpioIds { enum gpioId_t { @@ -25,7 +25,31 @@ namespace gpioIds { MGM_3_RM3100_CS, TEST_ID_0, - TEST_ID_1 + TEST_ID_1, + + RTD_IC3, + RTD_IC4, + RTD_IC5, + RTD_IC6, + RTD_IC7, + RTD_IC8, + RTD_IC9, + RTD_IC10, + RTD_IC11, + RTD_IC12, + RTD_IC13, + RTD_IC14, + RTD_IC15, + RTD_IC16, + RTD_IC17, + RTD_IC18, + + SPI_MUX_BIT_1, + SPI_MUX_BIT_2, + SPI_MUX_BIT_3, + SPI_MUX_BIT_4, + SPI_MUX_BIT_5, + SPI_MUX_BIT_6 }; } diff --git a/fsfwconfig/devices/spi.h b/fsfwconfig/devices/spi.h index 7085aa4d..55271a40 100644 --- a/fsfwconfig/devices/spi.h +++ b/fsfwconfig/devices/spi.h @@ -2,7 +2,7 @@ #define FSFWCONFIG_DEVICES_SPI_H_ #include -#include +#include /** * SPI configuration will be contained here to let the device handlers remain independent diff --git a/fsfwconfig/objects/systemObjectList.h b/fsfwconfig/objects/systemObjectList.h index 459cbd77..110b5c17 100644 --- a/fsfwconfig/objects/systemObjectList.h +++ b/fsfwconfig/objects/systemObjectList.h @@ -30,7 +30,8 @@ namespace objects { ARDUINO_COM_IF = 0x49000001, CSP_COM_IF = 0x49000002, I2C_COM_IF = 0x49000003, - SPI_COM_IF = 0x49000004, + UART_COM_IF = 0x49000004, + SPI_COM_IF = 0x49000005, /* 0x47 ('G') for Gpio Interfaces */ GPIO_IF = 0x47000001, @@ -42,7 +43,7 @@ namespace objects { ACU_HANDLER = 0x44000004, TMP1075_HANDLER_1 = 0x44000005, TMP1075_HANDLER_2 = 0x44000006, - MGM_0_LIS3_HANDLER = 0x4400007, + MGM_0_LIS3_HANDLER = 0x44000007, MGM_1_RM3100_HANDLER = 0x44000008, MGM_2_LIS3_HANDLER = 0x44000009, MGM_3_RM3100_HANDLER = 0x44000010, @@ -50,12 +51,35 @@ namespace objects { GYRO_1_L3G_HANDLER = 0x44000012, GYRO_2_L3G_HANDLER = 0x44000013, + IMTQ_HANDLER = 0x44000014, + /* Custom device handler */ PCDU_HANDLER = 0x44001000, SOLAR_ARRAY_DEPL_HANDLER = 0x44001001, + SYRLINKS_HK_HANDLER = 0x44001002, /* 0x54 ('T') for thermal objects */ HEATER_HANDLER = 0x54000003, + /** + * Not yet specified which pt1000 will measure which device/location in the satellite. + * Therefore object ids are named according to the IC naming of the RTDs in the schematic. + */ + RTD_IC3 = 0x54000004, + RTD_IC4 = 0x54000005, + RTD_IC5 = 0x54000006, + RTD_IC6 = 0x54000007, + RTD_IC7 = 0x54000008, + RTD_IC8 = 0x54000009, + RTD_IC9 = 0x5400000A, + RTD_IC10 = 0x5400000B, + RTD_IC11 = 0x5400000C, + RTD_IC12 = 0x5400000D, + RTD_IC13 = 0x5400000E, + RTD_IC14 = 0x5400000F, + RTD_IC15 = 0x5400001F, + RTD_IC16 = 0x5400002F, + RTD_IC17 = 0x5400003F, + RTD_IC18 = 0x5400004F, /* 0x54 ('T') for test handlers */ TEST_TASK = 0x54694269, diff --git a/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 04b1367b..02e6b625 100644 --- a/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -1,5 +1,5 @@ #include "pollingSequenceFactory.h" - +#include #include #include #include @@ -20,18 +20,122 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) thisSequence->addSlot(objects::SOLAR_ARRAY_DEPL_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); +#if Q7S_ADD_RTD_DEVICES == 1 + thisSequence->addSlot(objects::RTD_IC3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC5, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC6, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC7, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC8, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC9, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC10, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC11, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC12, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC13, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC14, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC15, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC16, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC17, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RTD_IC18, length * 0, DeviceHandlerIF::PERFORM_OPERATION); +#endif /* Q7S_ADD_RTD_DEVICES */ + + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.2, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.2, DeviceHandlerIF::SEND_WRITE); +#if Q7S_ADD_RTD_DEVICES == 1 + thisSequence->addSlot(objects::RTD_IC3, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC4, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC5, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC6, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC7, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC8, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC9, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC10, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC11, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC12, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC13, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC14, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC15, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC16, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC17, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RTD_IC18, length * 0.2, DeviceHandlerIF::SEND_WRITE); +#endif /* Q7S_ADD_RTD_DEVICES */ + + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.4, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.4, DeviceHandlerIF::GET_WRITE); +#if Q7S_ADD_RTD_DEVICES == 1 + thisSequence->addSlot(objects::RTD_IC3, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC4, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC5, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC6, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC7, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC8, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC9, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC10, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC11, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC12, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC13, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC14, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC15, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC16, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC17, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RTD_IC18, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); +#endif /* Q7S_ADD_RTD_DEVICES */ + thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.6, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.6, DeviceHandlerIF::SEND_READ); +#if Q7S_ADD_RTD_DEVICES == 1 + thisSequence->addSlot(objects::RTD_IC3, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC4, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC5, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC6, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC7, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC8, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC9, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC10, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC11, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC12, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC13, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC14, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC15, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC16, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC17, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RTD_IC18, length * 0.6, DeviceHandlerIF::SEND_READ); +#endif /* Q7S_ADD_RTD_DEVICES */ + + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.8, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.8, DeviceHandlerIF::GET_READ); +#if Q7S_ADD_RTD_DEVICES == 1 + thisSequence->addSlot(objects::RTD_IC3, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC4, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC5, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC6, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC7, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC8, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC9, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC10, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC11, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC12, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC13, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC14, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC15, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC16, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC17, length * 0.8, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RTD_IC18, length * 0.8, DeviceHandlerIF::GET_READ); +#endif /* Q7S_ADD_RTD_DEVICES */ + + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); + if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) { return HasReturnvaluesIF::RETURN_OK; } @@ -91,6 +195,52 @@ ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){ thisSequence->addSlot(objects::ACU_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); +#if OBSW_ADD_ACS_BOARD == 1 +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, +// DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2, +// DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4, +// DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, +// DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8, +// DeviceHandlerIF::GET_READ); +// +// thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0, +// DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.2, +// DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.4, +// DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6, +// DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.8, +// DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.6, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.8, + DeviceHandlerIF::GET_READ); + +// thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0, +// DeviceHandlerIF::PERFORM_OPERATION); +// thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.2, +// DeviceHandlerIF::SEND_WRITE); +// thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.4, +// DeviceHandlerIF::GET_WRITE); +// thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6, +// DeviceHandlerIF::SEND_READ); +// thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.8, +// DeviceHandlerIF::GET_READ); +#endif + if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { sif::error << "Initialization of GomSpace PST failed" << std::endl; return HasReturnvaluesIF::RETURN_FAILED; diff --git a/fsfwconfig/returnvalues/classIds.h b/fsfwconfig/returnvalues/classIds.h index deafbd75..93349596 100644 --- a/fsfwconfig/returnvalues/classIds.h +++ b/fsfwconfig/returnvalues/classIds.h @@ -17,7 +17,9 @@ enum { LINUX_SPI_COM_IF, PCDU_HANDLER, HEATER_HANDLER, - SA_DEPL_HANDLER + SA_DEPL_HANDLER, + SYRLINKS_HANDLER, + IMTQ_HANDLER, }; } diff --git a/generators/modgen b/generators/modgen new file mode 160000 index 00000000..5ed4a2ae --- /dev/null +++ b/generators/modgen @@ -0,0 +1 @@ +Subproject commit 5ed4a2ae59e90c3763616bdaf41eb3317e849100 diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index 77bea0ef..7e6d1f35 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -1,7 +1,4 @@ -add_subdirectory(gpio) -add_subdirectory(i2c) add_subdirectory(csp) -add_subdirectory(spi) +add_subdirectory(uart) add_subdirectory(utility) add_subdirectory(boardtest) - diff --git a/linux/gpio/CMakeLists.txt b/linux/archive/gpio/CMakeLists.txt similarity index 100% rename from linux/gpio/CMakeLists.txt rename to linux/archive/gpio/CMakeLists.txt diff --git a/linux/gpio/GpioCookie.cpp b/linux/archive/gpio/GpioCookie.cpp similarity index 66% rename from linux/gpio/GpioCookie.cpp rename to linux/archive/gpio/GpioCookie.cpp index f957d070..c729502b 100644 --- a/linux/gpio/GpioCookie.cpp +++ b/linux/archive/gpio/GpioCookie.cpp @@ -4,14 +4,18 @@ GpioCookie::GpioCookie() { } -ReturnValue_t GpioCookie::addGpio(gpioId_t gpioId, GpiodRegular& gpioConfig){ +ReturnValue_t GpioCookie::addGpio(gpioId_t gpioId, GpioBase* gpioConfig){ + if (gpioConfig == nullptr) { + sif::debug << "GpioCookie::addGpio: gpioConfig is nullpointer" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } auto gpioMapIter = gpioMap.find(gpioId); if(gpioMapIter == gpioMap.end()) { - auto statusPair = gpioMap.emplace(gpioId, new GpiodRegular(gpioConfig)); + auto statusPair = gpioMap.emplace(gpioId, gpioConfig); if (statusPair.second == false) { #if FSFW_VERBOSE_LEVEL >= 1 sif::error << "GpioCookie::addGpio: Failed to add GPIO " << gpioId << - "to GPIO map" << std::endl; + " to GPIO map" << std::endl; #endif return HasReturnvaluesIF::RETURN_FAILED; } diff --git a/linux/gpio/GpioCookie.h b/linux/archive/gpio/GpioCookie.h similarity index 92% rename from linux/gpio/GpioCookie.h rename to linux/archive/gpio/GpioCookie.h index c7721a98..3f8531ee 100644 --- a/linux/gpio/GpioCookie.h +++ b/linux/archive/gpio/GpioCookie.h @@ -23,7 +23,7 @@ public: virtual ~GpioCookie(); - ReturnValue_t addGpio(gpioId_t gpioId, GpiodRegular& gpioConfig); + ReturnValue_t addGpio(gpioId_t gpioId, GpioBase* gpioConfig); /** * @brief Get map with registered GPIOs. */ diff --git a/linux/gpio/GpioIF.h b/linux/archive/gpio/GpioIF.h similarity index 100% rename from linux/gpio/GpioIF.h rename to linux/archive/gpio/GpioIF.h diff --git a/linux/gpio/LinuxLibgpioIF.cpp b/linux/archive/gpio/LinuxLibgpioIF.cpp similarity index 98% rename from linux/gpio/LinuxLibgpioIF.cpp rename to linux/archive/gpio/LinuxLibgpioIF.cpp index 7f6be6c5..64ee2982 100644 --- a/linux/gpio/LinuxLibgpioIF.cpp +++ b/linux/archive/gpio/LinuxLibgpioIF.cpp @@ -56,11 +56,11 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { break; } case(gpio::GpioTypes::CALLBACK): { - auto gpioCallback = dynamic_cast(gpioMapIter->second); + auto gpioCallback = dynamic_cast(gpioConfig.second); if(gpioCallback->callback == nullptr) { return GPIO_INVALID_INSTANCE; } - gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::READ, + gpioCallback->callback(gpioConfig.first, gpio::GpioOperation::WRITE, gpioCallback->initValue, gpioCallback->callbackArgs); } } @@ -88,7 +88,8 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, GpiodRegular lineNum = regularGpio->lineNum; lineHandle = gpiod_chip_get_line(chip, lineNum); if (!lineHandle) { - sif::error << "LinuxLibgpioIF::configureGpios: Failed to open line" << std::endl; + sif::error << "LinuxLibgpioIF::configureGpios: Failed to open line for GPIO with id " + << gpioId << std::endl; gpiod_chip_close(chip); return RETURN_FAILED; } diff --git a/linux/gpio/LinuxLibgpioIF.h b/linux/archive/gpio/LinuxLibgpioIF.h similarity index 100% rename from linux/gpio/LinuxLibgpioIF.h rename to linux/archive/gpio/LinuxLibgpioIF.h diff --git a/linux/gpio/gpioDefinitions.h b/linux/archive/gpio/gpioDefinitions.h similarity index 98% rename from linux/gpio/gpioDefinitions.h rename to linux/archive/gpio/gpioDefinitions.h index 3b0f70fd..66c0b005 100644 --- a/linux/gpio/gpioDefinitions.h +++ b/linux/archive/gpio/gpioDefinitions.h @@ -7,6 +7,12 @@ using gpioId_t = uint16_t; namespace gpio { + +enum Levels { + LOW = 0, + HIGH = 1 +}; + enum Direction { IN = 0, OUT = 1 diff --git a/linux/boardtest/LibgpiodTest.h b/linux/boardtest/LibgpiodTest.h index e9c6c030..a18c618e 100644 --- a/linux/boardtest/LibgpiodTest.h +++ b/linux/boardtest/LibgpiodTest.h @@ -2,8 +2,8 @@ #define TEST_TESTTASKS_LIBGPIODTEST_H_ #include "TestTask.h" -#include -#include +#include +#include #include /** diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index 3090c115..387c7eb5 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -7,19 +7,21 @@ #include #include -#include -#include -#include +#include +#include +#include +#include #include #include + #include #include #include SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objectId), - gpioIF(gpioIF) { +gpioIF(gpioIF) { if(gpioIF == nullptr) { sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl; } @@ -34,11 +36,11 @@ ReturnValue_t SpiTestClass::performOneShotAction() { break; } case(TestModes::MGM_LIS3MDL): { - performLis3MdlTest(mgm0Lis3ChipSelect); + performLis3MdlTest(mgm2Lis3mdlChipSelect); break; } case(TestModes::MGM_RM3100): { - performRm3100Test(mgm1Rm3100ChipSelect); + performRm3100Test(mgm3Rm3100ChipSelect); break; } case(TestModes::GYRO_L3GD20H): { @@ -76,12 +78,12 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else - std::string deviceName = "placeholder"; + std::string deviceName = "/dev/spidev2.0"; #endif int fileDescriptor = 0; - utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR, + UnixFileGuard fileHelper(deviceName, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface: "); if(fileHelper.getOpenResult()) { sif::error << "SpiTestClass::performRm3100Test: File descriptor could not be opened!" @@ -158,13 +160,14 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { acsInit(); /* Adapt accordingly */ - if(lis3Id != mgm0Lis3ChipSelect and lis3Id != mgm2Lis3mdlChipSelect) { + if(lis3Id != mgm0Lis3mdlChipSelect and lis3Id != mgm2Lis3mdlChipSelect) { sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl; } gpioId_t currentGpioId = 0; uint8_t chipSelectPin = lis3Id; uint8_t whoAmIReg = 0b0000'1111; - if(chipSelectPin == mgm0Lis3ChipSelect) { + uint8_t whoAmIRegExpectedVal = 0b0011'1101; + if(chipSelectPin == mgm0Lis3mdlChipSelect) { currentGpioId = gpioIds::MGM_0_LIS3_CS; } else { @@ -175,11 +178,11 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else - std::string deviceName = "placeholder"; + std::string deviceName = "/dev/spidev2.0"; #endif int fileDescriptor = 0; - utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR, + UnixFileGuard fileHelper(deviceName, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface: "); if(fileHelper.getOpenResult()) { sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!" @@ -187,10 +190,15 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { return; } setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + spiTransferStruct.delay_usecs = 0; uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false); sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I register 0b" << std::bitset<8>(whoAmIRegVal) << std::endl; + if(whoAmIRegVal != whoAmIRegExpectedVal) { + sif::warning << "SpiTestClass::performLis3MdlTest: WHO AM I register invalid!" + << std::endl; + } } @@ -221,11 +229,11 @@ void SpiTestClass::performL3gTest(uint8_t l3gId) { #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else - std::string deviceName = "placeholder"; + std::string deviceName = "/dev/spidev2.0"; #endif int fileDescriptor = 0; - utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR, + UnixFileGuard fileHelper(deviceName, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface: "); if(fileHelper.getOpenResult()) { sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!" @@ -257,8 +265,8 @@ void SpiTestClass::performL3gTest(uint8_t l3gId) { sizeof(readRegs)); for(uint8_t idx = 0; idx < sizeof(readRegs); idx++) { if(readRegs[idx] != commandRegs[0]) { - sif::warning << "SpiTestClass::performL3gTest: Read control register" << - static_cast(idx + 1) << "not equal to configured value" << std::endl; + sif::warning << "SpiTestClass::performL3gTest: Read control register " << + static_cast(idx + 1) << " not equal to configured value" << std::endl; } } } @@ -289,46 +297,70 @@ void SpiTestClass::performL3gTest(uint8_t l3gId) { } - - void SpiTestClass::acsInit() { GpioCookie* gpioCookie = new GpioCookie(); + GpiodRegular* gpio = nullptr; +#ifdef RASPBERRY_PI std::string rpiGpioName = "gpiochip0"; - { - GpiodRegular gpio(rpiGpioName, mgm0Lis3ChipSelect, "MGM_0_LIS3", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio); - } - { - GpiodRegular gpio(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio); - } - { - GpiodRegular gpio(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio); - } - { - GpiodRegular gpio(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio); - } - { - GpiodRegular gpio(rpiGpioName, gyro2L3gd20ChipSelect, "GYRO_2_L3G", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio); - } - { - GpiodRegular gpio(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio); - } - { - GpiodRegular gpio(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100", - gpio::Direction::OUT, 1); - gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio); - } + gpio = new GpiodRegular(rpiGpioName, mgm0Lis3mdlChipSelect, "MGM_0_LIS3", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio); + + gpio = new GpiodRegular(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio); + + gpio = new GpiodRegular(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio); + + gpio = new GpiodRegular(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio); + + gpio = new GpiodRegular(rpiGpioName, gyro2L3gd20ChipSelect, "GYRO_2_L3G", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio); + + gpio = new GpiodRegular(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio); + + gpio = new GpiodRegular(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100", + gpio::Direction::OUT, 1); + gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio); +#elif defined(XIPHOS_Q7S) + std::string q7sGpioName5 = "gpiochip5"; + std::string q7sGpioName6 = "gpiochip6"; + + gpio = new GpiodRegular(q7sGpioName5, mgm0Lis3mdlChipSelect, "MGM_0_LIS3", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio); + + gpio = new GpiodRegular(q7sGpioName5, mgm1Rm3100ChipSelect, "MGM_1_RM3100", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio); + + gpio = new GpiodRegular(q7sGpioName5, gyro0AdisChipSelect, "GYRO_0_ADIS", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio); + + gpio = new GpiodRegular(q7sGpioName5, gyro1L3gd20ChipSelect, "GYRO_1_L3G", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio); + + gpio = new GpiodRegular(q7sGpioName5, gyro2L3gd20ChipSelect, "GYRO_2_L3G", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio); + + gpio = new GpiodRegular(q7sGpioName6, mgm2Lis3mdlChipSelect, "MGM_2_LIS3", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio); + + gpio = new GpiodRegular(q7sGpioName5, mgm3Rm3100ChipSelect, "MGM_3_RM3100", + gpio::Direction::OUT, gpio::HIGH); + gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio); +#endif if(gpioIF != nullptr) { gpioIF->addGpios(gpioCookie); } @@ -471,4 +503,3 @@ uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) { } return recvBuffer[1]; } - diff --git a/linux/boardtest/SpiTestClass.h b/linux/boardtest/SpiTestClass.h index c567bc45..f93eb9c2 100644 --- a/linux/boardtest/SpiTestClass.h +++ b/linux/boardtest/SpiTestClass.h @@ -1,8 +1,8 @@ #ifndef LINUX_BOARDTEST_SPITESTCLASS_H_ #define LINUX_BOARDTEST_SPITESTCLASS_H_ -#include -#include +#include +#include #include #include @@ -38,13 +38,24 @@ private: void acsInit(); /* ACS board specific variables */ - uint8_t mgm0Lis3ChipSelect = 0; +#ifdef RASPBERRY_PI + uint8_t mgm0Lis3mdlChipSelect = 0; uint8_t mgm1Rm3100ChipSelect = 1; uint8_t gyro0AdisChipSelect = 5; uint8_t gyro1L3gd20ChipSelect = 6; uint8_t gyro2L3gd20ChipSelect = 4; uint8_t mgm2Lis3mdlChipSelect = 17; uint8_t mgm3Rm3100ChipSelect = 27; +#elif defined(XIPHOS_Q7S) + uint8_t mgm0Lis3mdlChipSelect = 5; + uint8_t mgm1Rm3100ChipSelect = 17; + uint8_t gyro0AdisResetLine = 20; + uint8_t gyro0AdisChipSelect = 21; + uint8_t gyro1L3gd20ChipSelect = 10; + uint8_t gyro2L3gd20ChipSelect = 3; + uint8_t mgm2Lis3mdlChipSelect = 0; + uint8_t mgm3Rm3100ChipSelect = 23; +#endif static constexpr uint8_t STM_READ_MASK = 0b1000'0000; static constexpr uint8_t RM3100_READ_MASK = STM_READ_MASK; diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index bfe7e7e9..1798b152 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -28,7 +28,7 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF *cookie) { int buf_count = 10; int buf_size = 300; /* Init CSP and CSP buffer system */ - if (csp_init(cspClientAddress) != CSP_ERR_NONE + if (csp_init(cspOwnAddress) != CSP_ERR_NONE || csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) { sif::error << "Failed to init CSP\r\n" << std::endl; return HasReturnvaluesIF::RETURN_FAILED; diff --git a/linux/csp/CspComIF.h b/linux/csp/CspComIF.h index 5120f232..4b8d323b 100644 --- a/linux/csp/CspComIF.h +++ b/linux/csp/CspComIF.h @@ -65,7 +65,7 @@ private: uint16_t replySize = 0; /* This is the CSP address of the OBC. */ - node_t cspClientAddress = 1; + node_t cspOwnAddress = 1; /* Interface struct for csp protocol stack */ csp_iface_t csp_if; diff --git a/linux/i2c/I2cComIF.cpp b/linux/i2c/I2cComIF.cpp deleted file mode 100644 index ed50293d..00000000 --- a/linux/i2c/I2cComIF.cpp +++ /dev/null @@ -1,196 +0,0 @@ -#include "I2cComIF.h" -#include -#include -#include -#include -#include -#include -#include - -#include - - -I2cComIF::I2cComIF(object_id_t objectId): SystemObject(objectId){ -} - -I2cComIF::~I2cComIF() {} - -ReturnValue_t I2cComIF::initializeInterface(CookieIF* cookie) { - - address_t i2cAddress; - std::string deviceFile; - - if(cookie == nullptr) { - sif::error << "I2cComIF::initializeInterface: Invalid cookie!" << std::endl; - return NULLPOINTER; - } - I2cCookie* i2cCookie = dynamic_cast(cookie); - if(i2cCookie == nullptr) { - sif::error << "I2cComIF::initializeInterface: Invalid I2C cookie!" << std::endl; - return NULLPOINTER; - } - - i2cAddress = i2cCookie->getAddress(); - - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); - if(i2cDeviceMapIter == i2cDeviceMap.end()) { - size_t maxReplyLen = i2cCookie->getMaxReplyLen(); - I2cInstance_t i2cInstance = {std::vector(maxReplyLen), 0}; - auto statusPair = i2cDeviceMap.emplace(i2cAddress, i2cInstance); - if (not statusPair.second) { - sif::error << "I2cComIF::initializeInterface: Failed to insert device with address " << - i2cAddress << "to I2C device " << "map" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; - } - - sif::error << "I2cComIF::initializeInterface: Device with address " << i2cAddress << - "already in use" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t I2cComIF::sendMessage(CookieIF *cookie, - const uint8_t *sendData, size_t sendLen) { - - ReturnValue_t result; - int fd; - std::string deviceFile; - - if(sendData == nullptr) { - sif::error << "I2cComIF::sendMessage: Send Data is nullptr" - << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(sendLen == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - I2cCookie* i2cCookie = dynamic_cast(cookie); - if(i2cCookie == nullptr) { - sif::error << "I2cComIF::sendMessage: Invalid I2C Cookie!" << std::endl; - return NULLPOINTER; - } - - address_t i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); - if (i2cDeviceMapIter == i2cDeviceMap.end()) { - sif::error << "I2cComIF::sendMessage: i2cAddress of Cookie not " - << "registered in i2cDeviceMap" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - - deviceFile = i2cCookie->getDeviceFile(); - utility::UnixFileHelper fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::sendMessage"); - if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { - return fileHelper.getOpenResult(); - } - result = openDevice(deviceFile, i2cAddress, &fd); - if (result != HasReturnvaluesIF::RETURN_OK){ - return result; - } - - if (write(fd, sendData, sendLen) != (int)sendLen) { - sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " - "device with error code " << errno << ". Error description: " - << strerror(errno) << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t I2cComIF::getSendSuccess(CookieIF *cookie) { - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF *cookie, - size_t requestLen) { - ReturnValue_t result; - int fd; - std::string deviceFile; - - if (requestLen == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - I2cCookie* i2cCookie = dynamic_cast(cookie); - if(i2cCookie == nullptr) { - sif::error << "I2cComIF::requestReceiveMessage: Invalid I2C Cookie!" << std::endl; - i2cDeviceMapIter->second.replyLen = 0; - return NULLPOINTER; - } - - address_t i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); - if (i2cDeviceMapIter == i2cDeviceMap.end()) { - sif::error << "I2cComIF::requestReceiveMessage: i2cAddress of Cookie not " - << "registered in i2cDeviceMap" << std::endl; - i2cDeviceMapIter->second.replyLen = 0; - return HasReturnvaluesIF::RETURN_FAILED; - } - - deviceFile = i2cCookie->getDeviceFile(); - utility::UnixFileHelper fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::requestReceiveMessage"); - if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { - return fileHelper.getOpenResult(); - } - result = openDevice(deviceFile, i2cAddress, &fd); - if (result != HasReturnvaluesIF::RETURN_OK){ - i2cDeviceMapIter->second.replyLen = 0; - return result; - } - - uint8_t* replyBuffer = i2cDeviceMapIter->second.replyBuffer.data(); - - if (read(fd, replyBuffer, requestLen) != static_cast(requestLen)) { - sif::error << "I2cComIF::requestReceiveMessage: Reading from I2C " - << "device failed with error code " << errno <<". Description" - << " of error: " << strerror(errno) << std::endl; - i2cDeviceMapIter->second.replyLen = 0; - return HasReturnvaluesIF::RETURN_FAILED; - } - - i2cDeviceMapIter->second.replyLen = requestLen; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t I2cComIF::readReceivedMessage(CookieIF *cookie, - uint8_t **buffer, size_t* size) { - I2cCookie* i2cCookie = dynamic_cast(cookie); - if(i2cCookie == nullptr) { - sif::error << "I2cComIF::readReceivedMessage: Invalid I2C Cookie!" << std::endl; - return NULLPOINTER; - } - - address_t i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); - if (i2cDeviceMapIter == i2cDeviceMap.end()) { - sif::error << "I2cComIF::readReceivedMessage: i2cAddress of Cookie not " - << "found in i2cDeviceMap" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - *buffer = i2cDeviceMapIter->second.replyBuffer.data(); - *size = i2cDeviceMapIter->second.replyLen; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t I2cComIF::openDevice(std::string deviceFile, - address_t i2cAddress, int* fileDescriptor) { - - if (ioctl(*fileDescriptor, I2C_SLAVE, i2cAddress) < 0) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "I2cComIF: Specifying target device failed with error code " << errno << "." - << std::endl; - sif::warning << "Error description " << strerror(errno) << std::endl; -#else - sif::printWarning("I2cComIF: Specifying target device failed with error code %d.\n"); - sif::printWarning("Error description: %s\n", strerror(errno)); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; -} diff --git a/linux/i2c/I2cComIF.h b/linux/i2c/I2cComIF.h deleted file mode 100644 index 3529bde7..00000000 --- a/linux/i2c/I2cComIF.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef LINUX_I2C_I2COMIF_H_ -#define LINUX_I2C_I2COMIF_H_ - -#include "I2cCookie.h" -#include -#include - -#include -#include - -/** - * @brief This is the communication interface for i2c devices connected - * to a system running a linux OS. - * - * @author J. Meier - */ -class I2cComIF: public DeviceCommunicationIF, public SystemObject { -public: - I2cComIF(object_id_t objectId); - - virtual ~I2cComIF(); - - ReturnValue_t initializeInterface(CookieIF * cookie) override; - ReturnValue_t sendMessage(CookieIF *cookie,const uint8_t *sendData, - size_t sendLen) override; - ReturnValue_t getSendSuccess(CookieIF *cookie) override; - ReturnValue_t requestReceiveMessage(CookieIF *cookie, - size_t requestLen) override; - ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer, - size_t *size) override; - -private: - - typedef struct I2cInstance { - std::vector replyBuffer; - size_t replyLen; - } I2cInstance_t; - - using I2cDeviceMap = std::unordered_map; - using I2cDeviceMapIter = I2cDeviceMap::iterator; - - /* In this map all i2c devices will be registered with their address and - * the appropriate file descriptor will be stored */ - I2cDeviceMap i2cDeviceMap; - I2cDeviceMapIter i2cDeviceMapIter; - - /** - * @brief This function opens an I2C device and binds the opened file - * to a specific I2C address. - * @param deviceFile The name of the device file. E.g. i2c-0 - * @param i2cAddress The address of the i2c slave device. - * @param fileDescriptor Pointer to device descriptor. - * @return RETURN_OK if successful, otherwise RETURN_FAILED. - */ - ReturnValue_t openDevice(std::string deviceFile, - address_t i2cAddress, int* fileDescriptor); -}; - -#endif /* LINUX_I2C_I2COMIF_H_ */ diff --git a/linux/i2c/I2cCookie.cpp b/linux/i2c/I2cCookie.cpp deleted file mode 100644 index fe0f3f92..00000000 --- a/linux/i2c/I2cCookie.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "I2cCookie.h" - -I2cCookie::I2cCookie(address_t i2cAddress_, size_t maxReplyLen_, - std::string deviceFile_) : - i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(deviceFile_) { -} - -address_t I2cCookie::getAddress() const { - return i2cAddress; -} - -size_t I2cCookie::getMaxReplyLen() const { - return maxReplyLen; -} - -std::string I2cCookie::getDeviceFile() const { - return deviceFile; -} - -I2cCookie::~I2cCookie() {} diff --git a/linux/i2c/I2cCookie.h b/linux/i2c/I2cCookie.h deleted file mode 100644 index c924eb4e..00000000 --- a/linux/i2c/I2cCookie.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef LINUX_I2C_I2CCOOKIE_H_ -#define LINUX_I2C_I2CCOOKIE_H_ - -#include -#include - -/** - * @brief Cookie for the i2cDeviceComIF. - * - * @author J. Meier - */ -class I2cCookie: public CookieIF { -public: - - /** - * @brief Constructor for the I2C cookie. - * @param i2cAddress_ The i2c address of the target device. - * @param maxReplyLen The maximum expected length of a reply from the - * target device. - */ - I2cCookie(address_t i2cAddress_, size_t maxReplyLen_, - std::string deviceFile_); - - virtual ~I2cCookie(); - - address_t getAddress() const; - size_t getMaxReplyLen() const; - std::string getDeviceFile() const; - -private: - - address_t i2cAddress = 0; - size_t maxReplyLen = 0; - std::string deviceFile; -}; - -#endif /* LINUX_I2C_I2CCOOKIE_H_ */ diff --git a/linux/spi/CMakeLists.txt b/linux/spi/CMakeLists.txt deleted file mode 100644 index cb1c9277..00000000 --- a/linux/spi/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -target_sources(${TARGET_NAME} PUBLIC - SpiComIF.cpp - SpiCookie.cpp -) - - - - diff --git a/linux/spi/SpiComIF.cpp b/linux/spi/SpiComIF.cpp deleted file mode 100644 index b94e1249..00000000 --- a/linux/spi/SpiComIF.cpp +++ /dev/null @@ -1,321 +0,0 @@ -#include "SpiComIF.h" -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -SpiComIF::SpiComIF(object_id_t objectId, GpioIF* gpioComIF): SystemObject(objectId), - gpioComIF(gpioComIF) { - if(gpioComIF == nullptr) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::SpiComIF: GPIO communication interface invalid!" << std::endl; -#else - sif::printError("SpiComIF::SpiComIF: GPIO communication interface invalid!\n"); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - } - - spiMutex = MutexFactory::instance()->createMutex(); -} - -ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) { - int retval = 0; - SpiCookie* spiCookie = dynamic_cast(cookie); - if(spiCookie == nullptr) { - return NULLPOINTER; - } - - address_t spiAddress = spiCookie->getSpiAddress(); - - auto iter = spiDeviceMap.find(spiAddress); - if(iter == spiDeviceMap.end()) { - size_t bufferSize = spiCookie->getMaxBufferSize(); - SpiInstance spiInstance = {std::vector(bufferSize)}; - auto statusPair = spiDeviceMap.emplace(spiAddress, spiInstance); - if (not statusPair.second) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::initializeInterface: Failed to insert device with address " << - spiAddress << "to SPI device map" << std::endl; -#else - sif::printError("SpiComIF::initializeInterface: Failed to insert device with address " - "%lu to SPI device map\n", static_cast(spiAddress)); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - return HasReturnvaluesIF::RETURN_FAILED; - } - /* Now we emplaced the read buffer in the map, we still need to assign that location - to the SPI driver transfer struct */ - spiCookie->assignReadBuffer(statusPair.first->second.replyBuffer.data()); - } - else { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::initializeInterface: SPI address already exists!" << std::endl; -#else - sif::printError("SpiComIF::initializeInterface: SPI address already exists!\n"); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - return HasReturnvaluesIF::RETURN_FAILED; - } - - /* Pull CS high in any case to be sure that device is inactive */ - gpioId_t gpioId = spiCookie->getChipSelectPin(); - if(gpioId != gpio::NO_GPIO) { - gpioComIF->pullHigh(gpioId); - } - - size_t spiSpeed = 0; - spi::SpiModes spiMode = spi::SpiModes::MODE_0; - - SpiCookie::UncommonParameters params; - spiCookie->getSpiParameters(spiMode, spiSpeed, ¶ms); - - int fileDescriptor = 0; - utility::UnixFileHelper fileHelper(spiCookie->getSpiDevice(), &fileDescriptor, O_RDWR, - "SpiComIF::initializeInterface: "); - if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { - return fileHelper.getOpenResult(); - } - - /* These flags are rather uncommon */ - if(params.threeWireSpi or params.noCs or params.csHigh) { - uint32_t currentMode = 0; - retval = ioctl(fileDescriptor, SPI_IOC_RD_MODE32, ¤tMode); - if(retval != 0) { - utility::handleIoctlError("SpiComIF::initialiezInterface: Could not read full mode!"); - } - - if(params.threeWireSpi) { - currentMode |= SPI_3WIRE; - } - if(params.noCs) { - /* Some drivers like the Raspberry Pi ignore this flag in any case */ - currentMode |= SPI_NO_CS; - } - if(params.csHigh) { - currentMode |= SPI_CS_HIGH; - } - /* Write adapted mode */ - retval = ioctl(fileDescriptor, SPI_IOC_WR_MODE32, ¤tMode); - if(retval != 0) { - utility::handleIoctlError("SpiComIF::initialiezInterface: Could not write full mode!"); - } - } - if(params.lsbFirst) { - retval = ioctl(fileDescriptor, SPI_IOC_WR_LSB_FIRST, ¶ms.lsbFirst); - if(retval != 0) { - utility::handleIoctlError("SpiComIF::initializeInterface: Setting LSB first failed"); - } - } - if(params.bitsPerWord != 8) { - retval = ioctl(fileDescriptor, SPI_IOC_WR_BITS_PER_WORD, ¶ms.bitsPerWord); - if(retval != 0) { - utility::handleIoctlError("SpiComIF::initializeInterface: " - "Could not write bits per word!"); - } - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, size_t sendLen) { - SpiCookie* spiCookie = dynamic_cast(cookie); - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - int retval = 0; - - if(spiCookie == nullptr) { - return NULLPOINTER; - } - - if(sendLen > spiCookie->getMaxBufferSize()) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "SpiComIF::sendMessage: Too much data sent, send length" << sendLen << - "larger than maximum buffer length" << spiCookie->getMaxBufferSize() << std::endl; -#else - sif::printWarning("SpiComIF::sendMessage: Too much data sent, send length %lu larger " - "than maximum buffer length %lu!\n", static_cast(sendLen), - static_cast(spiCookie->getMaxBufferSize())); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - return DeviceCommunicationIF::TOO_MUCH_DATA; - } - - - /* Prepare transfer */ - int fileDescriptor = 0; - std::string device = spiCookie->getSpiDevice(); - utility::UnixFileHelper fileHelper(device, &fileDescriptor, O_RDWR, - "SpiComIF::sendMessage: "); - if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { - return OPENING_FILE_FAILED; - } - spi::SpiModes spiMode = spi::SpiModes::MODE_0; - uint32_t spiSpeed = 0; - spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr); - setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); - spiCookie->assignWriteBuffer(sendData); - spiCookie->assignTransferSize(sendLen); - - bool fullDuplex = spiCookie->isFullDuplex(); - gpioId_t gpioId = spiCookie->getChipSelectPin(); - - /* GPIO access is mutex protected */ - MutexGuard(spiMutex, timeoutType, timeoutMs); - - /* Pull SPI CS low. For now, no support for active high given */ - if(gpioId != gpio::NO_GPIO) { - gpioComIF->pullLow(gpioId); - } - - /* Execute transfer */ - if(fullDuplex) { - /* Initiate a full duplex SPI transfer. */ - retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), spiCookie->getTransferStructHandle()); - if(retval < 0) { - utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); - result = FULL_DUPLEX_TRANSFER_FAILED; - } -#if FSFW_LINUX_SPI_WIRETAPPING == 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "Sent SPI data: " << std::endl; - size_t dataLen = spiCookie->getTransferStructHandle()->len; - uint8_t* dataPtr = reinterpret_cast(spiCookie->getTransferStructHandle()->tx_buf); - arrayprinter::print(dataPtr, dataLen, OutputType::HEX, false); - sif::info << "Received SPI data: " << std::endl; - dataPtr = reinterpret_cast(spiCookie->getTransferStructHandle()->rx_buf); - arrayprinter::print(dataPtr, dataLen, OutputType::HEX, false); -#else -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ - } - else { - /* We write with a blocking half-duplex transfer here */ - if (write(fileDescriptor, sendData, sendLen) != static_cast(sendLen)) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "SpiComIF::sendMessage: Half-Duplex write operation failed!" << - std::endl; -#else - sif::printWarning("SpiComIF::sendMessage: Half-Duplex write operation failed!\n"); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - result = HALF_DUPLEX_TRANSFER_FAILED; - } - } - - if(gpioId != gpio::NO_GPIO) { - gpioComIF->pullHigh(gpioId); - } - return result; -} - -ReturnValue_t SpiComIF::getSendSuccess(CookieIF *cookie) { - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t SpiComIF::requestReceiveMessage(CookieIF *cookie, size_t requestLen) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - SpiCookie* spiCookie = dynamic_cast(cookie); - if(spiCookie == nullptr) { - return NULLPOINTER; - } - - bool fullDuplex = spiCookie->isFullDuplex(); - if(fullDuplex) { - return HasReturnvaluesIF::RETURN_OK; - } - - std::string device = spiCookie->getSpiDevice(); - int fileDescriptor = 0; - utility::UnixFileHelper fileHelper(device, &fileDescriptor, O_RDWR, - "SpiComIF::requestReceiveMessage: "); - if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { - return OPENING_FILE_FAILED; - } - - uint8_t* rxBuf = nullptr; - size_t readSize = spiCookie->getCurrentTransferSize(); - result = getReadBuffer(spiCookie->getSpiAddress(), &rxBuf); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - gpioId_t gpioId = spiCookie->getChipSelectPin(); - MutexGuard(spiMutex, timeoutType, timeoutMs); - if(gpioId != gpio::NO_GPIO) { - gpioComIF->pullLow(gpioId); - } - - if(read(fileDescriptor, rxBuf, readSize) != static_cast(readSize)) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "SpiComIF::sendMessage: Half-Duplex read operation failed!" << std::endl; -#else - sif::printWarning("SpiComIF::sendMessage: Half-Duplex read operation failed!\n"); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - result = HALF_DUPLEX_TRANSFER_FAILED; - } - - if(gpioId != gpio::NO_GPIO) { - gpioComIF->pullHigh(gpioId); - } - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t SpiComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) { - SpiCookie* spiCookie = dynamic_cast(cookie); - if(spiCookie == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - uint8_t* rxBuf = nullptr; - ReturnValue_t result = getReadBuffer(spiCookie->getSpiAddress(), &rxBuf); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - *buffer = rxBuf; - *size = spiCookie->getCurrentTransferSize(); - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t SpiComIF::getReadBuffer(address_t spiAddress, uint8_t** buffer) { - if(buffer == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - auto iter = spiDeviceMap.find(spiAddress); - if(iter == spiDeviceMap.end()) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - *buffer = iter->second.replyBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; -} - -void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) { - int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast(&mode)); - if(retval != 0) { - utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); - } - - retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed); - if(retval != 0) { - utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!"); - } -} diff --git a/linux/spi/SpiComIF.h b/linux/spi/SpiComIF.h deleted file mode 100644 index 0e27a595..00000000 --- a/linux/spi/SpiComIF.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef LINUX_SPI_SPICOMIF_H_ -#define LINUX_SPI_SPICOMIF_H_ - -#include -#include -#include -#include -#include - -#include -#include - -/** - * @brief Encapsulates access to linux SPI driver for FSFW objects - * @details - * Right now, only full-duplex SPI is supported. - * @author R. Mueller - */ -class SpiComIF: public DeviceCommunicationIF, public SystemObject { -public: - static constexpr uint8_t spiRetvalId = CLASS_ID::LINUX_SPI_COM_IF; - static constexpr ReturnValue_t OPENING_FILE_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 0); - /* Full duplex (ioctl) transfer failure */ - static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 1); - /* Half duplex (read/write) transfer failure */ - static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 2); - - SpiComIF(object_id_t objectId, GpioIF* gpioComIF); - - ReturnValue_t initializeInterface(CookieIF * cookie) override; - ReturnValue_t sendMessage(CookieIF *cookie,const uint8_t *sendData, - size_t sendLen) override; - ReturnValue_t getSendSuccess(CookieIF *cookie) override; - ReturnValue_t requestReceiveMessage(CookieIF *cookie, - size_t requestLen) override; - ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer, - size_t *size) override; -private: - - struct SpiInstance { - std::vector replyBuffer; - }; - - GpioIF* gpioComIF = nullptr; - - MutexIF* spiMutex = nullptr; - MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = 20; - - using SpiDeviceMap = std::unordered_map; - using SpiDeviceMapIter = SpiDeviceMap::iterator; - - SpiDeviceMap spiDeviceMap; - - - ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); - void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); -}; - -#endif /* LINUX_SPI_SPICOMIF_H_ */ diff --git a/linux/spi/SpiCookie.cpp b/linux/spi/SpiCookie.cpp deleted file mode 100644 index 91117682..00000000 --- a/linux/spi/SpiCookie.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "SpiCookie.h" - -SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed): spiAddress(spiAddress), - chipSelectPin(chipSelect), spiDevice(spiDev), maxSize(maxSize), spiMode(spiMode), - spiSpeed(spiSpeed) { -} - -SpiCookie::SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxSize, - spi::SpiModes spiMode, uint32_t spiSpeed): - SpiCookie(spiAddress, gpio::NO_GPIO, spiDev, maxSize, spiMode, spiSpeed) { -} - -void SpiCookie::getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed, - UncommonParameters* parameters) const { - spiMode = this->spiMode; - spiSpeed = this->spiSpeed; - - if(parameters != nullptr) { - parameters->threeWireSpi = uncommonParameters.threeWireSpi; - parameters->lsbFirst = uncommonParameters.lsbFirst; - parameters->noCs = uncommonParameters.noCs; - parameters->bitsPerWord = uncommonParameters.bitsPerWord; - parameters->csHigh = uncommonParameters.csHigh; - } -} - -gpioId_t SpiCookie::getChipSelectPin() const { - return chipSelectPin; -} - -size_t SpiCookie::getMaxBufferSize() const { - return maxSize; -} - -address_t SpiCookie::getSpiAddress() const { - return spiAddress; -} - -std::string SpiCookie::getSpiDevice() const { - return spiDevice; -} - -void SpiCookie::setThreeWireSpi(bool enable) { - uncommonParameters.threeWireSpi = enable; -} - -void SpiCookie::setLsbFirst(bool enable) { - uncommonParameters.lsbFirst = enable; -} - -void SpiCookie::setNoCs(bool enable) { - uncommonParameters.noCs = enable; -} - -void SpiCookie::setBitsPerWord(uint8_t bitsPerWord) { - uncommonParameters.bitsPerWord = bitsPerWord; -} - -void SpiCookie::setCsHigh(bool enable) { - uncommonParameters.csHigh = enable; -} - -void SpiCookie::activateCsDeselect(bool deselectCs, uint16_t delayUsecs) { - spiTransferStruct.cs_change = deselectCs; - spiTransferStruct.delay_usecs = delayUsecs; -} - -void SpiCookie::assignReadBuffer(uint8_t* rx) { - if(rx != nullptr) { - spiTransferStruct.rx_buf = reinterpret_cast<__u64>(rx); - } -} - -void SpiCookie::assignWriteBuffer(const uint8_t* tx) { - if(tx != nullptr) { - spiTransferStruct.tx_buf = reinterpret_cast<__u64>(tx); - } -} - -spi_ioc_transfer* SpiCookie::getTransferStructHandle() { - return &spiTransferStruct; -} - -void SpiCookie::setFullOrHalfDuplex(bool halfDuplex) { - this->halfDuplex = halfDuplex; -} - -bool SpiCookie::isFullDuplex() const { - return not this->halfDuplex; -} - -void SpiCookie::assignTransferSize(size_t transferSize) { - spiTransferStruct.len = transferSize; -} - -size_t SpiCookie::getCurrentTransferSize() const { - return spiTransferStruct.len; -} - -void SpiCookie::setSpiSpeed(uint32_t newSpeed) { - this->spiSpeed = newSpeed; -} - -void SpiCookie::setSpiMode(spi::SpiModes newMode) { - this->spiMode = newMode; -} diff --git a/linux/spi/SpiCookie.h b/linux/spi/SpiCookie.h deleted file mode 100644 index 59d0e206..00000000 --- a/linux/spi/SpiCookie.h +++ /dev/null @@ -1,119 +0,0 @@ -#ifndef LINUX_SPI_SPICOOKIE_H_ -#define LINUX_SPI_SPICOOKIE_H_ - -#include "spiDefinitions.h" -#include -#include -#include - -class SpiCookie: public CookieIF { -public: - - /** - * Each SPI device will have a corresponding cookie. The cookie is used by the communication - * interface and contains device specific information like the largest expected size to be - * sent and received and the GPIO pin used to toggle the SPI slave select pin. - * @param spiAddress - * @param chipSelect Chip select. gpio::NO_GPIO can be used for hardware slave selects. - * @param spiDev - * @param maxSize - */ - SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxReplySize, spi::SpiModes spiMode, uint32_t spiSpeed); - - /** - * Like constructor above, but without a dedicated GPIO CS. Can be used for hardware - * slave select or if CS logic is performed with decoders. - */ - SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxReplySize, - spi::SpiModes spiMode, uint32_t spiSpeed); - - address_t getSpiAddress() const; - std::string getSpiDevice() const; - gpioId_t getChipSelectPin() const; - size_t getMaxBufferSize() const; - - /** Enables changing SPI speed at run-time */ - void setSpiSpeed(uint32_t newSpeed); - /** Enables changing the SPI mode at run-time */ - void setSpiMode(spi::SpiModes newMode); - - /** - * True if SPI transfers should be performed in full duplex mode - * @return - */ - bool isFullDuplex() const; - - /** - * Set transfer type to full duplex or half duplex. Full duplex is the default setting, - * ressembling common SPI hardware implementation with shift registers, where read and writes - * happen simultaneosly. - * @param fullDuplex - */ - void setFullOrHalfDuplex(bool halfDuplex); - - /** - * This needs to be called to specify where the SPI driver writes to or reads from. - * @param readLocation - * @param writeLocation - */ - void assignReadBuffer(uint8_t* rx); - void assignWriteBuffer(const uint8_t* tx); - /** - * Assign size for the next transfer. - * @param transferSize - */ - void assignTransferSize(size_t transferSize); - size_t getCurrentTransferSize() const; - - struct UncommonParameters { - uint8_t bitsPerWord = 8; - bool noCs = false; - bool csHigh = false; - bool threeWireSpi = false; - /* MSB first is more common */ - bool lsbFirst = false; - }; - - /** - * Can be used to explicitely disable hardware chip select. - * Some drivers like the Raspberry Pi Linux driver will not use hardware chip select by default - * (see https://www.raspberrypi.org/documentation/hardware/raspberrypi/spi/README.md) - * @param enable - */ - void setNoCs(bool enable); - void setThreeWireSpi(bool enable); - void setLsbFirst(bool enable); - void setCsHigh(bool enable); - void setBitsPerWord(uint8_t bitsPerWord); - - void getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed, - UncommonParameters* parameters = nullptr) const; - - /** - * See spidev.h cs_change and delay_usecs - * @param deselectCs - * @param delayUsecs - */ - void activateCsDeselect(bool deselectCs, uint16_t delayUsecs); - - spi_ioc_transfer* getTransferStructHandle(); -private: - size_t currentTransferSize = 0; - - address_t spiAddress; - gpioId_t chipSelectPin; - std::string spiDevice; - - const size_t maxSize; - spi::SpiModes spiMode; - uint32_t spiSpeed; - bool halfDuplex = false; - - struct spi_ioc_transfer spiTransferStruct = {}; - UncommonParameters uncommonParameters; -}; - - - -#endif /* LINUX_SPI_SPICOOKIE_H_ */ diff --git a/linux/spi/spiDefinitions.h b/linux/spi/spiDefinitions.h deleted file mode 100644 index e8c48147..00000000 --- a/linux/spi/spiDefinitions.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef LINUX_SPI_SPIDEFINITONS_H_ -#define LINUX_SPI_SPIDEFINITONS_H_ - -#include - -namespace spi { - -enum SpiModes: uint8_t { - MODE_0, - MODE_1, - MODE_2, - MODE_3 -}; - -} - -#endif /* LINUX_SPI_SPIDEFINITONS_H_ */ diff --git a/linux/i2c/CMakeLists.txt b/linux/uart/CMakeLists.txt similarity index 58% rename from linux/i2c/CMakeLists.txt rename to linux/uart/CMakeLists.txt index 179abd06..7b503d02 100644 --- a/linux/i2c/CMakeLists.txt +++ b/linux/uart/CMakeLists.txt @@ -1,6 +1,6 @@ target_sources(${TARGET_NAME} PUBLIC - I2cComIF.cpp - I2cCookie.cpp + UartComIF.cpp + UartCookie.cpp ) diff --git a/linux/uart/UartComIF.cpp b/linux/uart/UartComIF.cpp new file mode 100644 index 00000000..79470a45 --- /dev/null +++ b/linux/uart/UartComIF.cpp @@ -0,0 +1,368 @@ +#include "UartComIF.h" + +#include +#include + +#include +#include +#include +#include + +UartComIF::UartComIF(object_id_t objectId): SystemObject(objectId){ +} + +UartComIF::~UartComIF() {} + +ReturnValue_t UartComIF::initializeInterface(CookieIF * cookie) { + + std::string deviceFile; + UartDeviceMapIter uartDeviceMapIter; + + if(cookie == nullptr) { + return NULLPOINTER; + } + + UartCookie* uartCookie = dynamic_cast(cookie); + if (uartCookie == nullptr) { + sif::error << "UartComIF::initializeInterface: Invalid UART Cookie!" << std::endl; + return NULLPOINTER; + } + + deviceFile = uartCookie->getDeviceFile(); + + uartDeviceMapIter = uartDeviceMap.find(deviceFile); + if(uartDeviceMapIter == uartDeviceMap.end()) { + int fileDescriptor = configureUartPort(uartCookie); + if (fileDescriptor < 0) { + return RETURN_FAILED; + } + size_t maxReplyLen = uartCookie->getMaxReplyLen(); + UartElements_t uartElements = {fileDescriptor, std::vector(maxReplyLen), 0}; + std::pair status = uartDeviceMap.emplace(deviceFile, uartElements); + if (status.second == false) { + sif::debug << "UartComIF::initializeInterface: Failed to insert device " << deviceFile + << "to Uart device map" << std::endl; + return RETURN_FAILED; + } + } + else { + sif::debug << "UartComIF::initializeInterface: Uart device " << deviceFile << "already in " + << "use" << std::endl; + return RETURN_FAILED; + } + + return RETURN_OK; +} + +int UartComIF::configureUartPort(UartCookie* uartCookie) { + + struct termios options; + + std::string deviceFile = uartCookie->getDeviceFile(); + int fd = open(deviceFile.c_str(), O_RDWR); + + if (fd < 0) { + sif::debug << "UartComIF::configureUartPort: Failed to open uart " << deviceFile << "with" + << " error code " << errno << strerror(errno) << std::endl; + return fd; + } + + /* Read in existing settings */ + if(tcgetattr(fd, &options) != 0) { + sif::debug << "UartComIF::configureUartPort: Error " << errno << "from tcgetattr: " + << strerror(errno) << std::endl; + return fd; + } + + setParityOptions(&options, uartCookie); + setStopBitOptions(&options, uartCookie); + setDatasizeOptions(&options, uartCookie); + setFixedOptions(&options); + + /* Sets uart to non-blocking mode. Read returns immediately when there are no data available */ + options.c_cc[VTIME] = 0; + options.c_cc[VMIN] = 0; + + configureBaudrate(&options, uartCookie); + + /* Save option settings */ + if (tcsetattr(fd, TCSANOW, &options) != 0) { + sif::debug << "UartComIF::configureUartPort: Failed to set options with error " << errno + << ": " << strerror(errno); + return fd; + } + return fd; +} + +void UartComIF::setParityOptions(struct termios* options, UartCookie* uartCookie) { + /* Clear parity bit */ + options->c_cflag &= ~PARENB; + switch (uartCookie->getParity()) { + case Parity::EVEN: + options->c_cflag |= PARENB; + options->c_cflag &= ~PARODD; + break; + case Parity::ODD: + options->c_cflag |= PARENB; + options->c_cflag |= PARODD; + break; + default: + break; + } +} + +void UartComIF::setStopBitOptions(struct termios* options, UartCookie* uartCookie) { + /* Clear stop field. Sets stop bit to one bit */ + options->c_cflag &= ~CSTOPB; + switch (uartCookie->getStopBits()) { + case StopBits::TWO_STOP_BITS: + options->c_cflag |= CSTOPB; + break; + default: + break; + } +} + +void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCookie) { + /* Clear size bits */ + options->c_cflag &= ~CSIZE; + switch (uartCookie->getBitsPerWord()) { + case 5: + options->c_cflag |= CS5; + break; + case 6: + options->c_cflag |= CS6; + break; + case 7: + options->c_cflag |= CS7; + break; + case 8: + options->c_cflag |= CS8; + break; + default: + sif::debug << "UartComIF::setDatasizeOptions: Invalid size specified" << std::endl; + break; + } +} + +void UartComIF::setFixedOptions(struct termios* options) { + /* Disable RTS/CTS hardware flow control */ + options->c_cflag &= ~CRTSCTS; + /* Turn on READ & ignore ctrl lines (CLOCAL = 1) */ + options->c_cflag |= CREAD | CLOCAL; + /* Disable canonical mode */ + options->c_lflag &= ~ICANON; + /* Disable echo */ + options->c_lflag &= ~ECHO; + /* Disable erasure */ + options->c_lflag &= ~ECHOE; + /* Disable new-line echo */ + options->c_lflag &= ~ECHONL; + /* Disable interpretation of INTR, QUIT and SUSP */ + options->c_lflag &= ~ISIG; + /* Turn off s/w flow ctrl */ + options->c_iflag &= ~(IXON | IXOFF | IXANY); + /* Disable any special handling of received bytes */ + options->c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); + /* Prevent special interpretation of output bytes (e.g. newline chars) */ + options->c_oflag &= ~OPOST; + /* Prevent conversion of newline to carriage return/line feed */ + options->c_oflag &= ~ONLCR; +} + +void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCookie) { + switch (uartCookie->getBaudrate()) { + case 50: + cfsetispeed(options, B50); + cfsetospeed(options, B50); + break; + case 75: + cfsetispeed(options, B75); + cfsetospeed(options, B75); + break; + case 110: + cfsetispeed(options, B110); + cfsetospeed(options, B110); + break; + case 134: + cfsetispeed(options, B134); + cfsetospeed(options, B134); + break; + case 150: + cfsetispeed(options, B150); + cfsetospeed(options, B150); + break; + case 200: + cfsetispeed(options, B200); + cfsetospeed(options, B200); + break; + case 300: + cfsetispeed(options, B300); + cfsetospeed(options, B300); + break; + case 600: + cfsetispeed(options, B600); + cfsetospeed(options, B600); + break; + case 1200: + cfsetispeed(options, B1200); + cfsetospeed(options, B1200); + break; + case 1800: + cfsetispeed(options, B1800); + cfsetospeed(options, B1800); + break; + case 2400: + cfsetispeed(options, B2400); + cfsetospeed(options, B2400); + break; + case 4800: + cfsetispeed(options, B4800); + cfsetospeed(options, B4800); + break; + case 9600: + cfsetispeed(options, B9600); + cfsetospeed(options, B9600); + break; + case 19200: + cfsetispeed(options, B19200); + cfsetospeed(options, B19200); + break; + case 38400: + cfsetispeed(options, B38400); + cfsetospeed(options, B38400); + break; + case 57600: + cfsetispeed(options, B57600); + cfsetospeed(options, B57600); + break; + case 115200: + cfsetispeed(options, B115200); + cfsetospeed(options, B115200); + break; + case 230400: + cfsetispeed(options, B230400); + cfsetospeed(options, B230400); + break; + case 460800: + cfsetispeed(options, B460800); + cfsetospeed(options, B460800); + break; + default: + sif::debug << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; + break; + } +} + +ReturnValue_t UartComIF::sendMessage(CookieIF *cookie, + const uint8_t *sendData, size_t sendLen) { + + int fd; + std::string deviceFile; + UartDeviceMapIter uartDeviceMapIter; + + if(sendData == nullptr) { + sif::debug << "UartComIF::sendMessage: Send Data is nullptr" << std::endl; + return RETURN_FAILED; + } + + if(sendLen == 0) { + return RETURN_OK; + } + + UartCookie* uartCookie = dynamic_cast(cookie); + if(uartCookie == nullptr) { + sif::debug << "UartComIF::sendMessasge: Invalid Uart Cookie!" << std::endl; + return NULLPOINTER; + } + + deviceFile = uartCookie->getDeviceFile(); + uartDeviceMapIter = uartDeviceMap.find(deviceFile); + if (uartDeviceMapIter == uartDeviceMap.end()) { + sif::debug << "UartComIF::sendMessage: Device file " << deviceFile << "not in uart map" + << std::endl; + return RETURN_FAILED; + } + + fd = uartDeviceMapIter->second.fileDescriptor; + + if (write(fd, sendData, sendLen) != (int)sendLen) { + sif::error << "UartComIF::sendMessage: Failed to send data with error code " << errno + << ": Error description: " << strerror(errno) << std::endl; + return RETURN_FAILED; + } + + return RETURN_OK; +} + +ReturnValue_t UartComIF::getSendSuccess(CookieIF *cookie) { + return RETURN_OK; +} + +ReturnValue_t UartComIF::requestReceiveMessage(CookieIF *cookie, + size_t requestLen) { + + int fd; + std::string deviceFile; + UartDeviceMapIter uartDeviceMapIter; + uint8_t* bufferPtr; + + if(requestLen == 0) { + return RETURN_OK; + } + + UartCookie* uartCookie = dynamic_cast(cookie); + if(uartCookie == nullptr) { + sif::debug << "UartComIF::requestReceiveMessage: Invalid Uart Cookie!" << std::endl; + return NULLPOINTER; + } + + deviceFile = uartCookie->getDeviceFile(); + uartDeviceMapIter = uartDeviceMap.find(deviceFile); + if (uartDeviceMapIter == uartDeviceMap.end()) { + sif::debug << "UartComIF::requestReceiveMessage: Device file " << deviceFile + << " not in uart map" << std::endl; + return RETURN_FAILED; + } + + fd = uartDeviceMapIter->second.fileDescriptor; + bufferPtr = uartDeviceMapIter->second.replyBuffer.data(); + int bytesRead = read(fd, bufferPtr, requestLen); + if (bytesRead != static_cast(requestLen)) { + sif::debug << "UartComIF::requestReceiveMessage: Only read " << bytesRead + << " of " << requestLen << " bytes" << std::endl; + return RETURN_FAILED; + } + else { + uartDeviceMapIter->second.replyLen = bytesRead; + } + + return RETURN_OK; +} + +ReturnValue_t UartComIF::readReceivedMessage(CookieIF *cookie, + uint8_t **buffer, size_t* size) { + + std::string deviceFile; + UartDeviceMapIter uartDeviceMapIter; + + UartCookie* uartCookie = dynamic_cast(cookie); + if(uartCookie == nullptr) { + sif::debug << "UartComIF::readReceivedMessage: Invalid uart cookie!" << std::endl; + return NULLPOINTER; + } + + deviceFile = uartCookie->getDeviceFile(); + uartDeviceMapIter = uartDeviceMap.find(deviceFile); + if (uartDeviceMapIter == uartDeviceMap.end()) { + sif::debug << "UartComIF::readReceivedMessage: Device file " << deviceFile + << " not in uart map" << std::endl; + return RETURN_FAILED; + } + + *buffer = uartDeviceMapIter->second.replyBuffer.data(); + *size = uartDeviceMapIter->second.replyLen; + + return RETURN_OK; +} + diff --git a/linux/uart/UartComIF.h b/linux/uart/UartComIF.h new file mode 100644 index 00000000..9dd854da --- /dev/null +++ b/linux/uart/UartComIF.h @@ -0,0 +1,94 @@ +#ifndef BSP_Q7S_COMIF_UARTCOMIF_H_ +#define BSP_Q7S_COMIF_UARTCOMIF_H_ + +#include +#include + +#include +#include + +#include "UartCookie.h" + +/** + * @brief This is the communication interface to access serial ports on linux based operating + * systems. + * + * @details The implementation follows the instructions from https://blog.mbedded.ninja/programming/ + * operating-systems/linux/linux-serial-ports-using-c-cpp/#disabling-canonical-mode + * + * @author J. Meier + */ +class UartComIF: public DeviceCommunicationIF, public SystemObject { +public: + UartComIF(object_id_t objectId); + + virtual ~UartComIF(); + + ReturnValue_t initializeInterface(CookieIF * cookie) override; + ReturnValue_t sendMessage(CookieIF *cookie,const uint8_t *sendData, + size_t sendLen) override; + ReturnValue_t getSendSuccess(CookieIF *cookie) override; + ReturnValue_t requestReceiveMessage(CookieIF *cookie, + size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer, + size_t *size) override; + +private: + + using UartDeviceFile_t = std::string; + + typedef struct UartElements { + int fileDescriptor; + std::vector replyBuffer; + /** Number of bytes read will be written to this variable */ + size_t replyLen; + } UartElements_t; + + using UartDeviceMap = std::unordered_map; + using UartDeviceMapIter = UartDeviceMap::iterator; + + /** + * The uart devie map stores informations of initialized uart ports. + */ + UartDeviceMap uartDeviceMap; + + /** + * @brief This function opens and configures a uart device by using the information stored + * in the uart cookie. + * @param uartCookie Pointer to uart cookie with information about the uart. Contains the + * uart device file, baudrate, parity, stopbits etc. + * @return The file descriptor of the configured uart. + */ + int configureUartPort(UartCookie* uartCookie); + + /** + * @brief This function adds the parity settings to the termios options struct. + * + * @param options Pointer to termios options struct which will be modified to enable or disable + * parity checking. + * @param uartCookie Pointer to uart cookie containing the information about the desired + * parity settings. + * + */ + void setParityOptions(struct termios* options, UartCookie* uartCookie); + + void setStopBitOptions(struct termios* options, UartCookie* uartCookie); + + /** + * @brief This function sets options which are not configurable by the uartCookie. + */ + void setFixedOptions(struct termios* options); + + /** + * @brief With this function the datasize settings are added to the termios options struct. + */ + void setDatasizeOptions(struct termios* options, UartCookie* uartCookie); + + /** + * @brief This functions adds the baudrate specified in the uartCookie to the termios options + * struct. + */ + void configureBaudrate(struct termios* options, UartCookie* uartCookie); +}; + +#endif /* BSP_Q7S_COMIF_UARTCOMIF_H_ */ diff --git a/linux/uart/UartCookie.cpp b/linux/uart/UartCookie.cpp new file mode 100644 index 00000000..b8149388 --- /dev/null +++ b/linux/uart/UartCookie.cpp @@ -0,0 +1,63 @@ +#include "UartCookie.h" + +#include + +UartCookie::UartCookie(std::string deviceFile, uint32_t baudrate, size_t maxReplyLen) : + deviceFile(deviceFile), baudrate(baudrate), maxReplyLen(maxReplyLen) { +} + +UartCookie::~UartCookie() {} + +uint32_t UartCookie::getBaudrate() const { + return baudrate; +} + +size_t UartCookie::getMaxReplyLen() const { + return maxReplyLen; +} + +std::string UartCookie::getDeviceFile() const { + return deviceFile; +} + +void UartCookie::setParityOdd() { + parity = Parity::ODD; +} + +void UartCookie::setParityEven() { + parity = Parity::EVEN; +} + +Parity UartCookie::getParity() const { + return parity; +} + +void UartCookie::setBitsPerWord(uint8_t bitsPerWord_) { + switch(bitsPerWord_) { + case 5: + case 6: + case 7: + case 8: + break; + default: + sif::debug << "UartCookie::setBitsPerWord: Invalid bits per word specified" << std::endl; + return; + } + bitsPerWord = bitsPerWord_; +} + +uint8_t UartCookie::getBitsPerWord() const { + return bitsPerWord; +} + +StopBits UartCookie::getStopBits() const { + return stopBits; +} + +void UartCookie::setTwoStopBits() { + stopBits = StopBits::TWO_STOP_BITS; +} + +void UartCookie::setOneStopBit() { + stopBits = StopBits::ONE_STOP_BIT; +} diff --git a/linux/uart/UartCookie.h b/linux/uart/UartCookie.h new file mode 100644 index 00000000..59004719 --- /dev/null +++ b/linux/uart/UartCookie.h @@ -0,0 +1,81 @@ +#ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ +#define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ + +#include +#include + +enum class Parity { + NONE, + EVEN, + ODD +}; + +enum class StopBits { + ONE_STOP_BIT, + TWO_STOP_BITS +}; + +/** + * @brief Cookie for the UartComIF. There are many options available to configure the uart driver. + * The constructor only requests for common options like the baudrate. Other options can + * be set by member functions. + * + * @author J. Meier + */ +class UartCookie: public CookieIF { +public: + + /** + * @brief Constructor for the uart cookie. + * @param deviceFile The device file specifying the uart to use. E.g. "/dev/ttyPS1". + * @param baudrate The baudrate to use for input and output. Possible Baudrates are: 50, + * 75, 110, 134, 150, 200, 300, 600, 1200, 1800, 2400, 4800, 9600, B19200, + * 38400, 57600, 115200, 230400, 460800 + * @param maxReplyLen The maximum size an object using this cookie expects. + * + * @details Default configuration: No parity + * 8 databits (number of bits transfered with one uart frame) + * One stop bit + * + * + */ + UartCookie(std::string deviceFile, uint32_t baudrate, size_t maxReplyLen); + + virtual ~UartCookie(); + + uint32_t getBaudrate() const; + size_t getMaxReplyLen() const; + std::string getDeviceFile() const; + Parity getParity() const; + uint8_t getBitsPerWord() const; + StopBits getStopBits() const; + + /** + * Functions two enable parity checking. + */ + void setParityOdd(); + void setParityEven(); + + /** + * Function two set number of bits per UART frame. + */ + void setBitsPerWord(uint8_t bitsPerWord_); + + /** + * Function to specify the number of stopbits. + */ + void setTwoStopBits(); + void setOneStopBit(); + + +private: + + std::string deviceFile; + uint32_t baudrate; + size_t maxReplyLen = 0; + Parity parity = Parity::NONE; + uint8_t bitsPerWord = 8; + StopBits stopBits = StopBits::ONE_STOP_BIT; +}; + +#endif diff --git a/linux/utility/CMakeLists.txt b/linux/utility/CMakeLists.txt index 71151b6c..45a7edcc 100644 --- a/linux/utility/CMakeLists.txt +++ b/linux/utility/CMakeLists.txt @@ -1,5 +1,4 @@ target_sources(${TARGET_NAME} PUBLIC - Utility.cpp ) diff --git a/linux/utility/Utility.cpp b/linux/utility/Utility.cpp deleted file mode 100644 index 45c347db..00000000 --- a/linux/utility/Utility.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "Utility.h" - -void utility::handleIoctlError(const char* const customPrintout) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - if(customPrintout != nullptr) { - sif::warning << customPrintout << std::endl; - } - sif::warning << "handleIoctlError: Error code " << errno << ", "<< strerror(errno) << - std::endl; -#else - if(customPrintout != nullptr) { - sif::printWarning("%s\n", customPrintout); - } - sif::printWarning("handleIoctlError: Error code %d, %s\n", errno, strerror(errno)); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - -} - -utility::UnixFileHelper::UnixFileHelper(std::string device, int* fileDescriptor, int flags, - std::string diagnosticPrefix): - fileDescriptor(fileDescriptor) { - if(fileDescriptor == nullptr) { - return; - } - *fileDescriptor = open(device.c_str(), flags); - if (*fileDescriptor < 0) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << diagnosticPrefix <<"Opening device failed with error code " << errno << - "." << std::endl; - sif::warning << "Error description: " << strerror(errno) << std::endl; -#else - sif::printError("%sOpening device failed with error code %d.\n", diagnosticPrefix); - sif::printWarning("Error description: %s\n", strerror(errno)); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - openStatus = OPEN_FILE_FAILED; - } -} - -utility::UnixFileHelper::~UnixFileHelper() { - if(fileDescriptor != nullptr) { - close(*fileDescriptor); - } -} - -ReturnValue_t utility::UnixFileHelper::getOpenResult() const { - return openStatus; -} - diff --git a/linux/utility/Utility.h b/linux/utility/Utility.h deleted file mode 100644 index a93b4936..00000000 --- a/linux/utility/Utility.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef LINUX_UTILITY_UTILITY_H_ -#define LINUX_UTILITY_UTILITY_H_ - -#include -#include -#include - -#include -#include - -namespace utility { - -void handleIoctlError(const char* const customPrintout); - -class UnixFileHelper { -public: - static constexpr int READ_WRITE_FLAG = O_RDWR; - static constexpr int READ_ONLY_FLAG = O_RDONLY; - static constexpr int NON_BLOCKING_IO_FLAG = O_NONBLOCK; - - static constexpr ReturnValue_t OPEN_FILE_FAILED = 1; - - UnixFileHelper(std::string device, int* fileDescriptor, int flags, - std::string diagnosticPrefix = ""); - - virtual~ UnixFileHelper(); - - ReturnValue_t getOpenResult() const; -private: - int* fileDescriptor = nullptr; - ReturnValue_t openStatus = HasReturnvaluesIF::RETURN_OK; -}; - -} - -#endif /* LINUX_UTILITY_UTILITY_H_ */ diff --git a/bsp_rpi/gpio/GPIORPi.cpp b/misc/archive/GPIORPi.cpp similarity index 100% rename from bsp_rpi/gpio/GPIORPi.cpp rename to misc/archive/GPIORPi.cpp diff --git a/bsp_rpi/gpio/GPIORPi.h b/misc/archive/GPIORPi.h similarity index 100% rename from bsp_rpi/gpio/GPIORPi.h rename to misc/archive/GPIORPi.h diff --git a/misc/eclipse/.project b/misc/eclipse/.project index 906c567f..059a5c19 100644 --- a/misc/eclipse/.project +++ b/misc/eclipse/.project @@ -7,7 +7,7 @@ org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, + full,incremental, diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index a8205276..6e8518b0 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -10,6 +10,9 @@ target_sources(${TARGET_NAME} PUBLIC PDU1Handler.cpp PDU2Handler.cpp ACUHandler.cpp + SyrlinksHkHandler.cpp + Max31865PT1000Handler.cpp + IMTQHandler.cpp ) diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp new file mode 100644 index 00000000..0c1b1d1e --- /dev/null +++ b/mission/devices/IMTQHandler.cpp @@ -0,0 +1,216 @@ +#include "IMTQHandler.h" + +#include +#include +#include +#include + +IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : + DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) { + if (comCookie == NULL) { + sif::error << "IMTQHandler: Invalid com cookie" << std::endl; + } +} + +IMTQHandler::~IMTQHandler() { +} + + +void IMTQHandler::doStartUp(){ + if(mode == _MODE_START_UP){ + //TODO: Set to MODE_ON again + setMode(MODE_NORMAL); + } +} + +void IMTQHandler::doShutDown(){ + +} + +ReturnValue_t IMTQHandler::buildNormalDeviceCommand( + DeviceCommandId_t * id) { + *id = IMTQ::GET_ENG_HK_DATA; + return buildCommandFromCommand(*id, NULL, 0); +} + +ReturnValue_t IMTQHandler::buildTransitionDeviceCommand( + DeviceCommandId_t * id){ + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t IMTQHandler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t * commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(IMTQ::GET_ENG_HK_DATA): { + commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } + case(IMTQ::START_ACTUATION_DIPOLE): { + /* IMTQ expects low byte first */ + commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; + commandBuffer[1] = *(commandData + 1); + commandBuffer[2] = *(commandData); + commandBuffer[3] = *(commandData + 3); + commandBuffer[4] = *(commandData + 2); + commandBuffer[5] = *(commandData + 5); + commandBuffer[6] = *(commandData + 4); + commandBuffer[7] = *(commandData + 7); + commandBuffer[8] = *(commandData + 6); + rawPacket = commandBuffer; + rawPacketLen = 9; + return RETURN_OK; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +void IMTQHandler::fillCommandAndReplyMap() { + this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, + IMTQ::SIZE_ENG_HK_DATA_REPLY, false, true, IMTQ::SIZE_ENG_HK_DATA_REPLY); +} + +ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, + size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { + + ReturnValue_t result = RETURN_OK; + + switch(*start) { + case(IMTQ::CC::GET_ENG_HK_DATA): + *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; + *foundId = IMTQ::GET_ENG_HK_DATA; + break; + default: + sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; + result = IGNORE_REPLY_DATA; + break; + } + + return result; +} + +ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + + ReturnValue_t result = RETURN_OK; + + result = parseStatusByte(packet); + + if (result != RETURN_OK) { + return result; + } + + switch (id) { + case (IMTQ::GET_ENG_HK_DATA): + fillEngHkDataset(packet); + break; + default: { + sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl; + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + } + + return RETURN_OK; +} + +ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { + uint8_t cmdErrorField = *(packet + 1) & 0xF; + switch (cmdErrorField) { + case 0: + return RETURN_OK; + case 1: + return REJECTED_WITHOUT_REASON; + case 2: + return INVALID_COMMAND_CODE; + case 3: + return PARAMETER_MISSING; + case 4: + return PARAMETER_INVALID; + case 5: + return CC_UNAVAILABLE; + case 7: + return INTERNAL_PROCESSING_ERROR; + default: + sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " + << cmdErrorField << std::endl; + return CMD_ERR_UNKNOWN; + } +} + +void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { + uint8_t offset = 2; + engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset); + offset += 2; + engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset); + offset += 2; + engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset)); + offset += 2; + engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset)); + offset += 2; + engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset)); + offset += 2; + engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset)); + +#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1 + sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl; + sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl; + sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentA << " A" << std::endl; + sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentA << " A" << std::endl; + sif::info << "IMTQ coil X current: " << engHkDataset.coilXcurrentA << " A" << std::endl; + sif::info << "IMTQ coil Y current: " << engHkDataset.coilYcurrentA << " A" << std::endl; + sif::info << "IMTQ coil Z current: " << engHkDataset.coilZcurrentA << " A" << std::endl; + sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C" + << std::endl; + sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C" + << std::endl; + sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C" + << std::endl; + sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C" + << std::endl; +#endif +} + +void IMTQHandler::setNormalDatapoolEntriesInvalid(){ + +} + +uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){ + return 500; +} + +ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + + localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry( { 0 })); + + return HasReturnvaluesIF::RETURN_OK; +} + +void IMTQHandler::setModeNormal() { + mode = MODE_NORMAL; +} + diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h new file mode 100644 index 00000000..2e2a7612 --- /dev/null +++ b/mission/devices/IMTQHandler.h @@ -0,0 +1,77 @@ +#ifndef MISSION_DEVICES_IMTQHANDLER_H_ +#define MISSION_DEVICES_IMTQHANDLER_H_ + +#include +#include +#include + +/** + * @brief This is the device handler for the ISIS Magnetorquer iMTQ. + * + * @author J. Meier + */ +class IMTQHandler: public DeviceHandlerBase { +public: + + IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie); + virtual ~IMTQHandler(); + + /** + * @brief Sets mode to MODE_NORMAL. Can be used for debugging. + */ + void setModeNormal(); + +protected: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override; + void fillCommandAndReplyMap() override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t * commandData,size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + void setNormalDatapoolEntriesInvalid() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override; + +private: + + static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; + + static const ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(0xA2); + static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(0xA3); + static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4); + static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5); + static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6); + + + IMTQ::EngHkDataset engHkDataset; + + uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; + + /** + * @brief Each reply contains a status byte giving information about a request. This function + * parses this byte and returns the associated failure message. + * + * @param packet Pointer to the received message containing the status byte. + * + * @return The return code derived from the received status byte. + */ + ReturnValue_t parseStatusByte(const uint8_t* packet); + + /** + * @brief This function fills the engineering housekeeping dataset with the received data. + + * @param packet Pointer to the received data. + * + */ + void fillEngHkDataset(const uint8_t* packet); +}; + +#endif /* MISSION_DEVICES_IMTQHANDLER_H_ */ diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index fc9b8b4f..15f6e325 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -1,3 +1,4 @@ +#include #include "MGMHandlerLIS3MDL.h" #include @@ -32,8 +33,8 @@ void MGMHandlerLIS3MDL::doStartUp() { /* Will be set by checking device ID (WHO AM I register) */ if(commandExecuted) { commandExecuted = false; + internalState = InternalState::STATE_SETUP; } - internalState = InternalState::STATE_SETUP; break; } case(InternalState::STATE_SETUP): { @@ -462,7 +463,7 @@ void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { } uint32_t MGMHandlerLIS3MDL::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 10000; + return 20000; } void MGMHandlerLIS3MDL::modeChanged(void) { diff --git a/mission/devices/Max31865PT1000Handler.cpp b/mission/devices/Max31865PT1000Handler.cpp new file mode 100644 index 00000000..c6d98c0a --- /dev/null +++ b/mission/devices/Max31865PT1000Handler.cpp @@ -0,0 +1,358 @@ +#include "Max31865PT1000Handler.h" + +#include +#include + +Max31865PT1000Handler::Max31865PT1000Handler(object_id_t objectId, + object_id_t comIF, CookieIF *comCookie, uint8_t switchId): + DeviceHandlerBase(objectId, comIF, comCookie), switchId(switchId), + sensorDataset(this), sensorDatasetSid(sensorDataset.getSid()) { +#if OBSW_VERBOSE_LEVEL >= 1 + debugDivider = new PeriodicOperationDivider(10); +#endif +} + +Max31865PT1000Handler::~Max31865PT1000Handler() { +} + +void Max31865PT1000Handler::doStartUp() { + if(internalState == InternalState::NONE) { + internalState = InternalState::WARMUP; + Clock::getUptime(&startTime); + } + + if(internalState == InternalState::WARMUP) { + dur_millis_t timeNow = 0; + Clock::getUptime(&timeNow); + if(timeNow - startTime >= 100) { + internalState = InternalState::CONFIGURE; + } + } + + if(internalState == InternalState::CONFIGURE) { + if(commandExecuted) { + internalState = InternalState::REQUEST_CONFIG; + commandExecuted = false; + } + } + + if(internalState == InternalState::REQUEST_CONFIG) { + if (commandExecuted) { + setMode(MODE_ON); + setMode(MODE_NORMAL); + commandExecuted = false; + internalState = InternalState::RUNNING; + } + } +} + +void Max31865PT1000Handler::doShutDown() { + commandExecuted = false; + setMode(MODE_OFF); +} + +ReturnValue_t Max31865PT1000Handler::buildNormalDeviceCommand( + DeviceCommandId_t *id) { + if(internalState == InternalState::RUNNING) { + *id = Max31865Definitions::REQUEST_RTD; + return buildCommandFromCommand(*id, nullptr, 0); + } + else if(internalState == InternalState::REQUEST_FAULT_BYTE) { + *id = Max31865Definitions::REQUEST_FAULT_BYTE; + return buildCommandFromCommand(*id, nullptr, 0); + } + else { + return DeviceHandlerBase::NOTHING_TO_SEND; + } +} + +ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand( + DeviceCommandId_t *id) { + switch(internalState) { + case(InternalState::NONE): + case(InternalState::WARMUP): + case(InternalState::RUNNING): + return DeviceHandlerBase::NOTHING_TO_SEND; + case(InternalState::CONFIGURE): { + *id = Max31865Definitions::CONFIG_CMD; + uint8_t config[1] = {DEFAULT_CONFIG}; + return buildCommandFromCommand(*id, config, 1); + } + case(InternalState::REQUEST_CONFIG): { + *id = Max31865Definitions::REQUEST_CONFIG; + return buildCommandFromCommand(*id, nullptr, 0); + } + + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Max31865PT1000Handler: Invalid internal state" << std::endl; +#else + sif::printError("Max31865PT1000Handler: Invalid internal state\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(Max31865Definitions::CONFIG_CMD) : { + commandBuffer[0] = static_cast(Max31865Definitions::CONFIG_CMD); + if(commandDataLen == 1) { + commandBuffer[1] = commandData[0]; + DeviceHandlerBase::rawPacketLen = 2; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + else { + return DeviceHandlerIF::NO_COMMAND_DATA; + } + } + case(Max31865Definitions::REQUEST_CONFIG): { + commandBuffer[0] = 0x00; // dummy byte + commandBuffer[1] = static_cast( + Max31865Definitions::REQUEST_CONFIG); + DeviceHandlerBase::rawPacketLen = 2; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + case(Max31865Definitions::REQUEST_RTD): { + commandBuffer[0] = static_cast( + Max31865Definitions::REQUEST_RTD); + // two dummy bytes + commandBuffer[1] = 0x00; + commandBuffer[2] = 0x00; + DeviceHandlerBase::rawPacketLen = 3; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + case(Max31865Definitions::REQUEST_FAULT_BYTE): { + commandBuffer[0] = static_cast( + Max31865Definitions::REQUEST_FAULT_BYTE); + commandBuffer[1] = 0x00; + DeviceHandlerBase::rawPacketLen = 2; + DeviceHandlerBase::rawPacket = commandBuffer.data(); + return HasReturnvaluesIF::RETURN_OK; + } + default: + //Unknown DeviceCommand + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } +} + +void Max31865PT1000Handler::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(Max31865Definitions::CONFIG_CMD, 3); + insertInCommandAndReplyMap(Max31865Definitions::REQUEST_CONFIG, 3); + insertInCommandAndReplyMap(Max31865Definitions::REQUEST_RTD, 3, + &sensorDataset); + insertInCommandAndReplyMap(Max31865Definitions::REQUEST_FAULT_BYTE, 3); +} + +ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, + size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { + size_t rtdReplySize = 3; + size_t configReplySize = 2; + + if(remainingSize == rtdReplySize and + internalState == InternalState::RUNNING) { + *foundId = Max31865Definitions::REQUEST_RTD; + *foundLen = rtdReplySize; + } + + if(remainingSize == configReplySize) { + if(internalState == InternalState::CONFIGURE) { + commandExecuted = true; + *foundLen = configReplySize; + *foundId = Max31865Definitions::CONFIG_CMD; + } + else if(internalState == InternalState::REQUEST_FAULT_BYTE) { + *foundId = Max31865Definitions::REQUEST_FAULT_BYTE; + *foundLen = 2; + internalState = InternalState::RUNNING; + } + else { + *foundId = Max31865Definitions::REQUEST_CONFIG; + *foundLen = configReplySize; + } + } + + return RETURN_OK; +} + +ReturnValue_t Max31865PT1000Handler::interpretDeviceReply( + DeviceCommandId_t id, const uint8_t *packet) { + switch(id) { + case(Max31865Definitions::REQUEST_CONFIG): { + if(packet[1] != DEFAULT_CONFIG) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + // it propably would be better if we at least try one restart.. + sif::error << "Max31865PT1000Handler: Invalid configuration reply!" << std::endl; +#else + sif::printError("Max31865PT1000Handler: Invalid configuration reply!\n"); +#endif + return HasReturnvaluesIF::RETURN_OK; + } + // set to true for invalid configs too for now. + if(internalState == InternalState::REQUEST_CONFIG) { + commandExecuted = true; + } + else if(internalState == InternalState::RUNNING) { + // we should propably generate a telemetry with the config byte + // as payload here. + } + break; + } + case(Max31865Definitions::REQUEST_RTD): { + // first bit of LSB reply byte is the fault bit + uint8_t faultBit = packet[2] & 0b0000'0001; + if(faultBit == 1) { + // Maybe we should attempt to restart it? + if(faultByte == 0) { + internalState = InternalState::REQUEST_FAULT_BYTE; + } + } + + // RTD value consists of last seven bits of the LSB reply byte and + // the MSB reply byte + uint16_t adcCode = ((packet[1] << 8) | packet[2]) >> 1; + // do something with rtd value, will propably be stored in + // dataset. + float rtdValue = adcCode * RTD_RREF_PT1000 / INT16_MAX; + + // calculate approximation + float approxTemp = adcCode / 32.0 - 256.0; + +#if OBSW_VERBOSE_LEVEL >= 1 + if(debugDivider->checkAndIncrement()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "Max31865PT1000Handler::interpretDeviceReply: Measured " + << "resistance is " << rtdValue << " Ohms." << std::endl; + sif::info << "Approximated temperature is " << approxTemp << " °C" + << std::endl; +#else + sif::printInfo("Max31865PT1000Handler::interpretDeviceReply: Measured resistance is %f" + " Ohms.\n", rtdValue); + sif::printInfo("Approximated temperature is %f C\n", approxTemp); +#endif + sensorDataset.setChanged(true); + } +#endif + + ReturnValue_t result = sensorDataset.read(); + if(result != HasReturnvaluesIF::RETURN_OK) { + // Configuration error +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "Max31865PT1000Handler::interpretDeviceReply: Error reading dataset!" + << std::endl; +#else + sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: Error reading dataset!\n"); +#endif + return result; + } + + if(not sensorDataset.isValid()) { + sensorDataset.temperatureCelcius.setValid(true); + } + + sensorDataset.temperatureCelcius = approxTemp; + + result = sensorDataset.commit(); + + if(result != HasReturnvaluesIF::RETURN_OK) { + // Configuration error +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "Max31865PT1000Handler::interpretDeviceReply: " + "Error commiting dataset!" << std::endl; +#else + sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: " + "Error commiting dataset!\n"); +#endif + return result; + } + + break; + } + case(Max31865Definitions::REQUEST_FAULT_BYTE): { + faultByte = packet[1]; +#if OBSW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "Max31865PT1000Handler::interpretDeviceReply: Fault byte" + " is: 0b" << std::bitset<8>(faultByte) << std::endl; +#else + sif::printInfo("Max31865PT1000Handler::interpretDeviceReply: Fault byte" + " is: 0b" BYTE_TO_BINARY_PATTERN "\n", BYTE_TO_BINARY(faultByte)); +#endif +#endif + ReturnValue_t result = sensorDataset.read(); + if(result != HasReturnvaluesIF::RETURN_OK) { + // Configuration error +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "Max31865PT1000Handler::interpretDeviceReply: " + "Error reading dataset!" << std::endl; +#else + sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: " + "Error reading dataset!\n"); +#endif + return result; + } + sensorDataset.errorByte.setValid(true); + sensorDataset.errorByte = faultByte; + if(faultByte != 0) { + sensorDataset.temperatureCelcius.setValid(false); + } + + result = sensorDataset.commit(); + if(result != HasReturnvaluesIF::RETURN_OK) { + // Configuration error +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "Max31865PT1000Handler::interpretDeviceReply: " + "Error commiting dataset!" << std::endl; +#else + sif::printDebug("Max31865PT1000Handler::interpretDeviceReply: " + "Error commiting dataset!\n"); +#endif + return result; + } + + break; + } + default: + break; + } + return HasReturnvaluesIF::RETURN_OK; +} + +void Max31865PT1000Handler::debugInterface(uint8_t positionTracker, + object_id_t objectId, uint32_t parameter) { +} + +uint32_t Max31865PT1000Handler::getTransitionDelayMs( + Mode_t modeFrom, Mode_t modeTo) { + return 5000; +} + +ReturnValue_t Max31865PT1000Handler::getSwitches( + const uint8_t **switches, uint8_t *numberOfSwitches) { + return DeviceHandlerBase::NO_SWITCH; +} + +void Max31865PT1000Handler::doTransition(Mode_t modeFrom, + Submode_t subModeFrom) { + DeviceHandlerBase::doTransition(modeFrom, subModeFrom); +} + +ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(Max31865Definitions::PoolIds::TEMPERATURE_C, + new PoolEntry({0}, 1, true)); + localDataPoolMap.emplace(Max31865Definitions::PoolIds::FAULT_BYTE, + new PoolEntry({0})); + poolManager.subscribeForPeriodicPacket(sensorDatasetSid, + false, 4.0, false); + return HasReturnvaluesIF::RETURN_OK; +} + +void Max31865PT1000Handler::modeChanged() { + internalState = InternalState::NONE; +} diff --git a/mission/devices/Max31865PT1000Handler.h b/mission/devices/Max31865PT1000Handler.h new file mode 100644 index 00000000..19780df2 --- /dev/null +++ b/mission/devices/Max31865PT1000Handler.h @@ -0,0 +1,104 @@ +#ifndef MISSION_DEVICES_MAX31865PT1000HANDLER_H_ +#define MISSION_DEVICES_MAX31865PT1000HANDLER_H_ + +#include + +#include +#include + +#include +#include +#include "devicedefinitions/Max31865Definitions.h" + +/** + * @brief Device Handler for the thermal sensors + * @details + * Documentation of devices: + * MAX31865 RTD converter: + * https://datasheets.maximintegrated.com/en/ds/MAX31865.pdf + * Pt1000 platinum resistance thermometers OMEGA F2020-1000-1/3B: + * https://br.omega.com/omegaFiles/temperature/pdf/F1500_F2000_F4000.pdf + * + * The thermal sensor values are measured using the MAX31865 RTD converter IC + * which creates digital values from the measured resistance of the Pt1000 + * devices which can be read with the SPI interface. + * Refer to the SOURCE system schematic for the exact setup and number + * of devices. + * + * @author R. Mueller + * @ingroup devices + */ +class Max31865PT1000Handler: public DeviceHandlerBase { +public: + Max31865PT1000Handler(object_id_t objectId, object_id_t comIF, + CookieIF * comCookie, uint8_t switchId); + virtual~ Max31865PT1000Handler(); + + // Configuration in 8 digit code: + // 1. 1 for V_BIAS enabled, 0 for disabled + // 2. 1 for Auto-conversion, 0 for off + // 3. 1 for 1-shot enabled, 0 for disabled + // 4. 1 for 3-wire disabled, 0 for disabled + // 5./6. Fault detection: 00 for no action, 01 for automatic delay, 1 + // 0 for run fault detection with manual delay, + // 11 for finish fault detection with manual delay + // 7. Fault status: 1 for auto-clear, 0 for auto-clear off + // 8. 1 for 50 Hz filter, 0 for 60 Hz filter (noise rejection filter) + static constexpr uint8_t DEFAULT_CONFIG = 0b11000001; + + static constexpr float RTD_RREF_PT1000 = 4000.0; //!< Ohm + static constexpr float RTD_RESISTANCE0_PT1000 = 1000.0; //!< Ohm +protected: + /* DeviceHandlerBase abstract function implementation */ + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t * commandData, size_t commandDataLen) override; + void fillCommandAndReplyMap() override; + ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t getSwitches(const uint8_t **switches, + uint8_t *numberOfSwitches) override; + + void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; + + void debugInterface(uint8_t positionTracker = 0, + object_id_t objectId = 0, uint32_t parameter = 0) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override; + void modeChanged() override; + +private: + const uint8_t switchId; + + enum class InternalState { + NONE, + WARMUP, + CONFIGURE, + REQUEST_CONFIG, + RUNNING, + REQUEST_FAULT_BYTE + }; + + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + dur_millis_t startTime = 0; + uint8_t faultByte = 0; + std::array commandBuffer { 0 }; + + Max31865Definitions::Max31865Set sensorDataset; + sid_t sensorDatasetSid; + +#if OBSW_VERBOSE_LEVEL >= 1 + PeriodicOperationDivider* debugDivider; +#endif +}; + +#endif /* MISSION_DEVICES_MAX31865PT1000HANDLER_H_ */ + diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index 074a0b9f..5a6d62cb 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -20,7 +20,7 @@ ReturnValue_t PDU1Handler::buildNormalDeviceCommand( void PDU1Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) { parseHkTableReply(packet); -// handleDeviceTM(&pdu1HkTableDataset, id, true); + handleDeviceTM(&pdu1HkTableDataset, id, true); #if OBSW_VERBOSE_LEVEL >= 1 && PDU1_DEBUG == 1 pdu1HkTableDataset.read(); diff --git a/mission/devices/SyrlinksHkHandler.cpp b/mission/devices/SyrlinksHkHandler.cpp new file mode 100644 index 00000000..bab5e551 --- /dev/null +++ b/mission/devices/SyrlinksHkHandler.cpp @@ -0,0 +1,463 @@ +#include +#include +#include +#include +#include + +SyrlinksHkHandler::SyrlinksHkHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : + DeviceHandlerBase(objectId, comIF, comCookie), rxDataset(this), txDataset(this) { + if (comCookie == NULL) { + sif::error << "SyrlinksHkHandler: Invalid com cookie" << std::endl; + } +} + +SyrlinksHkHandler::~SyrlinksHkHandler() { +} + + +void SyrlinksHkHandler::doStartUp(){ + if(mode == _MODE_START_UP){ + setMode(MODE_ON); + } +} + +void SyrlinksHkHandler::doShutDown(){ + +} + +ReturnValue_t SyrlinksHkHandler::buildNormalDeviceCommand( + DeviceCommandId_t * id) { + switch (nextCommand) { + case(SYRLINKS::READ_RX_STATUS_REGISTERS): + *id = SYRLINKS::READ_RX_STATUS_REGISTERS; + nextCommand = SYRLINKS::READ_TX_STATUS; + break; + case(SYRLINKS::READ_TX_STATUS): + *id = SYRLINKS::READ_TX_STATUS; + nextCommand = SYRLINKS::READ_TX_WAVEFORM; + break; + case(SYRLINKS::READ_TX_WAVEFORM): + *id = SYRLINKS::READ_TX_WAVEFORM; + nextCommand = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE; + break; + case(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE): + *id = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE; + nextCommand = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE; + break; + case(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE): + *id = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE; + nextCommand = SYRLINKS::READ_RX_STATUS_REGISTERS; + break; + default: + sif::debug << "SyrlinksHkHandler::buildNormalDeviceCommand: rememberCommandId has invalid" + << "command id" << std::endl; + break; + } + return buildCommandFromCommand(*id, NULL, 0); +} + +ReturnValue_t SyrlinksHkHandler::buildTransitionDeviceCommand( + DeviceCommandId_t * id){ + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t * commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(SYRLINKS::RESET_UNIT): { + resetCommand.copy(reinterpret_cast(commandBuffer), resetCommand.size(), 0); + rawPacketLen = resetCommand.size(); + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::SET_TX_MODE_STANDBY): { + setTxModeStandby.copy(reinterpret_cast(commandBuffer), setTxModeStandby.size(), 0); + rawPacketLen = setTxModeStandby.size(); + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::SET_TX_MODE_MODULATION): { + setTxModeModulation.copy(reinterpret_cast(commandBuffer), setTxModeModulation.size(), 0); + rawPacketLen = setTxModeModulation.size(); + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::SET_TX_MODE_CW): { + setTxModeCw.copy(reinterpret_cast(commandBuffer), setTxModeCw.size(), 0); + rawPacketLen = setTxModeCw.size(); + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::READ_RX_STATUS_REGISTERS): { + readRxStatusRegCommand.copy(reinterpret_cast(commandBuffer), readRxStatusRegCommand.size(), 0); + rawPacketLen = readRxStatusRegCommand.size(); + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::READ_TX_STATUS): { + readTxStatus.copy(reinterpret_cast(commandBuffer), readTxStatus.size(), 0); + rawPacketLen = readTxStatus.size(); + rememberCommandId = SYRLINKS::READ_TX_STATUS; + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::READ_TX_WAVEFORM): { + readTxWaveform.copy(reinterpret_cast(commandBuffer), readTxStatus.size(), 0); + rawPacketLen = readTxWaveform.size(); + rememberCommandId = SYRLINKS::READ_TX_WAVEFORM; + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE): { + readTxAgcValueHighByte.copy(reinterpret_cast(commandBuffer), readTxStatus.size(), 0); + rawPacketLen = readTxAgcValueHighByte.size(); + rememberCommandId = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE; + rawPacket = commandBuffer; + return RETURN_OK; + } + case(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE): { + readTxAgcValueLowByte.copy(reinterpret_cast(commandBuffer), readTxStatus.size(), 0); + rawPacketLen = readTxAgcValueLowByte.size(); + rememberCommandId = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE; + rawPacket = commandBuffer; + return RETURN_OK; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +void SyrlinksHkHandler::fillCommandAndReplyMap() { + this->insertInCommandAndReplyMap(SYRLINKS::RESET_UNIT, 1, nullptr, SYRLINKS::ACK_SIZE, false, + true, SYRLINKS::ACK_REPLY); + this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_STANDBY, 1, nullptr, SYRLINKS::ACK_SIZE, + false, true, SYRLINKS::ACK_REPLY); + this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_MODULATION, 1, nullptr, + SYRLINKS::ACK_SIZE, false, true, SYRLINKS::ACK_REPLY); + this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_CW, 1, nullptr, SYRLINKS::ACK_SIZE, + false, true, SYRLINKS::ACK_REPLY); + this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_STATUS, 1, &txDataset, + SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_WAVEFORM, 1, &txDataset, + SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE, 1, &txDataset, + SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE, 1, &txDataset, + SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + this->insertInCommandAndReplyMap(SYRLINKS::READ_RX_STATUS_REGISTERS, 1, &rxDataset, + SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE); +} + +ReturnValue_t SyrlinksHkHandler::scanForReply(const uint8_t *start, + size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { + + ReturnValue_t result = RETURN_OK; + + if(*start != '<') { + sif::error << "SyrlinksHkHandler::scanForReply: Missing start frame character" << std::endl; + return MISSING_START_FRAME_CHARACTER; + } + + switch(*(start + 1)) { + case('A'): + *foundLen = SYRLINKS::ACK_SIZE; + *foundId = SYRLINKS::ACK_REPLY; + break; + case('E'): + *foundLen = SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE; + *foundId = SYRLINKS::READ_RX_STATUS_REGISTERS; + break; + case('R'): + *foundId = rememberCommandId; + *foundLen = SYRLINKS::READ_ONE_REGISTER_REPLY_SIE; + break; + default: + sif::error << "SyrlinksHkHandler::scanForReply: Unknown reply identifier" << std::endl; + result = IGNORE_REPLY_DATA; + break; + } + + return result; +} + +ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + + ReturnValue_t result; + + switch (id) { + case (SYRLINKS::ACK_REPLY): + result = verifyReply(packet, SYRLINKS::ACK_SIZE); + if (result != RETURN_OK) { + sif::error << "SyrlinksHkHandler::interpretDeviceReply: Acknowledgement reply has " + "invalid crc" << std::endl; + return CRC_FAILURE; + } + result = parseReplyStatus(reinterpret_cast(packet + SYRLINKS::MESSAGE_HEADER_SIZE)); + if (result != RETURN_OK) { + return result; + } + break; + case(SYRLINKS::READ_RX_STATUS_REGISTERS): + result = verifyReply(packet, SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE); + if (result != RETURN_OK) { + sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply " + << "has invalid crc" << std::endl; + return CRC_FAILURE; + } + parseRxStatusRegistersReply(packet); + break; + case(SYRLINKS::READ_TX_STATUS): + result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + if (result != RETURN_OK) { + sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx status reply " + << "has invalid crc" << std::endl; + return CRC_FAILURE; + } + parseTxStatusReply(packet); + break; + case(SYRLINKS::READ_TX_WAVEFORM): + result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + if (result != RETURN_OK) { + sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx waveform reply " + << "has invalid crc" << std::endl; + return CRC_FAILURE; + } + parseTxWaveformReply(packet); + break; + case(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE): + result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + if (result != RETURN_OK) { + sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC high byte reply " + << "has invalid crc" << std::endl; + return CRC_FAILURE; + } + parseAgcHighByte(packet); + break; + case(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE): + result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE); + if (result != RETURN_OK) { + sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC low byte reply " + << "has invalid crc" << std::endl; + return CRC_FAILURE; + } + parseAgcLowByte(packet); + break; + default: { + sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Unknown device reply id" + << std::endl; + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + } + + return RETURN_OK; +} + +LocalPoolDataSetBase* SyrlinksHkHandler::getDataSetHandle(sid_t sid) { + if (sid == rxDataset.getSid()) { + return &rxDataset; + } + else if (sid== txDataset.getSid()) { + return &txDataset; + } + else { + sif::error << "SyrlinksHkHandler::getDataSetHandle: Invalid sid" << std::endl; + return nullptr; + } +} + +std::string SyrlinksHkHandler::convertUint16ToHexString(uint16_t intValue) { + std::stringstream stream; + stream << std::setfill('0') << std::setw(4) << std::hex << std::uppercase << intValue; + return stream.str(); +} + +uint8_t SyrlinksHkHandler::convertHexStringToUint8(const char* twoChars) { + uint32_t value; + std::string hexString(twoChars, 2); + std::stringstream stream; + stream << std::hex << hexString; + stream >> value; + return static_cast(value); +} + +uint16_t SyrlinksHkHandler::convertHexStringToUint16(const char* fourChars) { + uint16_t value = 0; + value = convertHexStringToUint8(fourChars) << 8 | convertHexStringToUint8(fourChars+2); + return value; +} + +uint32_t SyrlinksHkHandler::convertHexStringToUint32(const char* characters, uint8_t numberOfChars) { + + uint32_t value = 0; + + switch (numberOfChars) { + case 6: + value = convertHexStringToUint8(characters) << 16 + | convertHexStringToUint8(characters + 2) << 8 + | convertHexStringToUint8(characters + 4); + return value; + case 8: + value = convertHexStringToUint8(characters) << 24 + | convertHexStringToUint8(characters + 2) << 16 + | convertHexStringToUint8(characters + 4) << 8 + | convertHexStringToUint8(characters + 4); + return value; + default: + sif::debug << "SyrlinksHkHandler::convertHexStringToUint32: Invalid number of characters. " + << "Must be either 6 or 8" << std::endl; + return 0; + } +} + +ReturnValue_t SyrlinksHkHandler::parseReplyStatus(const char* status) { + switch (*status) { + case '0': + return RETURN_OK; + case '1': + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Uart faming or parity error" + << std::endl; + return UART_FRAMIN_OR_PARITY_ERROR_ACK; + case '2': + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad character detected" << std::endl; + return BAD_CHARACTER_ACK; + case '3': + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad parameter value (unexpected value " + << "detected" << std::endl; + return BAD_PARAMETER_VALUE_ACK; + case '4': + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad end of frame" << std::endl; + return BAD_END_OF_FRAME_ACK; + case '5': + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Unknown command id or attempt to access" + << " a protected register" << std::endl; + return UNKNOWN_COMMAND_ID_ACK; + case '6': + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad CRC" << std::endl; + return BAD_CRC_ACK; + default: + sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply contains an invalid " + << "status id" << std::endl; + return RETURN_FAILED; + } +} + +ReturnValue_t SyrlinksHkHandler::verifyReply(const uint8_t* packet, uint8_t size) { + int result = 0; + /* Calculate crc from received packet */ + uint16_t crc = CRC::crc16ccitt(packet, size - SYRLINKS::SIZE_CRC_AND_TERMINATION, + CRC_INITIAL_VALUE); + std::string recalculatedCrc = convertUint16ToHexString(crc); + + const char* startOfCrc = reinterpret_cast(packet + size - SYRLINKS::SIZE_CRC_AND_TERMINATION); + const char* endOfCrc = reinterpret_cast(packet + size - 1); + + std::string replyCrc(startOfCrc, endOfCrc); + + result = recalculatedCrc.compare(replyCrc); + if (result != 0) { + return RETURN_FAILED; + } + return RETURN_OK; +} + +void SyrlinksHkHandler::parseRxStatusRegistersReply(const uint8_t* packet) { + PoolReadGuard readHelper(&rxDataset); + uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE; + rxDataset.rxStatus = convertHexStringToUint8(reinterpret_cast(packet + offset)); + offset += 2; + rxDataset.rxSensitivity = convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxFrequencyShift = convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxIqPower = convertHexStringToUint16(reinterpret_cast(packet + offset)); + offset += 4; + rxDataset.rxAgcValue = convertHexStringToUint16(reinterpret_cast(packet + offset)); + offset += 4; + offset += 2; // reserved register + rxDataset.rxDemodEb= convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxDemodN0= convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxDataRate = convertHexStringToUint8(reinterpret_cast(packet + offset)); + +#if OBSW_VERBOSE_LEVEL >= 1 && SYRLINKS_DEBUG == 1 + sif::info << "Syrlinks RX Status: 0x" << std::hex << (unsigned int)rxDataset.rxStatus.value << std::endl; + sif::info << "Syrlinks RX Sensitivity: " << std::dec << rxDataset.rxSensitivity << std::endl; + sif::info << "Syrlinks RX Frequency Shift: " << rxDataset.rxFrequencyShift << std::endl; + sif::info << "Syrlinks RX IQ Power: " << rxDataset.rxIqPower << std::endl; + sif::info << "Syrlinks RX AGC Value: " << rxDataset.rxAgcValue << std::endl; + sif::info << "Syrlinks RX Demod Eb: " << rxDataset.rxDemodEb << std::endl; + sif::info << "Syrlinks RX Demod N0: " << rxDataset.rxDemodN0 << std::endl; + sif::info << "Syrlinks RX Datarate: " << (unsigned int)rxDataset.rxDataRate.value << std::endl; +#endif +} + +void SyrlinksHkHandler::parseTxStatusReply(const uint8_t* packet) { + PoolReadGuard readHelper(&txDataset); + uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE; + txDataset.txStatus = convertHexStringToUint8(reinterpret_cast(packet + offset)); +#if OBSW_VERBOSE_LEVEL >= 1 && SYRLINKS_DEBUG == 1 + sif::info << "Syrlinks TX Status: 0x" << std::hex << (unsigned int) txDataset.txStatus.value + << std::endl; +#endif +} + +void SyrlinksHkHandler::parseTxWaveformReply(const uint8_t* packet) { + PoolReadGuard readHelper(&txDataset); + uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE; + txDataset.txWaveform = convertHexStringToUint8(reinterpret_cast(packet + offset)); +#if OBSW_VERBOSE_LEVEL >= 1 && SYRLINKS_DEBUG == 1 + sif::info << "Syrlinks TX Waveform: 0x" << std::hex << (unsigned int) txDataset.txWaveform.value + << std::endl; +#endif +} + +void SyrlinksHkHandler::parseAgcLowByte(const uint8_t* packet) { + PoolReadGuard readHelper(&txDataset); + uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE; + txDataset.txAgcValue = agcValueHighByte << 8 | convertHexStringToUint8(reinterpret_cast(packet + offset)); +#if OBSW_VERBOSE_LEVEL >= 1 && SYRLINKS_DEBUG == 1 + sif::info << "Syrlinks TX AGC Value: " << txDataset.txAgcValue << std::endl; +#endif +} + +void SyrlinksHkHandler::parseAgcHighByte(const uint8_t* packet) { + PoolReadGuard readHelper(&txDataset); + uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE; + agcValueHighByte = convertHexStringToUint8(reinterpret_cast(packet + offset)); +} + +void SyrlinksHkHandler::setNormalDatapoolEntriesInvalid(){ + +} + +uint32_t SyrlinksHkHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){ + return 500; +} + +ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + + localDataPoolMap.emplace(SYRLINKS::RX_STATUS, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_SENSITIVITY, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_FREQUENCY_SHIFT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_IQ_POWER, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_AGC_VALUE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_EB, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_N0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::RX_DATA_RATE, new PoolEntry( { 0 })); + + localDataPoolMap.emplace(SYRLINKS::TX_STATUS, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::TX_WAVEFORM, new PoolEntry( { 0 })); + localDataPoolMap.emplace(SYRLINKS::TX_AGC_VALUE, new PoolEntry( { 0 })); + + return HasReturnvaluesIF::RETURN_OK; +} + +void SyrlinksHkHandler::setModeNormal() { + mode = MODE_NORMAL; +} + diff --git a/mission/devices/SyrlinksHkHandler.h b/mission/devices/SyrlinksHkHandler.h new file mode 100644 index 00000000..c194fd18 --- /dev/null +++ b/mission/devices/SyrlinksHkHandler.h @@ -0,0 +1,178 @@ +#ifndef MISSION_DEVICES_SYRLINKSHKHANDLER_H_ +#define MISSION_DEVICES_SYRLINKSHKHANDLER_H_ + +#include +#include +#include + +/** + * @brief This is the device handler for the syrlinks transceiver. It handles the command + * transmission and reading of housekeeping data via the housekeeping interface. The + * transmission of telemetry and the reception of telecommands is handled by an additional + * class. + * + * @author J. Meier + */ +class SyrlinksHkHandler: public DeviceHandlerBase { +public: + + SyrlinksHkHandler(object_id_t objectId, object_id_t comIF, + CookieIF * comCookie); + virtual ~SyrlinksHkHandler(); + + /** + * @brief Sets mode to MODE_NORMAL. Can be used for debugging. + */ + void setModeNormal(); + +protected: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override; + void fillCommandAndReplyMap() override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t * commandData,size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + void setNormalDatapoolEntriesInvalid() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; + +private: + + static const uint8_t INTERFACE_ID = CLASS_ID::SYRLINKS_HANDLER; + + static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t UART_FRAMIN_OR_PARITY_ERROR_ACK = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t BAD_CHARACTER_ACK = MAKE_RETURN_CODE(0xA2); + static const ReturnValue_t BAD_PARAMETER_VALUE_ACK = MAKE_RETURN_CODE(0xA3); + static const ReturnValue_t BAD_END_OF_FRAME_ACK = MAKE_RETURN_CODE(0xA4); + static const ReturnValue_t UNKNOWN_COMMAND_ID_ACK = MAKE_RETURN_CODE(0xA5); + static const ReturnValue_t BAD_CRC_ACK = MAKE_RETURN_CODE(0xA6); + static const ReturnValue_t REPLY_WRONG_SIZE = MAKE_RETURN_CODE(0xA7); + static const ReturnValue_t MISSING_START_FRAME_CHARACTER = MAKE_RETURN_CODE(0xA8); + + static const uint8_t CRC_INITIAL_VALUE = 0x0; + + std::string resetCommand = ""; + std::string readRxStatusRegCommand = ""; + std::string setTxModeStandby = ""; + /** W - write, 04 - 4 bytes in data field, 01 - value, 40 register to write value */ + std::string setTxModeModulation = ""; + std::string setTxModeCw = ""; + std::string readTxStatus = ""; + std::string readTxWaveform = ""; + std::string readTxAgcValueHighByte = ""; + std::string readTxAgcValueLowByte = ""; + + /** + * In some cases it is not possible to extract from the received reply the information about + * the associated command. This variable is thus used to remember the command id. + */ + DeviceCommandId_t rememberCommandId = SYRLINKS::NONE; + + SYRLINKS::RxDataset rxDataset; + SYRLINKS::TxDataset txDataset; + + uint8_t agcValueHighByte; + + uint8_t commandBuffer[SYRLINKS::MAX_COMMAND_SIZE]; + + /** + * This object is used to store the id of the next command to execute. This controls the + * read out of multiple registers which can not be fetched with one single command. + */ + DeviceCommandId_t nextCommand = SYRLINKS::READ_RX_STATUS_REGISTERS; + + /** + * @brief This function converts an uint16_t into its hexadecimal string representation. + * + * @param intValue The value to convert. + * + * @return An std::string object containing the hex representation of intValue. + */ + std::string convertUint16ToHexString(uint16_t intValue); + + /** + * @brief This function converts a hex number represented by to chars to an 8-bit integer. + * + * @param twoChars Pointer to the two characters representing the hex value. + * + * @details E.g. when twoChars points to an array with the two characters "A5" then the function + * will return 0xA5. + * @return The converted integer. + */ + uint8_t convertHexStringToUint8(const char* twoChars); + + /** + * @brief This function converts a hex number represented by 4 chars to an uint16_t. + * + * @param Pointer to the fourCharacters representing the 16-bit integer. + * + * @return The uint16_t result. + */ + uint16_t convertHexStringToUint16(const char* fourChars); + + /** + * @brief Function converts a hex number represented by 6 or 8 characters to an uint32_t. + * + * @param characters Pointer to the hex characters array. + * @param numberOfChars Number of characters representing the hex value. Must be 6 or 8. + * + * @return The uint32_t value. + */ + uint32_t convertHexStringToUint32(const char* characters, uint8_t numberOfChars); + + /** + * @brief This function parses the status reply + * @param status Pointer to the status information. + * + * @details Some commands reply with a status message giving information about the preceding + * command transmission and/or execution was successful. + */ + ReturnValue_t parseReplyStatus(const char* status); + + /** + * @brief Function verifies the received reply from the syrlinks by recalculating and + * comparing the crc. + * + * @param packet Pointer to the received reply. + * @param size Size of the whole packet including the crc and the packet termination + * character '>'. + * + * @return RETURN_OK if successful, otherwise RETURN_FAILED. + */ + ReturnValue_t verifyReply(const uint8_t* packet, uint8_t size); + + /** + * @brief This function extracts the data from a rx status registers reply and writes the + * information to the status registers dataset. + * @param packet Pointer to the reply packet. + */ + void parseRxStatusRegistersReply(const uint8_t* packet); + + /** + * @brief This function writes the read tx status register to the txStatusDataset. + * @param packet Pointer to the received packet. + */ + void parseTxStatusReply(const uint8_t* packet); + + /** + * @brief This function writes the received waveform configuration to the txDataset. + */ + void parseTxWaveformReply(const uint8_t* packet); + + /** + * @brief The agc value is split over two registers. The parse agc functions are used to get + * the values from the received reply and write them into the txDataset. + */ + void parseAgcLowByte(const uint8_t* packet); + void parseAgcHighByte(const uint8_t* packet); +}; + +#endif /* MISSION_DEVICES_SYRLINKSHKHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h new file mode 100644 index 00000000..23ff8b9a --- /dev/null +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -0,0 +1,127 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ + +namespace IMTQ { + + static const DeviceCommandId_t NONE = 0x0; + static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1; + static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2; + + static const uint8_t GET_TEMP_REPLY_SIZE = 2; + static const uint8_t CFGR_CMD_SIZE = 3; + static const uint8_t POINTER_REG_SIZE = 1; + + static const uint32_t ENG_HK_DATA_SET_ID = GET_ENG_HK_DATA; + static const uint8_t SIZE_ENG_HK_COMMAND = 1; + static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24; + + static const uint8_t MAX_REPLY_SIZE = SIZE_ENG_HK_DATA_REPLY; + static const uint8_t MAX_COMMAND_SIZE = 9; + + static const uint8_t POOL_ENTRIES = 11; + + /** + * Command code definitions. Each command or reply of an IMTQ request will begin with one of + * the following command codes. + */ + namespace CC { + static const uint8_t START_ACTUATION_DIPOLE = 0x6; + static const uint8_t SOFTWARE_RESET = 0xAA; + static const uint8_t GET_ENG_HK_DATA = 0x4A; + }; + + enum IMTQPoolIds: lp_id_t { + DIGITAL_VOLTAGE_MV, + ANALOG_VOLTAGE_MV, + DIGITAL_CURRENT_A, + ANALOG_CURRENT_A, + COIL_X_CURRENT_A, + COIL_Y_CURRENT_A, + COIL_Z_CURRENT_A, + COIL_X_TEMPERATURE, + COIL_Y_TEMPERATURE, + COIL_Z_TEMPERATURE, + MCU_TEMPERATURE + }; + +class EngHkDataset: + public StaticLocalDataSet { +public: + + EngHkDataset(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) { + } + + EngHkDataset(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) { + } + + lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, + DIGITAL_VOLTAGE_MV, this); + lp_var_t analogVoltageMv = lp_var_t(sid.objectId, + ANALOG_VOLTAGE_MV, this); + lp_var_t digitalCurrentA = lp_var_t(sid.objectId, + DIGITAL_CURRENT_A, this); + lp_var_t analogCurrentA = lp_var_t(sid.objectId, + ANALOG_CURRENT_A, this); + lp_var_t coilXcurrentA = lp_var_t(sid.objectId, + COIL_X_CURRENT_A, this); + lp_var_t coilYcurrentA = lp_var_t(sid.objectId, + COIL_Y_CURRENT_A, this); + lp_var_t coilZcurrentA = lp_var_t(sid.objectId, + COIL_Z_CURRENT_A, this); + /** All temperatures in [°C] */ + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + COIL_X_TEMPERATURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + COIL_Z_TEMPERATURE, this); + lp_var_t mcuTemperature = lp_var_t(sid.objectId, + MCU_TEMPERATURE, this); +}; + +/** + * @brief This class can be used to ease the generation of an action message commanding the + * IMTQHandler to configure the magnettorquer with the desired dipoles. + * + * @details Deserialize the packet, write the deserialized data to the ipc store and store the + * the ipc store address in the action message. + */ +class CommandDipolePacket : public SerialLinkedListAdapter { +public: + + CommandDipolePacket() { + setLinks(); + } + +private: + + /** + * @brief Constructor + * + * @param xDipole The dipole of the x coil in 10^-4*Am^2 + * @param yDipole The dipole of the y coil in 10^-4*Am^2 + * @param zDipole The dipole of the z coil in 10^-4*Am^2 + * @param duration The duration in milliseconds the dipole will be generated by the coils. + * When set to 0, the dipole will be generated until a new dipole actuation + * command is sent. + */ + CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole, uint16_t duration) : + xDipole(xDipole), yDipole(yDipole), zDipole(zDipole), duration(duration) { + } + void setLinks() { + setStart(&xDipole); + xDipole.setNext(&yDipole); + yDipole.setNext(&zDipole); + zDipole.setNext(&duration); + } + SerializeElement xDipole; + SerializeElement yDipole; + SerializeElement zDipole; + SerializeElement duration; +}; +} + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/Max31865Definitions.h b/mission/devices/devicedefinitions/Max31865Definitions.h new file mode 100644 index 00000000..d21f41e4 --- /dev/null +++ b/mission/devices/devicedefinitions/Max31865Definitions.h @@ -0,0 +1,54 @@ +#ifndef MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_ +#define MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_ + +#include +#include +#include +#include + +namespace Max31865Definitions { + +enum PoolIds: lp_id_t { + TEMPERATURE_C, + FAULT_BYTE +}; + +static constexpr DeviceCommandId_t CONFIG_CMD = 0x80; +static constexpr DeviceCommandId_t REQUEST_CONFIG = 0x00; +static constexpr DeviceCommandId_t REQUEST_RTD = 0x01; +static constexpr DeviceCommandId_t REQUEST_FAULT_BYTE = 0x07; + +static constexpr uint32_t MAX31865_SET_ID = REQUEST_RTD; + +static constexpr size_t MAX_REPLY_SIZE = 5; + +class Max31865Set: + public StaticLocalDataSet { +public: + /** + * Constructor used by owner and data creators like device handlers. + * @param owner + * @param setId + */ + Max31865Set(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, MAX31865_SET_ID) { + } + + /** + * Constructor used by data users like controllers. + * @param sid + */ + Max31865Set(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, MAX31865_SET_ID)) { + } + + lp_var_t temperatureCelcius = lp_var_t(sid.objectId, + PoolIds::TEMPERATURE_C, this); + lp_var_t errorByte = lp_var_t(sid.objectId, + PoolIds::FAULT_BYTE, this); +}; + +} + +#endif /* MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_ */ + diff --git a/mission/devices/devicedefinitions/SyrlinksDefinitions.h b/mission/devices/devicedefinitions/SyrlinksDefinitions.h new file mode 100644 index 00000000..7aa75b99 --- /dev/null +++ b/mission/devices/devicedefinitions/SyrlinksDefinitions.h @@ -0,0 +1,101 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_SYRLINKSDEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_SYRLINKSDEFINITIONS_H_ + + +namespace SYRLINKS { + + static const DeviceCommandId_t NONE = 0x0; + static const DeviceCommandId_t RESET_UNIT = 0x01; + /** Reads out all status registers */ + static const DeviceCommandId_t READ_RX_STATUS_REGISTERS = 0x02; + /** Sets Tx mode to standby */ + static const DeviceCommandId_t SET_TX_MODE_STANDBY = 0x03; + /** Starts transmission mode. Only reached when clock signal is applying to the data tx input */ + static const DeviceCommandId_t SET_TX_MODE_MODULATION = 0x04; + /** Sends out a single carrier wave for testing purpose */ + static const DeviceCommandId_t SET_TX_MODE_CW = 0x05; + static const DeviceCommandId_t ACK_REPLY = 0x06; + static const DeviceCommandId_t READ_TX_STATUS = 0x07; + static const DeviceCommandId_t READ_TX_WAVEFORM = 0x08; + static const DeviceCommandId_t READ_TX_AGC_VALUE_HIGH_BYTE = 0x09; + static const DeviceCommandId_t READ_TX_AGC_VALUE_LOW_BYTE = 0x0A; + + /** Size of a simple transmission success response */ + static const uint8_t ACK_SIZE = 11; + static const uint8_t SIZE_CRC_AND_TERMINATION = 5; + /** The size of the header with the message identifier and the payload size field */ + static const uint8_t MESSAGE_HEADER_SIZE = 5; + /** Size of reply to an rx status registers request */ + static const uint8_t RX_STATUS_REGISTERS_REPLY_SIZE = 49; + static const uint8_t READ_ONE_REGISTER_REPLY_SIE = 13; + + static const uint8_t RX_DATASET_ID = 0x1; + static const uint8_t TX_DATASET_ID = 0x2; + + static const size_t MAX_REPLY_SIZE = RX_STATUS_REGISTERS_REPLY_SIZE; + static const size_t MAX_COMMAND_SIZE = 15; + + static const size_t CRC_FIELD_SIZE = 4; + + static const uint8_t RX_DATASET_SIZE = 8; + static const uint8_t TX_DATASET_SIZE = 3; + + enum SyrlinksPoolIds: lp_id_t { + RX_STATUS, + RX_SENSITIVITY, + RX_FREQUENCY_SHIFT, + RX_IQ_POWER, + RX_AGC_VALUE, + RX_DEMOD_EB, + RX_DEMOD_N0, + RX_DATA_RATE, + TX_STATUS, + TX_CONV_DIFF, + TX_CONV_FILTER, + TX_WAVEFORM, + TX_PCM_INDEX, + TX_AGC_VALUE, + }; + +class RxDataset: public StaticLocalDataSet { +public: + + RxDataset(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, RX_DATASET_ID) { + } + + RxDataset(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, RX_DATASET_ID)) { + } + + lp_var_t rxStatus = lp_var_t(sid.objectId, RX_STATUS, this); + lp_var_t rxSensitivity = lp_var_t(sid.objectId, RX_SENSITIVITY, this); + lp_var_t rxFrequencyShift = lp_var_t(sid.objectId, RX_FREQUENCY_SHIFT, this); + lp_var_t rxIqPower = lp_var_t(sid.objectId, RX_IQ_POWER, this); + lp_var_t rxAgcValue = lp_var_t(sid.objectId, RX_AGC_VALUE, this); + lp_var_t rxDemodEb = lp_var_t(sid.objectId, RX_DEMOD_EB, this); + lp_var_t rxDemodN0 = lp_var_t(sid.objectId, RX_DEMOD_N0, this); + lp_var_t rxDataRate = lp_var_t(sid.objectId, RX_DATA_RATE, this); +}; + + +class TxDataset: public StaticLocalDataSet { +public: + + TxDataset(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, TX_DATASET_ID) { + } + + TxDataset(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, TX_DATASET_ID)) { + } + + lp_var_t txStatus = lp_var_t(sid.objectId, TX_STATUS, this); + lp_var_t txWaveform = lp_var_t(sid.objectId, TX_WAVEFORM, this); + lp_var_t txAgcValue = lp_var_t(sid.objectId, TX_AGC_VALUE, this); +}; + +} + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_SYRLINKSDEFINITIONS_H_ */ diff --git a/mission/utility/TmFunnel.cpp b/mission/utility/TmFunnel.cpp index 263aff8d..a567cbd9 100644 --- a/mission/utility/TmFunnel.cpp +++ b/mission/utility/TmFunnel.cpp @@ -1,7 +1,7 @@ #include -#include #include #include +#include #include object_id_t TmFunnel::downlinkDestination = objects::NO_OBJECT; @@ -50,7 +50,7 @@ ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) { if(result != HasReturnvaluesIF::RETURN_OK){ return result; } - TmPacketBase packet(packetData); + TmPacketPusC packet(packetData); packet.setPacketSequenceCount(this->sourceSequenceCount); sourceSequenceCount++; sourceSequenceCount = sourceSequenceCount % diff --git a/test/testtasks/TestTask.cpp b/test/testtasks/TestTask.cpp index 86f4af38..88a5d13e 100644 --- a/test/testtasks/TestTask.cpp +++ b/test/testtasks/TestTask.cpp @@ -66,6 +66,14 @@ gps_rx_data[] = "" "$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n" "$GPRMB,A,,,,,,,,,,,,V*71\r\n"; +const char hyperion_gps_data[] = "" + "$GNGGA,173225.998892,4908.5596,N,00906.2765,E,1,05,2.1,215.0,M,48.2,M,,0000*74\r\n" + "$GNGLL,4908.5596,N,00906.2765,E,173225.998892,A,A*7F\r\n" + "$GPGSA,A,3,18,16,26,31,20,,,,,,,,3.2,2.1,2.4*3C\r\n" + "$GNRMC,173225.998892,A,4908.5596,N,00906.2765,E,000.0,040.7,270221,,,A*4F\r\n" + "$GNVTG,040.7,T,,M,000.0,N,000.0,K,A*10\r\n" + "$GNZDA,173225.998892,27,02,2021,00,00*75\r\n"; + ReturnValue_t TestTask::performOneShotAction() { #if OBSW_ADD_TEST_CODE == 1 //performLwgpsTest(); @@ -93,18 +101,18 @@ ReturnValue_t TestTask::performActionB() { void TestTask::performLwgpsTest() { /* Everything here will only be performed once. */ - etl::vector testVec; + sif::info << "Processing sample GPS output.." << std::endl; lwgps_t gpsStruct; sif::info << "Size of GPS struct: " << sizeof(gpsStruct) << std::endl; lwgps_init(&gpsStruct); /* Process all input data */ - lwgps_process(&gpsStruct, gps_rx_data, strlen(gps_rx_data)); + lwgps_process(&gpsStruct, hyperion_gps_data, strlen(hyperion_gps_data)); /* Print messages */ - printf("Valid status: %d\r\n", gpsStruct.is_valid); - printf("Latitude: %f degrees\r\n", gpsStruct.latitude); - printf("Longitude: %f degrees\r\n", gpsStruct.longitude); - printf("Altitude: %f meters\r\n", gpsStruct.altitude); + printf("Valid status: %d\n", gpsStruct.is_valid); + printf("Latitude: %f degrees\n", gpsStruct.latitude); + printf("Longitude: %f degrees\n", gpsStruct.longitude); + printf("Altitude: %f meters\n", gpsStruct.altitude); } diff --git a/tmtc b/tmtc index 5f1803b6..7cc06ef0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 5f1803b66393210ded8c5d88fbc28cd8130cef91 +Subproject commit 7cc06ef0e0882d286bab8156ca756e0211e5ebae