diff --git a/CHANGELOG.md b/CHANGELOG.md index 8b1b2ed5..1fdc3e1f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- Move ACS board polling to separate worker thread. + # [v1.32.0] eive-tmtc: v2.16.1 diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 59a17207..c28806bc 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -1,8 +1,10 @@ #include "ObjectFactory.h" #include +#include #include #include +#include #include #include "OBSWConfig.h" @@ -57,6 +59,7 @@ #if OBSW_TEST_LIBGPIOD == 1 #include "linux/boardtest/LibgpiodTest.h" #endif +#include #include #include #include @@ -83,7 +86,6 @@ #include "mission/core/GenericFactory.h" #include "mission/devices/ACUHandler.h" #include "mission/devices/BpxBatteryHandler.h" -#include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" #include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/P60DockHandler.h" @@ -239,8 +241,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF, return returnvalue::OK; } -void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, - PowerSwitchIF& pwrSwitcher) { +void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, + SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); @@ -343,6 +345,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo static_cast(fdir); #if OBSW_ADD_ACS_BOARD == 1 + new AcsBoardPolling(objects::ACS_BOARD_POLLING_TASK, spiComIF, *gpioComIF); std::string spiDev = q7s::SPI_DEFAULT_DEV; std::array assemblyChildren; SpiCookie* spiCookie = @@ -414,12 +417,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo // Commented until ACS board V2 in in clean room again // Gyro 0 Side A spiCookie = - new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE, + new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); auto adisHandler = - new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, - ADIS1650X::Type::ADIS16505); + new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, adis1650x::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[4] = adisHandler; @@ -431,11 +434,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo adisHandler->enablePeriodicPrintouts(true, 10); #endif // Gyro 1 Side A - spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, + spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, l3gd20h::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto gyroL3gHandler1 = new GyroHandlerL3GD20H( - objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler1 = + new GyrL3gCustomHandler(objects::GYRO_1_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER); gyroL3gHandler1->setCustomFdir(fdir); assemblyChildren[5] = gyroL3gHandler1; @@ -448,11 +452,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo #endif // Gyro 2 Side B spiCookie = - new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE, + new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, - spiCookie, ADIS1650X::Type::ADIS16505); + adisHandler = + new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, adis1650x::Type::ADIS16505); fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); adisHandler->setCustomFdir(fdir); assemblyChildren[6] = adisHandler; @@ -461,11 +466,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo adisHandler->setToGoToNormalModeImmediately(); #endif // Gyro 3 Side B - spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE, + spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, l3gd20h::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT); - auto gyroL3gHandler3 = new GyroHandlerL3GD20H( - objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + auto gyroL3gHandler3 = + new GyrL3gCustomHandler(objects::GYRO_3_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK, + spiCookie, spi::L3G_TRANSITION_DELAY); fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER); gyroL3gHandler3->setCustomFdir(fdir); assemblyChildren[7] = gyroL3gHandler3; diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index 2118e3d7..7ec6c871 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -2,6 +2,7 @@ #define BSP_Q7S_OBJECTFACTORY_H_ #include +#include #include #include #include @@ -31,7 +32,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); void createTmpComponents(); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); -void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, +void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable, HeaterHandler*& heaterHandler); diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 7647af8b..7bb2ceec 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -194,6 +194,15 @@ void scheduling::initTasks() { scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); } +#if OBSW_ADD_ACS_BOARD == 1 + PeriodicTaskIF* acsBrdPolling = factory->createPeriodicTask( + "ACS_BOARD_POLLING", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + result = acsBrdPolling->addComponent(objects::ACS_BOARD_POLLING_TASK); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("ACS_BOARD_POLLING", objects::ACS_BOARD_POLLING_TASK); + } +#endif + #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = factory->createPeriodicTask( "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); @@ -351,6 +360,9 @@ void scheduling::initTasks() { #if OBSW_ADD_SA_DEPL == 1 solarArrayDeplTask->startTask(); #endif +#if OBSW_ADD_ACS_BOARD == 1 + acsBrdPolling->startTask(); +#endif #if OBSW_ADD_MGT == 1 imtqPolling->startTask(); #endif diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 36e9df64..e05e8098 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -43,7 +43,7 @@ void ObjectFactory::produce(void* args) { #endif #if OBSW_ADD_ACS_BOARD == 1 - createAcsBoardComponents(gpioComIF, uartComIF, *pwrSwitcher); + createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher); #endif HeaterHandler* heaterHandler; createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler); diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h index 08f07d3d..c7c7cce9 100644 --- a/common/config/eive/objects.h +++ b/common/config/eive/objects.h @@ -123,7 +123,9 @@ enum commonObjects : uint32_t { // CCSDS_IP_CORE_BRIDGE = 0x73500000, /* 0x49 ('I') for Communication Interfaces */ - SPI_RTD_COM_IF = 0x49020006, + ACS_BOARD_POLLING_TASK = 0x49060004, + RW_POLLING_TASK = 0x49060005, + SPI_RTD_COM_IF = 0x49060006, // 0x60 for other stuff HEATER_0_PLOC_PROC_BRD = 0x60000000, diff --git a/dummies/GyroAdisDummy.cpp b/dummies/GyroAdisDummy.cpp index 050c838d..ed581138 100644 --- a/dummies/GyroAdisDummy.cpp +++ b/dummies/GyroAdisDummy.cpp @@ -1,6 +1,6 @@ #include "GyroAdisDummy.h" -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {} @@ -40,13 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({-0.5}, true)); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.2}, true)); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry({-0.5}, true)); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry({0.2}, true)); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); + localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/dummies/GyroAdisDummy.h b/dummies/GyroAdisDummy.h index b3aad620..21de1eeb 100644 --- a/dummies/GyroAdisDummy.h +++ b/dummies/GyroAdisDummy.h @@ -2,8 +2,7 @@ #define DUMMIES_GYROADISDUMMY_H_ #include - -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include class GyroAdisDummy : public DeviceHandlerBase { public: diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index 20309c63..8c89f59d 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -1,6 +1,6 @@ #include "GyroL3GD20Dummy.h" -#include "fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h" +#include GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} @@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({1.2}, true)); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.7}, true)); - localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({1.2}, true)); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.7}, true)); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } diff --git a/fsfw b/fsfw index bdfe31db..cf735143 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit bdfe31dba48039b60fe700e7d03bfb95e9549688 +Subproject commit cf735143fe4b79db2fc7faba2b1cd239474e2cfc diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp new file mode 100644 index 00000000..9803dfe9 --- /dev/null +++ b/linux/devices/AcsBoardPolling.cpp @@ -0,0 +1,453 @@ +#include "AcsBoardPolling.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "devices/gpioIds.h" + +using namespace returnvalue; + +AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF) + : SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) { + while (true) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + semaphore->acquire(); + // Give all tasks or the PST some time to submit all consecutive requests. + TaskFactory::delayTask(2); + gyroAdisHandler(gyro0Adis); + gyroAdisHandler(gyro2Adis); + gyroL3gHandler(gyro1L3g); + gyroL3gHandler(gyro3L3g); + // To prevent task being not reactivated by tardy tasks + TaskFactory::delayTask(20); + } + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; } + +ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::MGM_0_LIS3_CS): { + mgm0L3Cookie = spiCookie; + break; + } + case (gpioIds::MGM_1_RM3100_CS): { + mgm1Rm3100Cookie = spiCookie; + break; + } + case (gpioIds::MGM_2_LIS3_CS): { + mgm2L3Cookie = spiCookie; + break; + } + case (gpioIds::MGM_3_RM3100_CS): { + mgm3Rm3100Cookie = spiCookie; + break; + } + case (gpioIds::GYRO_0_ADIS_CS): { + gyro0Adis.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_1_L3G_CS): { + gyro1L3g.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_2_ADIS_CS): { + gyro2Adis.cookie = spiCookie; + break; + } + case (gpioIds::GYRO_3_L3G_CS): { + gyro3L3g.cookie = spiCookie; + break; + } + default: { + sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl; + } + } + return spiComIF.initializeInterface(cookie); +} + +ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + auto handleAdisRequest = [&](GyroAdis& adis) { + if (sendLen != sizeof(acs::Adis1650XRequest)) { + sif::error << "AcsBoardPolling: invalid adis request send length"; + adis.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (req->mode != adis.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + adis.type = req->type; + adis.countdown.setTimeout(adis1650x::START_UP_TIME); + adis.countdown.resetTimer(); + if (adis.type == adis1650x::Type::ADIS16507) { + adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507; + } else if (adis.type == adis1650x::Type::ADIS16505) { + adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505; + } else { + sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl; + } + adis.performStartup = true; + } else if (req->mode == acs::SimpleSensorMode::OFF) { + adis.performStartup = false; + adis.ownReply.cfgWasSet = false; + adis.ownReply.dataWasSet = false; + } + adis.mode = req->mode; + } + return returnvalue::OK; + }; + auto handleL3gRequest = [&](GyroL3g& gyro) { + if (sendLen != sizeof(acs::GyroL3gRequest)) { + sif::error << "AcsBoardPolling: invalid l3g request send length"; + gyro.replyResult = returnvalue::FAILED; + return returnvalue::FAILED; + } + auto* req = reinterpret_cast(sendData); + MutexGuard mg(ipcLock); + if (req->mode != gyro.mode) { + if (req->mode == acs::SimpleSensorMode::NORMAL) { + gyro.performStartup = true; + } else { + gyro.ownReply.cfgWasSet = false; + } + gyro.mode = req->mode; + } + return returnvalue::OK; + }; + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::GYRO_0_ADIS_CS): { + handleAdisRequest(gyro0Adis); + break; + } + case (gpioIds::GYRO_2_ADIS_CS): { + handleAdisRequest(gyro2Adis); + break; + } + case (gpioIds::GYRO_1_L3G_CS): { + handleL3gRequest(gyro1L3g); + break; + } + case (gpioIds::GYRO_3_L3G_CS): { + handleL3gRequest(gyro3L3g); + break; + } + } + MutexGuard mg(ipcLock); + if (state == InternalState::IDLE) { + state = InternalState::BUSY; + semaphore->release(); + } + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, + size_t* size) { + SpiCookie* spiCookie = dynamic_cast(cookie); + if (spiCookie == nullptr) { + return returnvalue::FAILED; + } + MutexGuard mg(ipcLock); + auto handleAdisReply = [&](GyroAdis& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::Adis1650XReply); + }; + auto handleL3gReply = [&](GyroL3g& gyro) { + std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply)); + *buffer = reinterpret_cast(&gyro.readerReply); + *size = sizeof(acs::GyroL3gReply); + }; + switch (spiCookie->getChipSelectPin()) { + case (gpioIds::GYRO_0_ADIS_CS): { + handleAdisReply(gyro0Adis); + return gyro0Adis.replyResult; + } + case (gpioIds::GYRO_2_ADIS_CS): { + handleAdisReply(gyro2Adis); + return gyro2Adis.replyResult; + } + case (gpioIds::GYRO_1_L3G_CS): { + handleL3gReply(gyro1L3g); + return gyro1L3g.replyResult; + } + case (gpioIds::GYRO_3_L3G_CS): { + handleL3gReply(gyro3L3g); + return gyro3L3g.replyResult; + } + } + return returnvalue::OK; +} + +void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { + ReturnValue_t result; + acs::SimpleSensorMode mode; + bool gyroPerformStartup; + { + MutexGuard mg(ipcLock); + mode = l3g.mode; + gyroPerformStartup = l3g.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL) { + if (gyroPerformStartup) { + cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK; + std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + // Ignore useless reply and red config + cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, 5); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::OK; + } + MutexGuard mg(ipcLock); + // Cross check configuration as verification that communication is working + for (uint8_t idx = 0; idx < 5; idx++) { + if (rawReply[idx + 1] != l3g.sensorCfg[idx]) { + sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl; + l3g.replyResult = returnvalue::FAILED; + return; + } + } + l3g.ownReply.cfgWasSet = true; + l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]); + } + cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + // The regular read function always returns the full sensor config as well. Use that + // to verify communications. + for (uint8_t idx = 0; idx < 5; idx++) { + if (rawReply[idx + 1] != l3g.sensorCfg[idx]) { + sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl; + l3g.replyResult = returnvalue::FAILED; + return; + } + } + l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX]; + l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L]; + l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L]; + l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L]; + l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX]; + } +} + +ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { + ReturnValue_t result = returnvalue::OK; + int retval = 0; + // Prepare transfer + int fileDescriptor = 0; + std::string device = spiComIF.getSpiDev(); + UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return SpiComIF::OPENING_FILE_FAILED; + } + spi::SpiModes spiMode = spi::SpiModes::MODE_0; + uint32_t spiSpeed = 0; + cookie.getSpiParameters(spiMode, spiSpeed, nullptr); + spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + cookie.assignWriteBuffer(cmdBuf.data()); + cookie.setTransferSize(2); + + gpioId_t gpioId = cookie.getChipSelectPin(); + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 0; + MutexIF* mutex = spiComIF.getCsMutex(); + cookie.getMutexParams(timeoutType, timeoutMs); + if (mutex == nullptr) { + sif::warning << "GyroADIS16507Handler::spiSendCallback: " + "Mutex or GPIO interface invalid" + << std::endl; + return returnvalue::FAILED; + } + + size_t idx = 0; + spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); + uint64_t origTx = transferStruct->tx_buf; + uint64_t origRx = transferStruct->rx_buf; + while (idx < transferLen) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl; +#endif + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullLow(gpioId); + } + + // Execute transfer + // Initiate a full duplex SPI transfer. + retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle()); + if (retval < 0) { + utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); + result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; + } +#if FSFW_HAL_SPI_WIRETAPPING == 1 + comIf->performSpiWiretapping(cookie); +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ + + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullHigh(gpioId); + } + mutex->unlockMutex(); + + idx += 2; + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < transferLen) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(transferLen); + return returnvalue::OK; +} + +void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { + ReturnValue_t result; + acs::SimpleSensorMode mode; + bool cdHasTimedOut = false; + bool mustPerformStartup = false; + { + MutexGuard mg(ipcLock); + mode = gyro.mode; + cdHasTimedOut = gyro.countdown.hasTimedOut(); + mustPerformStartup = gyro.performStartup; + } + if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) { + if (mustPerformStartup) { + uint8_t regList[6]; + // Read configuration + regList[0] = adis1650x::DIAG_STAT_REG; + regList[1] = adis1650x::FILTER_CTRL_REG; + regList[2] = adis1650x::RANG_MDL_REG; + regList[3] = adis1650x::MSC_CTRL_REG; + regList[4] = adis1650x::DEC_RATE_REG; + regList[5] = adis1650x::PROD_ID_REG; + size_t transferLen = + adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size()); + result = readAdisCfg(*gyro.cookie, transferLen); + if (result != returnvalue::OK) { + gyro.replyResult = result; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = result; + return; + } + uint16_t prodId = (rawReply[12] << 8) | rawReply[13]; + if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or + ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) { + sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + MutexGuard mg(ipcLock); + gyro.ownReply.cfgWasSet = true; + gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3]; + gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9]; + gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.cfg.prodId = prodId; + gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); + gyro.performStartup = false; + } + // Read regular registers + std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), + adis1650x::BURST_READ_ENABLE.size()); + std::memset(cmdBuf.data() + 2, 0, 10 * 2); + result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE); + if (result != returnvalue::OK) { + gyro.replyResult = returnvalue::FAILED; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = returnvalue::FAILED; + return; + } + uint16_t checksum = (rawReply[20] << 8) | rawReply[21]; + + // Now verify the read checksum with the expected checksum according to datasheet p. 20 + uint16_t calcChecksum = 0; + for (size_t idx = 2; idx < 20; idx++) { + calcChecksum += rawReply[idx]; + } + if (checksum != calcChecksum) { + sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg); + if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode" + " not implemented!" + << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + MutexGuard mg(ipcLock); + gyro.ownReply.dataWasSet = true; + gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3]; + gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9]; + + gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13]; + gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15]; + + gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17]; + } +} diff --git a/linux/devices/AcsBoardPolling.h b/linux/devices/AcsBoardPolling.h new file mode 100644 index 00000000..bacc2a84 --- /dev/null +++ b/linux/devices/AcsBoardPolling.h @@ -0,0 +1,81 @@ +#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_ +#define LINUX_DEVICES_ACSBOARDPOLLING_H_ + +#include +#include +#include +#include +#include +#include +#include + +class AcsBoardPolling : public SystemObject, + public ExecutableObjectIF, + public DeviceCommunicationIF { + public: + AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + MutexIF* ipcLock; + SemaphoreIF* semaphore; + std::array cmdBuf; + std::array replyBuf; + + bool mgm0L3IsOn = false; + SpiCookie* mgm0L3Cookie = nullptr; + bool mgm1Rm3100IsOn = false; + SpiCookie* mgm1Rm3100Cookie = nullptr; + bool mgm2L3IsOn = false; + SpiCookie* mgm2L3Cookie = nullptr; + bool mgm3Rm3100IsOn = false; + SpiCookie* mgm3Rm3100Cookie = nullptr; + + struct GyroAdis { + adis1650x::Type type; + bool isOn = false; + bool performStartup = false; + SpiCookie* cookie = nullptr; + Countdown countdown; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + ReturnValue_t replyResult; + acs::Adis1650XReply ownReply; + acs::Adis1650XReply readerReply; + }; + GyroAdis gyro0Adis{}; + GyroAdis gyro2Adis{}; + + struct GyroL3g { + bool performStartup = false; + SpiCookie* cookie = nullptr; + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + uint8_t sensorCfg[5]; + ReturnValue_t replyResult; + acs::GyroL3gReply ownReply; + acs::GyroL3gReply readerReply; + }; + GyroL3g gyro1L3g{}; + GyroL3g gyro3L3g{}; + + uint8_t* rawReply = nullptr; + size_t dummy = 0; + + SpiComIF& spiComIF; + GpioIF& gpioIF; + + 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; + + void gyroL3gHandler(GyroL3g& l3g); + void gyroAdisHandler(GyroAdis& gyro); + // Special readout: 16us stall time between small 2 byte transfers. + ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); +}; + +#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */ diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 22b39840..ad4e8efe 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -4,8 +4,13 @@ endif() target_sources( ${OBSW_NAME} - PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp - ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) + PRIVATE Max31865RtdPolling.cpp + ScexUartReader.cpp + ImtqPollingTask.cpp + ScexDleParser.cpp + ScexHelper.cpp + RwPollingTask.cpp + AcsBoardPolling.cpp) add_subdirectory(ploc) diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h index 6cb70a06..26124b76 100644 --- a/linux/fsfwconfig/objects/systemObjectList.h +++ b/linux/fsfwconfig/objects/systemObjectList.h @@ -47,7 +47,6 @@ enum sourceObjects : uint32_t { GPIO_IF = 0x49010005, /* Custom device handler */ - RW_POLLING_TASK = 0x49020005, /* 0x54 ('T') for test handlers */ TEST_TASK = 0x54694269, diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 98f65427..3097cb5e 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -7,9 +7,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -854,7 +854,7 @@ void ThermalController::copyDevices() { { lp_var_t tempGyro0 = - lp_var_t(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + lp_var_t(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE); PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl; @@ -867,7 +867,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE); + lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE); PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl; @@ -881,7 +881,7 @@ void ThermalController::copyDevices() { { lp_var_t tempGyro2 = - lp_var_t(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE); + lp_var_t(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE); PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl; @@ -894,7 +894,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE); + lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE); PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 49e3d5fa..87a21589 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,6 +1,7 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include #include #include @@ -9,7 +10,6 @@ #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" -#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" namespace ACS { diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp index 0b134d87..cdf086fd 100644 --- a/mission/core/pollingSeqTables.cpp +++ b/mission/core/pollingSeqTables.cpp @@ -189,110 +189,7 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) { /* Length of a communication cycle */ uint32_t length = thisSequence->getPeriodMs(); - bool enableAside = true; - bool enableBside = true; - if (cfg.scheduleAcsBoard) { - if (enableAside) { - // A side - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - } - if (enableBside) { - // B side - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, - DeviceHandlerIF::GET_READ); - } - } // SUS: 16 ms bool addSus0 = true; bool addSus1 = true; @@ -583,6 +480,113 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ); } + bool enableAside = true; + bool enableBside = true; + if (cfg.scheduleAcsBoard) { + if (enableAside) { + // A side + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); + } + if (enableBside) { + // B side + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); + } + if (enableAside) { + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + if (enableBside) { + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, + length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD, + DeviceHandlerIF::GET_READ); + } + } + if (cfg.scheduleImtq) { // This is the MTM measurement cycle thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index ce02c688..adcbaf73 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -14,7 +14,8 @@ target_sources( ImtqHandler.cpp HeaterHandler.cpp RadiationSensorHandler.cpp - GyroADIS1650XHandler.cpp + GyrAdis1650XHandler.cpp + GyrL3gCustomHandler.cpp RwHandler.cpp max1227.cpp SusHandler.cpp diff --git a/mission/devices/GyrAdis1650XHandler.cpp b/mission/devices/GyrAdis1650XHandler.cpp new file mode 100644 index 00000000..7a13ec1a --- /dev/null +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -0,0 +1,515 @@ +#include "mission/devices/GyrAdis1650XHandler.h" + +#include "fsfw/action/HasActionsIF.h" +#include "fsfw/datapool/PoolReadGuard.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, adis1650x::Type type) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + adisType(type), + primaryDataset(this), + configDataset(this), + breakCountdown() { + adisRequest.type = adisType; +} + +void GyrAdis1650XHandler::doStartUp() { + // Initial 310 ms start up time after power-up + if (internalState == InternalState::STARTUP) { + if (not commandExecuted) { + warningSwitch = true; + breakCountdown.setTimeout(adis1650x::START_UP_TIME); + commandExecuted = true; + } + if (breakCountdown.hasTimedOut()) { + updatePeriodicReply(true, adis1650x::REPLY); + if (goToNormalMode) { + setMode(MODE_NORMAL); + } else { + setMode(MODE_ON); + } + } + } + + // // Read all configuration registers first + // if (internalState == InternalState::CONFIG) { + // if (commandExecuted) { + // commandExecuted = false; + // internalState = InternalState::IDLE; + // } + // } + // + // if (internalState == InternalState::IDLE) { + // } +} + +void GyrAdis1650XHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + commandExecuted = false; + primaryDataset.setValidity(false, true); + internalState = InternalState::SHUTDOWN; + } + if (commandExecuted) { + updatePeriodicReply(false, adis1650x::REPLY); + setMode(_MODE_POWER_DOWN); + } +} + +ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = adis1650x::REQUEST; + return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch (internalState) { + case (InternalState::STARTUP): { + *id = adis1650x::REQUEST; + return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); + } + case (InternalState::SHUTDOWN): { + *id = adis1650x::REQUEST; + acs::Adis1650XRequest *request = reinterpret_cast(cmdBuf.data()); + request->mode = acs::SimpleSensorMode::OFF; + request->type = adisType; + return returnvalue::OK; + } + default: { + // Might be a configuration error + sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " + "Unknown internal state!" + << std::endl; + return returnvalue::OK; + } + } + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + // switch (deviceCommand) { + // case (adis1650x::READ_OUT_CONFIG): { + // this->rawPacketLen = adis1650x::CONFIG_READOUT_SIZE; + // uint8_t regList[6] = {}; + // regList[0] = adis1650x::DIAG_STAT_REG; + // regList[1] = adis1650x::FILTER_CTRL_REG; + // regList[2] = adis1650x::RANG_MDL_REG; + // regList[3] = adis1650x::MSC_CTRL_REG; + // regList[4] = adis1650x::DEC_RATE_REG; + // regList[5] = adis1650x::PROD_ID_REG; + // adis1650x::prepareReadCommand(regList, sizeof(regList), commandBuffer.data(), + // commandBuffer.size()); + // this->rawPacket = commandBuffer.data(); + // break; + // } + // case (adis1650x::READ_SENSOR_DATA): { + // if (breakCountdown.isBusy()) { + // // A glob command is pending and sensor data can't be read currently + // return NO_REPLY_EXPECTED; + // } + // std::memcpy(commandBuffer.data(), adis1650x::BURST_READ_ENABLE.data(), + // adis1650x::BURST_READ_ENABLE.size()); + // std::memset(commandBuffer.data() + 2, 0, 10 * 2); + // this->rawPacketLen = adis1650x::SENSOR_READOUT_SIZE; + // this->rawPacket = commandBuffer.data(); + // break; + // } + // TODO: Convert to special request + + // case (adis1650x::SELF_TEST_SENSORS): { + // if (breakCountdown.isBusy()) { + // // Another glob command is pending + // return HasActionsIF::IS_BUSY; + // } + // prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SENSOR_SELF_TEST, 0x00); + // breakCountdown.setTimeout(adis1650x::SELF_TEST_BREAK); + // break; + // } + // case (adis1650x::SELF_TEST_MEMORY): { + // if (breakCountdown.isBusy()) { + // // Another glob command is pending + // return HasActionsIF::IS_BUSY; + // } + // prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_TEST, 0x00); + // breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_TEST_BREAK); + // break; + // } + // case (adis1650x::UPDATE_NV_CONFIGURATION): { + // if (breakCountdown.isBusy()) { + // // Another glob command is pending + // return HasActionsIF::IS_BUSY; + // } + // prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_UPDATE, + // 0x00); breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_UPDATE_BREAK); break; + // } + // case (adis1650x::RESET_SENSOR_CONFIGURATION): { + // if (breakCountdown.isBusy()) { + // // Another glob command is pending + // return HasActionsIF::IS_BUSY; + // } + // prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FACTORY_CALIBRATION, + // 0x00); breakCountdown.setTimeout(adis1650x::FACTORY_CALIBRATION_BREAK); break; + // } + // case (adis1650x::SW_RESET): { + // if (breakCountdown.isBusy()) { + // // Another glob command is pending + // return HasActionsIF::IS_BUSY; + // } + // prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SOFTWARE_RESET, 0x00); + // breakCountdown.setTimeout(adis1650x::SW_RESET_BREAK); + // break; + // } + // case (adis1650x::PRINT_CURRENT_CONFIGURATION): { + //#if OBSW_VERBOSE_LEVEL >= 1 + // PoolReadGuard pg(&configDataset); + // sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) + // << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl; + // sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" + // << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" + // << configDataset.filterSetting.value << " | DEC_RATE: 0x" + // << configDataset.decRateReg.value << std::setfill(' ') << std::endl; + //#endif /* OBSW_VERBOSE_LEVEL >= 1 */ + // + //} + //} + // return returnvalue::OK; + return NOTHING_TO_SEND; +} + +void GyrAdis1650XHandler::fillCommandAndReplyMap() { + insertInCommandMap(adis1650x::REQUEST); + insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true); + // insertInCommandAndReplyMap(adis1650x::READ_SENSOR_DATA, 1, &primaryDataset); + // insertInCommandAndReplyMap(adis1650x::READ_OUT_CONFIG, 1, &configDataset); + // insertInCommandAndReplyMap(adis1650x::SELF_TEST_SENSORS, 1); + // insertInCommandAndReplyMap(adis1650x::SELF_TEST_MEMORY, 1); + // insertInCommandAndReplyMap(adis1650x::UPDATE_NV_CONFIGURATION, 1); + // insertInCommandAndReplyMap(adis1650x::RESET_SENSOR_CONFIGURATION, 1); + // insertInCommandAndReplyMap(adis1650x::SW_RESET, 1); + // insertInCommandAndReplyMap(adis1650x::PRINT_CURRENT_CONFIGURATION, 1); +} + +ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (remainingSize != sizeof(acs::Adis1650XReply)) { + *foundLen = remainingSize; + return returnvalue::FAILED; + } + *foundId = adis1650x::REPLY; + *foundLen = remainingSize; + + return returnvalue::OK; +} + +ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + using namespace adis1650x; + const acs::Adis1650XReply *reply = reinterpret_cast(packet); + if (internalState == InternalState::STARTUP and reply->cfgWasSet) { + switch (adisType) { + case (adis1650x::Type::ADIS16507): { + if (reply->cfg.prodId != adis1650x::PROD_ID_16507) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " + << reply->cfg.prodId << "for ADIS type 16507" << std::endl; + return returnvalue::FAILED; + } + break; + } + case (adis1650x::Type::ADIS16505): { + if (reply->cfg.prodId != adis1650x::PROD_ID_16505) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " + << reply->cfg.prodId << "for ADIS type 16505" << std::endl; + return returnvalue::FAILED; + } + break; + } + } + PoolReadGuard rg(&configDataset); + configDataset.setValidity(true, true); + configDataset.mscCtrlReg = reply->cfg.mscCtrlReg; + configDataset.rangMdl = reply->cfg.rangMdl; + configDataset.diagStatReg = reply->cfg.diagStat; + configDataset.filterSetting = reply->cfg.filterSetting; + configDataset.decRateReg = reply->cfg.decRateReg; + commandExecuted = true; + } + if (reply->dataWasSet) { + { + PoolReadGuard rg(&configDataset); + configDataset.diagStatReg = reply->cfg.diagStat; + } + auto sensitivity = reply->data.sensitivity; + auto accelSensitivity = reply->data.accelScaling; + PoolReadGuard pg(&primaryDataset); + primaryDataset.setValidity(true, true); + primaryDataset.angVelocX = static_cast(reply->data.angVelocities[0]) * sensitivity; + primaryDataset.angVelocY = static_cast(reply->data.angVelocities[1]) * sensitivity; + primaryDataset.angVelocZ = static_cast(reply->data.angVelocities[2]) * sensitivity; + // TODO: Check whether we need to divide by INT16_MAX + primaryDataset.accelX = static_cast(reply->data.accelerations[0]) * accelSensitivity; + primaryDataset.accelY = static_cast(reply->data.accelerations[1]) * accelSensitivity; + primaryDataset.accelZ = static_cast(reply->data.accelerations[2]) * accelSensitivity; + primaryDataset.temperature.value = static_cast(reply->data.temperatureRaw) * 0.1; + } + return returnvalue::OK; +} + +LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) { + if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) { + return &primaryDataset; + } else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) { + return &configDataset; + } + return nullptr; +} + +// ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { +// using namespace adis1650x; +// BurstModes burstMode = getBurstMode(); +// switch (burstMode) { +// case (BurstModes::BURST_16_BURST_SEL_1): +// case (BurstModes::BURST_32_BURST_SEL_1): { +// sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1" +// " not implemented!" +// << std::endl; +// return returnvalue::OK; +// } +// case (adis1650x::BurstModes::BURST_16_BURST_SEL_0): { +// uint16_t checksum = packet[20] << 8 | packet[21]; +// // Now verify the read checksum with the expected checksum according to datasheet p. 20 +// uint16_t calcChecksum = 0; +// for (size_t idx = 2; idx < 20; idx++) { +// calcChecksum += packet[idx]; +// } +// if (checksum != calcChecksum) { +//#if OBSW_VERBOSE_LEVEL >= 1 +// sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: " +// "Invalid checksum detected!" +// << std::endl; +//#endif +// return returnvalue::FAILED; +// } +// +// ReturnValue_t result = configDataset.diagStatReg.read(); +// if (result == returnvalue::OK) { +// configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; +// configDataset.diagStatReg.setValid(true); +// } +// configDataset.diagStatReg.commit(); +// +// { +// PoolReadGuard pg(&primaryDataset); +// int16_t angVelocXRaw = packet[4] << 8 | packet[5]; +// primaryDataset.angVelocX.value = static_cast(angVelocXRaw) * sensitivity; +// int16_t angVelocYRaw = packet[6] << 8 | packet[7]; +// primaryDataset.angVelocY.value = static_cast(angVelocYRaw) * sensitivity; +// int16_t angVelocZRaw = packet[8] << 8 | packet[9]; +// primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) * sensitivity; +// +// float accelScaling = 0; +// if (adisType == adis1650x::Type::ADIS16507) { +// accelScaling = adis1650x::ACCELEROMETER_RANGE_16507; +// } else if (adisType == adis1650x::Type::ADIS16505) { +// accelScaling = adis1650x::ACCELEROMETER_RANGE_16505; +// } else { +// sif::warning << "GyroADIS1650XHandler::handleSensorData: " +// "Unknown ADIS type" +// << std::endl; +// } +// int16_t accelXRaw = packet[10] << 8 | packet[11]; +// primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * accelScaling; +// int16_t accelYRaw = packet[12] << 8 | packet[13]; +// primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * accelScaling; +// int16_t accelZRaw = packet[14] << 8 | packet[15]; +// primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * accelScaling; +// +// int16_t temperatureRaw = packet[16] << 8 | packet[17]; +// primaryDataset.temperature.value = static_cast(temperatureRaw) * 0.1; +// // Ignore data counter for now +// primaryDataset.setValidity(true, true); +// } +// +// if (periodicPrintout) { +// if (debugDivider.checkAndIncrement()) { +// sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl; +// sif::info << "X: " << primaryDataset.angVelocX.value << std::endl; +// sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl; +// sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl; +// sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl; +// sif::info << "X: " << primaryDataset.accelX.value << std::endl; +// sif::info << "Y: " << primaryDataset.accelY.value << std::endl; +// sif::info << "Z: " << primaryDataset.accelZ.value << std::endl; +// } +// } +// +// break; +// } +// case (BurstModes::BURST_32_BURST_SEL_0): { +// break; +// } +// } +// return returnvalue::OK; +// } + +uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } + +void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, + uint8_t valueTwo) { + uint8_t secondReg = startReg + 1; + startReg |= adis1650x::WRITE_MASK; + secondReg |= adis1650x::WRITE_MASK; + cmdBuf[0] = startReg; + cmdBuf[1] = valueOne; + cmdBuf[2] = secondReg; + cmdBuf[3] = valueTwo; + this->rawPacketLen = 4; + this->rawPacket = cmdBuf.data(); +} + +ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry({0.0})); + + localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl); + localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry()); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); + return returnvalue::OK; +} + +adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() { + using namespace adis1650x; + configDataset.mscCtrlReg.read(); + uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; + configDataset.mscCtrlReg.commit(); + return adis1650x::burstModeFromMscCtrl(currentCtrlReg); +} + +//#ifdef FSFW_OSAL_LINUX + +// ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, +// const uint8_t *sendData, size_t sendLen, +// void *args) { +// GyroADIS1650XHandler *handler = reinterpret_cast(args); +// if (handler == nullptr) { +// sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" +// << std::endl; +// return returnvalue::FAILED; +// } +// DeviceCommandId_t currentCommand = handler->getPendingCommand(); +// switch (currentCommand) { +// case (adis1650x::READ_SENSOR_DATA): { +// return comIf->performRegularSendOperation(cookie, sendData, sendLen); +// } +// case (adis1650x::READ_OUT_CONFIG): +// default: { +// ReturnValue_t result = returnvalue::OK; +// int retval = 0; +// // Prepare transfer +// int fileDescriptor = 0; +// std::string device = comIf->getSpiDev(); +// UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); +// if (fileHelper.getOpenResult() != returnvalue::OK) { +// return SpiComIF::OPENING_FILE_FAILED; +// } +// spi::SpiModes spiMode = spi::SpiModes::MODE_0; +// uint32_t spiSpeed = 0; +// cookie->getSpiParameters(spiMode, spiSpeed, nullptr); +// comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); +// cookie->assignWriteBuffer(sendData); +// cookie->setTransferSize(2); +// +// gpioId_t gpioId = cookie->getChipSelectPin(); +// GpioIF &gpioIF = comIf->getGpioInterface(); +// MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; +// uint32_t timeoutMs = 0; +// MutexIF *mutex = comIf->getCsMutex(); +// cookie->getMutexParams(timeoutType, timeoutMs); +// if (mutex == nullptr) { +//#if OBSW_VERBOSE_LEVEL >= 1 +// sif::warning << "GyroADIS16507Handler::spiSendCallback: " +// "Mutex or GPIO interface invalid" +// << std::endl; +// return returnvalue::FAILED; +//#endif +// } +// +// if (gpioId != gpio::NO_GPIO) { +// result = mutex->lockMutex(timeoutType, timeoutMs); +// if (result != returnvalue::OK) { +//#if FSFW_CPP_OSTREAM_ENABLED == 1 +// sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; +//#endif +// return result; +// } +// } +// +// size_t idx = 0; +// spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle(); +// uint64_t origTx = transferStruct->tx_buf; +// uint64_t origRx = transferStruct->rx_buf; +// while (idx < sendLen) { +// // Pull SPI CS low. For now, no support for active high given +// if (gpioId != gpio::NO_GPIO) { +// gpioIF.pullLow(gpioId); +// } +// +// // Execute transfer +// // Initiate a full duplex SPI transfer. +// retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle()); +// if (retval < 0) { +// utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); +// result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; +// } +//#if FSFW_HAL_SPI_WIRETAPPING == 1 +// comIf->performSpiWiretapping(cookie); +//#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ +// +// if (gpioId != gpio::NO_GPIO) { +// gpioIF.pullHigh(gpioId); +// } +// +// idx += 2; +// if (idx < sendLen) { +// usleep(adis1650x::STALL_TIME_MICROSECONDS); +// } +// +// transferStruct->tx_buf += 2; +// transferStruct->rx_buf += 2; +// } +// transferStruct->tx_buf = origTx; +// transferStruct->rx_buf = origRx; +// if (gpioId != gpio::NO_GPIO) { +// mutex->unlockMutex(); +// } +// } +// } +// return returnvalue::OK; +// } + +void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } + +void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) { + adisRequest.mode = mode; + rawPacket = reinterpret_cast(&adisRequest); + rawPacketLen = sizeof(acs::Adis1650XRequest); + return returnvalue::OK; +} diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyrAdis1650XHandler.h similarity index 70% rename from mission/devices/GyroADIS1650XHandler.h rename to mission/devices/GyrAdis1650XHandler.h index d348baa2..3d8e82cf 100644 --- a/mission/devices/GyroADIS1650XHandler.h +++ b/mission/devices/GyrAdis1650XHandler.h @@ -1,27 +1,24 @@ #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_ #define MISSION_DEVICES_GYROADIS16507HANDLER_H_ +#include +#include + #include "FSFWConfig.h" #include "OBSWConfig.h" -#include "devicedefinitions/GyroADIS1650XDefinitions.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" -#ifdef FSFW_OSAL_LINUX -class SpiComIF; -class SpiCookie; -#endif - /** * @brief Device handle for the ADIS16507 Gyroscope by Analog Devices * @details * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro */ -class GyroADIS1650XHandler : public DeviceHandlerBase { +class GyrAdis1650XHandler : public DeviceHandlerBase { public: - GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, - ADIS1650X::Type type); + GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + adis1650x::Type type); void enablePeriodicPrintouts(bool enable, uint8_t divider); void setToGoToNormalModeImmediately(); @@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { 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: - std::array commandBuffer; - ADIS1650X::Type adisType; + std::array cmdBuf; + acs::Adis1650XRequest adisRequest; + adis1650x::Type adisType; AdisGyroPrimaryDataset primaryDataset; AdisGyroConfigDataset configDataset; - double sensitivity = ADIS1650X::SENSITIVITY_UNSET; + double sensitivity = adis1650x::SENSITIVITY_UNSET; bool goToNormalMode = false; bool warningSwitch = true; - enum class InternalState { STARTUP, CONFIG, IDLE }; - - enum class BurstModes { - BURST_16_BURST_SEL_0, - BURST_16_BURST_SEL_1, - BURST_32_BURST_SEL_0, - BURST_32_BURST_SEL_1 - }; + enum class InternalState { STARTUP, SHUTDOWN, IDLE }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; PoolEntry rangMdl = PoolEntry(); - void prepareReadCommand(uint8_t *regList, size_t len); - - BurstModes getBurstMode(); - -#ifdef FSFW_OSAL_LINUX - static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData, - size_t sendLen, void *args); -#endif + adis1650x::BurstModes getBurstMode(); Countdown breakCountdown; void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo); + ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode); ReturnValue_t handleSensorData(const uint8_t *packet); diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp new file mode 100644 index 00000000..0af32d0c --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -0,0 +1,216 @@ + +#include + +#include + +#include "fsfw/datapool/PoolReadGuard.h" +#include "mission/devices/devicedefinitions/acsPolling.h" + +GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie, uint32_t transitionDelayMs) + : DeviceHandlerBase(objectId, deviceCommunication, comCookie), + transitionDelayMs(transitionDelayMs), + dataset(this) {} + +GyrL3gCustomHandler::~GyrL3gCustomHandler() = default; + +void GyrL3gCustomHandler::doStartUp() { + if (internalState == InternalState::NONE) { + internalState = InternalState::STARTUP; + updatePeriodicReply(true, l3gd20h::REPLY); + commandExecuted = false; + } + + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + if (goNormalModeImmediately) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + } + } +} + +void GyrL3gCustomHandler::doShutDown() { + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + commandExecuted = false; + } + if (commandExecuted) { + internalState = InternalState::NONE; + updatePeriodicReply(false, l3gd20h::REPLY); + commandExecuted = false; + setMode(_MODE_POWER_DOWN); + } +} + +ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch (internalState) { + case (InternalState::NONE): + case (InternalState::NORMAL): { + return NOTHING_TO_SEND; + } + case (InternalState::STARTUP): { + *id = l3gd20h::REQUEST; + gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL; + gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL; + gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL; + gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL; + gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL; + return doPeriodicRequest(acs::SimpleSensorMode::NORMAL); + } + case (InternalState::SHUTDOWN): { + *id = l3gd20h::REQUEST; + return doPeriodicRequest(acs::SimpleSensorMode::OFF); + } + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + /* Might be a configuration error. */ + sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!" + << std::endl; +#else + sif::printDebug( + "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!\n"); +#endif + return returnvalue::OK; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = l3gd20h::REQUEST; + return doPeriodicRequest(acs::SimpleSensorMode::NORMAL); +} + +ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, + size_t commandDataLen) { + switch (deviceCommand) { + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) { + return IGNORE_FULL_PACKET; + } + if (len != sizeof(acs::GyroL3gReply)) { + *foundLen = len; + return returnvalue::FAILED; + } + *foundId = l3gd20h::REPLY; + *foundLen = len; + return returnvalue::OK; +} + +ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + const acs::GyroL3gReply *reply = reinterpret_cast(packet); + if (reply->cfgWasSet) { + if (internalState == InternalState::STARTUP) { + commandExecuted = true; + } + PoolReadGuard readSet(&dataset); + dataset.setValidity(true, true); + dataset.angVelocX = reply->angVelocities[0]; + dataset.angVelocY = reply->angVelocities[1]; + dataset.angVelocZ = reply->angVelocities[2]; + if (std::abs(dataset.angVelocX.value) > absLimitX) { + dataset.angVelocX.setValid(false); + } + if (std::abs(dataset.angVelocY.value) > absLimitY) { + dataset.angVelocY.setValid(false); + } + if (std::abs(dataset.angVelocZ.value) > absLimitZ) { + dataset.angVelocZ.setValid(false); + } + dataset.temperature = 25.0 - reply->tempOffsetRaw; + } + // PoolReadGuard readSet(&dataset); + // if (readSet.getReadResult() == returnvalue::OK) { + // if (std::abs(angVelocX) < this->absLimitX) { + // dataset.angVelocX = angVelocX; + // dataset.angVelocX.setValid(true); + // } else { + // dataset.angVelocX.setValid(false); + // } + // + // if (std::abs(angVelocY) < this->absLimitY) { + // dataset.angVelocY = angVelocY; + // dataset.angVelocY.setValid(true); + // } else { + // dataset.angVelocY.setValid(false); + // } + // + // if (std::abs(angVelocZ) < this->absLimitZ) { + // dataset.angVelocZ = angVelocZ; + // dataset.angVelocZ.setValid(true); + // } else { + // dataset.angVelocZ.setValid(false); + // } + // + // dataset.temperature = temperature; + // dataset.temperature.setValid(true); + // } + // break; + // } + return returnvalue::OK; +} + +uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelayMs; +} + +void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; } + +ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0)); + return returnvalue::OK; +} + +void GyrL3gCustomHandler::fillCommandAndReplyMap() { + insertInCommandMap(l3gd20h::REQUEST); + insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true); + // insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset); + // insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1); + // insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1); +} + +void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; } + +void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) { + this->absLimitX = limitX; + this->absLimitY = limitY; + this->absLimitZ = limitZ; +} + +void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { + periodicPrintout = enable; + debugDivider.setDivider(divider); +} + +LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) { + if (sid == dataset.getSid()) { + return &dataset; + } + return nullptr; +} + +ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) { + gyrRequest.mode = mode; + rawPacket = reinterpret_cast(&gyrRequest); + rawPacketLen = sizeof(acs::GyroL3gRequest); + return returnvalue::OK; +} diff --git a/mission/devices/GyrL3gCustomHandler.h b/mission/devices/GyrL3gCustomHandler.h new file mode 100644 index 00000000..7d762ad8 --- /dev/null +++ b/mission/devices/GyrL3gCustomHandler.h @@ -0,0 +1,92 @@ +#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ + +#include +#include +#include +#include + +/** + * @brief Device Handler for the L3GD20H gyroscope sensor + * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) + * @details + * Advanced documentation: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro + * + * Data is read big endian with the smallest possible range of 245 degrees per second. + */ +class GyrL3gCustomHandler : public DeviceHandlerBase { + public: + GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, + uint32_t transitionDelayMs); + virtual ~GyrL3gCustomHandler(); + + void enablePeriodicPrintouts(bool enable, uint8_t divider); + + /** + * Set the absolute limit for the values on the axis in degrees per second. + * The dataset values will be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float limitX, float limitY, float limitZ); + + /** + * @brief Configure device handler to go to normal mode immediately + */ + void setToGoToNormalMode(bool enable); + + protected: + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged() override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override; + + private: + uint32_t transitionDelayMs = 0; + GyroPrimaryDataset dataset; + + float absLimitX = l3gd20h::RANGE_DPS_00; + float absLimitY = l3gd20h::RANGE_DPS_00; + float absLimitZ = l3gd20h::RANGE_DPS_00; + + enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + uint8_t statusReg = 0; + bool goNormalModeImmediately = false; + + uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL; + uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL; + uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL; + uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL; + uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL; + + std::array cmdBuf; + acs::GyroL3gRequest gyrRequest{}; + + // Set default value + float sensitivity = l3gd20h::SENSITIVITY_00; + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); + + ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode); +}; + +#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp deleted file mode 100644 index daff1b06..00000000 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ /dev/null @@ -1,524 +0,0 @@ -#include "GyroADIS1650XHandler.h" - -#include -#include - -#include "fsfw/FSFW.h" - -#ifdef FSFW_OSAL_LINUX -#include -#include - -#include "fsfw_hal/linux/UnixFileGuard.h" -#include "fsfw_hal/linux/spi/SpiComIF.h" -#include "fsfw_hal/linux/spi/SpiCookie.h" -#include "fsfw_hal/linux/utility.h" -#endif - -GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, ADIS1650X::Type type) - : DeviceHandlerBase(objectId, deviceCommunication, comCookie), - adisType(type), - primaryDataset(this), - configDataset(this), - breakCountdown() { -#ifdef FSFW_OSAL_LINUX - SpiCookie *cookie = dynamic_cast(comCookie); - if (cookie != nullptr) { - cookie->setCallbackMode(&spiSendCallback, this); - } -#endif -} - -void GyroADIS1650XHandler::doStartUp() { - // Initial 310 ms start up time after power-up - if (internalState == InternalState::STARTUP) { - if (not commandExecuted) { - warningSwitch = true; - breakCountdown.setTimeout(ADIS1650X::START_UP_TIME); - commandExecuted = true; - } - if (breakCountdown.hasTimedOut()) { - internalState = InternalState::CONFIG; - commandExecuted = false; - } - } - - // Read all configuration registers first - if (internalState == InternalState::CONFIG) { - if (commandExecuted) { - commandExecuted = false; - internalState = InternalState::IDLE; - } - } - - if (internalState == InternalState::IDLE) { - if (goToNormalMode) { - setMode(MODE_NORMAL); - } else { - setMode(MODE_ON); - } - } -} - -void GyroADIS1650XHandler::doShutDown() { - commandExecuted = false; - internalState = InternalState::STARTUP; - setMode(_MODE_POWER_DOWN); -} - -ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = ADIS1650X::READ_SENSOR_DATA; - return buildCommandFromCommand(*id, nullptr, 0); -} - -ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - switch (internalState) { - case (InternalState::CONFIG): { - *id = ADIS1650X::READ_OUT_CONFIG; - buildCommandFromCommand(*id, nullptr, 0); - break; - } - case (InternalState::STARTUP): { - return NOTHING_TO_SEND; - break; - } - default: { - // Might be a configuration error - sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " - "Unknown internal state!" - << std::endl; - return returnvalue::OK; - } - } - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, - const uint8_t *commandData, - size_t commandDataLen) { - switch (deviceCommand) { - case (ADIS1650X::READ_OUT_CONFIG): { - this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE; - uint8_t regList[6] = {}; - regList[0] = ADIS1650X::DIAG_STAT_REG; - regList[1] = ADIS1650X::FILTER_CTRL_REG; - regList[2] = ADIS1650X::RANG_MDL_REG; - regList[3] = ADIS1650X::MSC_CTRL_REG; - regList[4] = ADIS1650X::DEC_RATE_REG; - regList[5] = ADIS1650X::PROD_ID_REG; - prepareReadCommand(regList, sizeof(regList)); - this->rawPacket = commandBuffer.data(); - break; - } - case (ADIS1650X::READ_SENSOR_DATA): { - if (breakCountdown.isBusy()) { - // A glob command is pending and sensor data can't be read currently - return NO_REPLY_EXPECTED; - } - std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(), - ADIS1650X::BURST_READ_ENABLE.size()); - std::memset(commandBuffer.data() + 2, 0, 10 * 2); - this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE; - this->rawPacket = commandBuffer.data(); - break; - } - case (ADIS1650X::SELF_TEST_SENSORS): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00); - breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK); - break; - } - case (ADIS1650X::SELF_TEST_MEMORY): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00); - breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK); - break; - } - case (ADIS1650X::UPDATE_NV_CONFIGURATION): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00); - breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK); - break; - } - case (ADIS1650X::RESET_SENSOR_CONFIGURATION): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00); - breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK); - break; - } - case (ADIS1650X::SW_RESET): { - if (breakCountdown.isBusy()) { - // Another glob command is pending - return HasActionsIF::IS_BUSY; - } - prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00); - breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK); - break; - } - case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): { -#if OBSW_VERBOSE_LEVEL >= 1 - PoolReadGuard pg(&configDataset); - sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) - << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl; - sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" - << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" - << configDataset.filterSetting.value << " | DEC_RATE: 0x" - << configDataset.decRateReg.value << std::setfill(' ') << std::endl; -#endif /* OBSW_VERBOSE_LEVEL >= 1 */ - } - } - return returnvalue::OK; -} - -void GyroADIS1650XHandler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset); - insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset); - insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1); - insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1); - insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1); - insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1); -} - -ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize, - DeviceCommandId_t *foundId, size_t *foundLen) { - // For SPI, the ID will always be the one of the last sent command - *foundId = this->getPendingCommand(); - *foundLen = this->rawPacketLen; - - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - using namespace ADIS1650X; - switch (id) { - case (ADIS1650X::READ_OUT_CONFIG): { - uint16_t readProdId = packet[12] << 8 | packet[13]; - if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or - ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) { -#if OBSW_VERBOSE_LEVEL >= 1 - if (warningSwitch) { - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID " - << readProdId << std::endl; - } - warningSwitch = false; -#endif - return returnvalue::FAILED; - } - PoolReadGuard rg(&configDataset); - configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; - configDataset.filterSetting.value = packet[4] << 8 | packet[5]; - uint16_t rangMdlRaw = packet[6] << 8 | packet[7]; - ADIS1650X::RangMdlBitfield bitfield = - static_cast((rangMdlRaw >> 2) & 0b11); - switch (bitfield) { - case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): { - sensitivity = SENSITIVITY_1BMLZ; - break; - } - case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): { - sensitivity = SENSITIVITY_2BMLZ; - break; - } - case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): { - sensitivity = SENSITIVITY_3BMLZ; - break; - } - case (RangMdlBitfield::RESERVED): { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl; -#endif - break; - } - } - configDataset.rangMdl.value = rangMdlRaw; - configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9]; - configDataset.decRateReg.value = packet[10] << 8 | packet[11]; - configDataset.setValidity(true, true); - if (internalState == InternalState::CONFIG) { - commandExecuted = true; - } - break; - } - case (ADIS1650X::READ_SENSOR_DATA): { - return handleSensorData(packet); - } - } - return returnvalue::OK; -} - -ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { - BurstModes burstMode = getBurstMode(); - switch (burstMode) { - case (BurstModes::BURST_16_BURST_SEL_1): - case (BurstModes::BURST_32_BURST_SEL_1): { - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1" - " not implemented!" - << std::endl; - return returnvalue::OK; - } - case (BurstModes::BURST_16_BURST_SEL_0): { - uint16_t checksum = packet[20] << 8 | packet[21]; - // Now verify the read checksum with the expected checksum according to datasheet p. 20 - uint16_t calcChecksum = 0; - for (size_t idx = 2; idx < 20; idx++) { - calcChecksum += packet[idx]; - } - if (checksum != calcChecksum) { -#if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: " - "Invalid checksum detected!" - << std::endl; -#endif - return returnvalue::FAILED; - } - - ReturnValue_t result = configDataset.diagStatReg.read(); - if (result == returnvalue::OK) { - configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; - configDataset.diagStatReg.setValid(true); - } - configDataset.diagStatReg.commit(); - - { - PoolReadGuard pg(&primaryDataset); - int16_t angVelocXRaw = packet[4] << 8 | packet[5]; - primaryDataset.angVelocX.value = static_cast(angVelocXRaw) * sensitivity; - int16_t angVelocYRaw = packet[6] << 8 | packet[7]; - primaryDataset.angVelocY.value = static_cast(angVelocYRaw) * sensitivity; - int16_t angVelocZRaw = packet[8] << 8 | packet[9]; - primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) * sensitivity; - - float accelScaling = 0; - if (adisType == ADIS1650X::Type::ADIS16507) { - accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507; - } else if (adisType == ADIS1650X::Type::ADIS16505) { - accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505; - } else { - sif::warning << "GyroADIS1650XHandler::handleSensorData: " - "Unknown ADIS type" - << std::endl; - } - int16_t accelXRaw = packet[10] << 8 | packet[11]; - primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * accelScaling; - int16_t accelYRaw = packet[12] << 8 | packet[13]; - primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * accelScaling; - int16_t accelZRaw = packet[14] << 8 | packet[15]; - primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * accelScaling; - - int16_t temperatureRaw = packet[16] << 8 | packet[17]; - primaryDataset.temperature.value = static_cast(temperatureRaw) * 0.1; - // Ignore data counter for now - primaryDataset.setValidity(true, true); - } - - if (periodicPrintout) { - if (debugDivider.checkAndIncrement()) { - sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl; - sif::info << "X: " << primaryDataset.angVelocX.value << std::endl; - sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl; - sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl; - sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl; - sif::info << "X: " << primaryDataset.accelX.value << std::endl; - sif::info << "Y: " << primaryDataset.accelY.value << std::endl; - sif::info << "Z: " << primaryDataset.accelZ.value << std::endl; - } - } - - break; - } - case (BurstModes::BURST_32_BURST_SEL_0): { - break; - } - } - return returnvalue::OK; -} - -uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } - -void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, - uint8_t valueTwo) { - uint8_t secondReg = startReg + 1; - startReg |= ADIS1650X::WRITE_MASK; - secondReg |= ADIS1650X::WRITE_MASK; - commandBuffer[0] = startReg; - commandBuffer[1] = valueOne; - commandBuffer[2] = secondReg; - commandBuffer[3] = valueTwo; - this->rawPacketLen = 4; - this->rawPacket = commandBuffer.data(); -} - -void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) { - for (size_t idx = 0; idx < len; idx++) { - commandBuffer[idx * 2] = regList[idx]; - commandBuffer[idx * 2 + 1] = 0x00; - } - commandBuffer[len * 2] = 0x00; - commandBuffer[len * 2 + 1] = 0x00; -} - -ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, - LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); - - localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl); - localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry()); - poolManager.subscribeForRegularPeriodicPacket( - subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); - return returnvalue::OK; -} - -GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() { - configDataset.mscCtrlReg.read(); - uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; - configDataset.mscCtrlReg.commit(); - if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) { - if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { - return BurstModes::BURST_32_BURST_SEL_1; - } else { - return BurstModes::BURST_32_BURST_SEL_0; - } - } else { - if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { - return BurstModes::BURST_16_BURST_SEL_1; - } else { - return BurstModes::BURST_16_BURST_SEL_0; - } - } -} - -#ifdef FSFW_OSAL_LINUX - -ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, - const uint8_t *sendData, size_t sendLen, - void *args) { - GyroADIS1650XHandler *handler = reinterpret_cast(args); - if (handler == nullptr) { - sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" - << std::endl; - return returnvalue::FAILED; - } - DeviceCommandId_t currentCommand = handler->getPendingCommand(); - switch (currentCommand) { - case (ADIS1650X::READ_SENSOR_DATA): { - return comIf->performRegularSendOperation(cookie, sendData, sendLen); - } - case (ADIS1650X::READ_OUT_CONFIG): - default: { - ReturnValue_t result = returnvalue::OK; - int retval = 0; - // Prepare transfer - int fileDescriptor = 0; - std::string device = comIf->getSpiDev(); - UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); - if (fileHelper.getOpenResult() != returnvalue::OK) { - return SpiComIF::OPENING_FILE_FAILED; - } - spi::SpiModes spiMode = spi::SpiModes::MODE_0; - uint32_t spiSpeed = 0; - cookie->getSpiParameters(spiMode, spiSpeed, nullptr); - comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); - cookie->assignWriteBuffer(sendData); - cookie->setTransferSize(2); - - gpioId_t gpioId = cookie->getChipSelectPin(); - GpioIF &gpioIF = comIf->getGpioInterface(); - MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = 0; - MutexIF *mutex = comIf->getCsMutex(); - cookie->getMutexParams(timeoutType, timeoutMs); - if (mutex == nullptr) { -#if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS16507Handler::spiSendCallback: " - "Mutex or GPIO interface invalid" - << std::endl; - return returnvalue::FAILED; -#endif - } - - if (gpioId != gpio::NO_GPIO) { - result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != returnvalue::OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; -#endif - return result; - } - } - - size_t idx = 0; - spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle(); - uint64_t origTx = transferStruct->tx_buf; - uint64_t origRx = transferStruct->rx_buf; - while (idx < sendLen) { - // Pull SPI CS low. For now, no support for active high given - if (gpioId != gpio::NO_GPIO) { - gpioIF.pullLow(gpioId); - } - - // Execute transfer - // Initiate a full duplex SPI transfer. - retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle()); - if (retval < 0) { - utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); - result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED; - } -#if FSFW_HAL_SPI_WIRETAPPING == 1 - comIf->performSpiWiretapping(cookie); -#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ - - if (gpioId != gpio::NO_GPIO) { - gpioIF.pullHigh(gpioId); - } - - idx += 2; - if (idx < sendLen) { - usleep(ADIS1650X::STALL_TIME_MICROSECONDS); - } - - transferStruct->tx_buf += 2; - transferStruct->rx_buf += 2; - } - transferStruct->tx_buf = origTx; - transferStruct->rx_buf = origRx; - if (gpioId != gpio::NO_GPIO) { - mutex->unlockMutex(); - } - } - } - return returnvalue::OK; -} - -void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } - -void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) { - periodicPrintout = enable; - debugDivider.setDivider(divider); -} - -#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */ diff --git a/mission/devices/devicedefinitions/CMakeLists.txt b/mission/devices/devicedefinitions/CMakeLists.txt index b2394b2a..eab4d4cb 100644 --- a/mission/devices/devicedefinitions/CMakeLists.txt +++ b/mission/devices/devicedefinitions/CMakeLists.txt @@ -1,2 +1,2 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp - imtqHelpers.cpp) + imtqHelpers.cpp gyroAdisHelpers.cpp) diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index 4c4543d0..43783c82 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -6,7 +6,7 @@ #include -namespace L3GD20H { +namespace l3gd20h { /* Actual size is 15 but we round up a bit */ static constexpr size_t MAX_BUFFER_SIZE = 16; @@ -104,27 +104,27 @@ static constexpr uint32_t GYRO_DATASET_ID = READ_REGS; enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE }; -} // namespace L3GD20H +} // namespace l3gd20h class GyroPrimaryDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ GyroPrimaryDataset(object_id_t mgmId) - : StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) { + : StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, L3GD20H::TEMPERATURE, this); + lp_var_t angVelocX = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, l3gd20h::TEMPERATURE, this); private: friend class GyroHandlerL3GD20H; /** Constructor for the data creator */ GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/acsPolling.h b/mission/devices/devicedefinitions/acsPolling.h new file mode 100644 index 00000000..66cf559c --- /dev/null +++ b/mission/devices/devicedefinitions/acsPolling.h @@ -0,0 +1,58 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ + +#include "gyroAdisHelpers.h" + +namespace acs { + +enum SimpleSensorMode { NORMAL = 0, OFF = 1 }; + +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; +}; + +struct Adis1650XConfig { + uint16_t diagStat; + uint16_t filterSetting; + uint16_t rangMdl; + uint16_t mscCtrlReg; + uint16_t decRateReg; + uint16_t prodId; +}; + +struct Adis1650XData { + double sensitivity; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]; + double accelScaling; + // Accelerations in all axes + int16_t accelerations[3]; + int16_t temperatureRaw; +}; + +struct Adis1650XReply { + bool cfgWasSet = false; + Adis1650XConfig cfg; + bool dataWasSet = false; + Adis1650XData data; +}; + +struct GyroL3gRequest { + SimpleSensorMode mode; + uint8_t ctrlRegs[5]; +}; + +struct GyroL3gReply { + bool cfgWasSet; + uint8_t statusReg; + // Angular velocities in all axes (X, Y and Z) + int16_t angVelocities[3]; + int8_t tempOffsetRaw; + uint8_t ctrlRegs[5]; + float sensitivity; +}; + +} // namespace acs + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ */ diff --git a/mission/devices/devicedefinitions/gyroAdisHelpers.cpp b/mission/devices/devicedefinitions/gyroAdisHelpers.cpp new file mode 100644 index 00000000..0f41b058 --- /dev/null +++ b/mission/devices/devicedefinitions/gyroAdisHelpers.cpp @@ -0,0 +1,54 @@ +#include "gyroAdisHelpers.h" + +size_t adis1650x::prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, + size_t maxLen) { + if (len * 2 + 2 > maxLen) { + return 0; + } + for (size_t idx = 0; idx < len; idx++) { + outBuf[idx * 2] = regList[idx]; + outBuf[idx * 2 + 1] = 0x00; + } + outBuf[len * 2] = 0x00; + outBuf[len * 2 + 1] = 0x00; + return len * 2 + 2; +} + +adis1650x::BurstModes adis1650x::burstModeFromMscCtrl(uint16_t mscCtrl) { + if ((mscCtrl & adis1650x::BURST_32_BIT) == adis1650x::BURST_32_BIT) { + if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) { + return BurstModes::BURST_32_BURST_SEL_1; + } else { + return BurstModes::BURST_32_BURST_SEL_0; + } + } else { + if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) { + return BurstModes::BURST_16_BURST_SEL_1; + } else { + return BurstModes::BURST_16_BURST_SEL_0; + } + } +} + +double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { + adis1650x::RangMdlBitfield bitfield = + static_cast((rangMdl >> 2) & 0b11); + switch (bitfield) { + case (adis1650x::RangMdlBitfield::RANGE_125_1BMLZ): { + return SENSITIVITY_1BMLZ; + } + case (adis1650x::RangMdlBitfield::RANGE_500_2BMLZ): { + return SENSITIVITY_2BMLZ; + } + case (adis1650x::RangMdlBitfield::RANGE_2000_3BMLZ): { + return SENSITIVITY_3BMLZ; + } + case (RangMdlBitfield::RESERVED): + default: { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl; +#endif + return 0.0; + } + } +} diff --git a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h b/mission/devices/devicedefinitions/gyroAdisHelpers.h similarity index 72% rename from mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h rename to mission/devices/devicedefinitions/gyroAdisHelpers.h index 3cfda2d4..09f0aa1e 100644 --- a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h +++ b/mission/devices/devicedefinitions/gyroAdisHelpers.h @@ -6,9 +6,20 @@ #include "fsfw/datapoollocal/StaticLocalDataSet.h" #include "fsfw/devicehandlers/DeviceHandlerIF.h" -namespace ADIS1650X { +namespace adis1650x { -enum class Type { ADIS16505, ADIS16507 }; +enum class BurstModes { + BURST_16_BURST_SEL_0, + BURST_16_BURST_SEL_1, + BURST_32_BURST_SEL_0, + BURST_32_BURST_SEL_1 +}; + +size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); +double rangMdlToSensitivity(uint16_t rangMdl); + +enum class Type : uint8_t { ADIS16505, ADIS16507 }; static constexpr size_t MAXIMUM_REPLY_SIZE = 64; static constexpr uint8_t WRITE_MASK = 0b1000'0000; @@ -67,6 +78,9 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30; static constexpr DeviceCommandId_t SW_RESET = 31; static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32; +static constexpr DeviceCommandId_t REQUEST = 0x70; +static constexpr DeviceCommandId_t REPLY = 0x77; + static constexpr uint16_t BURST_32_BIT = 1 << 9; static constexpr uint16_t BURST_SEL_BIT = 1 << 8; static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7; @@ -111,57 +125,57 @@ enum FilterSettings : uint8_t { SIXTYFOUR_TAPS = 6 }; -} // namespace ADIS1650X +} // namespace adis1650x class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> { public: /** Constructor for data users like controllers */ AdisGyroPrimaryDataset(object_id_t adisId) - : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) { + : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, ADIS1650X::ANG_VELOC_Z, this); - lp_var_t accelX = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_X, this); - lp_var_t accelY = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_Y, this); - lp_var_t accelZ = lp_var_t(sid.objectId, ADIS1650X::ACCELERATION_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, ADIS1650X::TEMPERATURE, this); + lp_var_t angVelocX = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, adis1650x::ANG_VELOC_Z, this); + lp_var_t accelX = lp_var_t(sid.objectId, adis1650x::ACCELERATION_X, this); + lp_var_t accelY = lp_var_t(sid.objectId, adis1650x::ACCELERATION_Y, this); + lp_var_t accelZ = lp_var_t(sid.objectId, adis1650x::ACCELERATION_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, adis1650x::TEMPERATURE, this); private: - friend class GyroADIS1650XHandler; + friend class GyrAdis1650XHandler; friend class GyroAdisDummy; /** Constructor for the data creator */ AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, adis1650x::ADIS_DATASET_ID) {} }; class AdisGyroConfigDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ AdisGyroConfigDataset(object_id_t adisId) - : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) { + : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_CFG_DATASET_ID)) { setAllVariablesReadOnly(); } lp_var_t diagStatReg = - lp_var_t(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::DIAG_STAT_REGISTER, this); lp_var_t filterSetting = - lp_var_t(sid.objectId, ADIS1650X::FILTER_SETTINGS, this); - lp_var_t rangMdl = lp_var_t(sid.objectId, ADIS1650X::RANG_MDL, this); + lp_var_t(sid.objectId, adis1650x::FILTER_SETTINGS, this); + lp_var_t rangMdl = lp_var_t(sid.objectId, adis1650x::RANG_MDL, this); lp_var_t mscCtrlReg = - lp_var_t(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::MSC_CTRL_REGISTER, this); lp_var_t decRateReg = - lp_var_t(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this); + lp_var_t(sid.objectId, adis1650x::DEC_RATE_REGISTER, this); private: - friend class GyroADIS1650XHandler; + friend class GyrAdis1650XHandler; /** Constructor for the data creator */ AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, adis1650x::ADIS_CFG_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */ diff --git a/mission/system/objects/AcsBoardAssembly.cpp b/mission/system/objects/AcsBoardAssembly.cpp index bcc2eb7a..01dd76b1 100644 --- a/mission/system/objects/AcsBoardAssembly.cpp +++ b/mission/system/objects/AcsBoardAssembly.cpp @@ -274,4 +274,9 @@ void AcsBoardAssembly::refreshHelperModes() { } } -ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t AcsBoardAssembly::initialize() { + for (const auto& child : childrenMap) { + updateChildModeByObjId(child.first, MODE_OFF, 0); + } + return AssemblyBase::initialize(); +} diff --git a/mission/system/objects/SusAssembly.cpp b/mission/system/objects/SusAssembly.cpp index 3f75a369..62a47616 100644 --- a/mission/system/objects/SusAssembly.cpp +++ b/mission/system/objects/SusAssembly.cpp @@ -120,7 +120,12 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan return returnvalue::OK; } -ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t SusAssembly::initialize() { + for (const auto& child : childrenMap) { + updateChildModeByObjId(child.first, MODE_OFF, 0); + } + return AssemblyBase::initialize(); +} bool SusAssembly::isUseable(object_id_t object, Mode_t mode) { if (healthHelper.healthTable->isFaulty(object)) { diff --git a/tmtc b/tmtc index 13014eb2..9720fcdd 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 13014eb25053368f4fb9a445788aba71ff98de19 +Subproject commit 9720fcddecb04b228dc5eb0d064f15a12ef8daca