diff --git a/bsp_linux_board/ObjectFactory.cpp b/bsp_linux_board/ObjectFactory.cpp index cefce4cd..648d1376 100644 --- a/bsp_linux_board/ObjectFactory.cpp +++ b/bsp_linux_board/ObjectFactory.cpp @@ -11,11 +11,12 @@ #include #include -#include +//#include #include #include #include #include +#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" #include #include @@ -59,7 +60,7 @@ void ObjectFactory::produce(void* args){ new UdpTcPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE); GpioIF* gpioIF = new LinuxLibgpioIF(objects::GPIO_IF); - + GpioCookie* gpioCookie = nullptr; #if RPI_ADD_SPI_TEST == 1 new SpiTestClass(objects::SPI_TEST, gpioIF); #endif @@ -80,27 +81,31 @@ void ObjectFactory::produce(void* args){ new SpiComIF(objects::SPI_COM_IF, gpioIF); + std::string spiDev; + SpiCookie* spiCookie = nullptr; + #if RPI_TEST_ACS_BOARD == 1 - - GpioCookie* gpioCookieAcsBoard = new GpioCookie(); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_0_LIS3_CS, gpio::MGM_0_BCM_PIN, + if(gpioCookie == nullptr) { + gpioCookie = new GpioCookie(); + } + gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_0_LIS3_CS, gpio::MGM_0_BCM_PIN, "MGM_0_LIS3", gpio::Direction::OUT, 1); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_1_RM3100_CS, gpio::MGM_1_BCM_PIN, + gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_1_RM3100_CS, gpio::MGM_1_BCM_PIN, "MGM_1_RM3100", gpio::Direction::OUT, 1); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_2_LIS3_CS, gpio::MGM_2_BCM_PIN, + gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_2_LIS3_CS, gpio::MGM_2_BCM_PIN, "MGM_2_LIS3", gpio::Direction::OUT, 1); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_3_RM3100_CS, gpio::MGM_3_BCM_PIN, + gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_3_RM3100_CS, gpio::MGM_3_BCM_PIN, "MGM_3_RM3100", gpio::Direction::OUT, 1); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN, + gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN, "GYRO_0_ADIS", gpio::Direction::OUT, 1); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_1_L3G_CS, gpio::GYRO_1_BCM_PIN, + gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_1_L3G_CS, gpio::GYRO_1_BCM_PIN, "GYRO_1_L3G", gpio::Direction::OUT, 1); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_2_L3G_CS, gpio::GYRO_2_BCM_PIN, + gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_2_L3G_CS, gpio::GYRO_2_BCM_PIN, "GYRO_2_L3G", gpio::Direction::OUT, 1); - gpioIF->addGpios(gpioCookieAcsBoard); + gpioIF->addGpios(gpioCookie); - std::string spiDev = "/dev/spidev0.0"; - SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, + spiDev = "/dev/spidev0.0"; + spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie); @@ -121,13 +126,15 @@ void ObjectFactory::produce(void* args){ #endif /* RPI_TEST_ACS_BOARD == 1 */ #if RPI_TEST_ADIS16507 == 1 - GpioCookie* gpioCookieAcsBoard = new GpioCookie(); - gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN, + if(gpioCookie == nullptr) { + gpioCookie = new GpioCookie(); + } + gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN, "GYRO_0_ADIS", gpio::Direction::OUT, 1); - gpioIF->addGpios(gpioCookieAcsBoard); + gpioIF->addGpios(gpioCookie); - std::string spiDev = "/dev/spidev0.0"; - SpiCookie* spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev, + spiDev = "/dev/spidev0.0"; + spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev, ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED, nullptr, nullptr); auto adisGyroHandler = new GyroADIS16507Handler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF, spiCookie); diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 5820d930..a2850dfa 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -1,260 +1,260 @@ -#include "GyroL3GD20Handler.h" -#include - -#include - -GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie), - dataset(this) { -#if L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(5); -#endif -} - -GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} - -void GyroHandlerL3GD20H::doStartUp() { - if(internalState == InternalState::NONE) { - internalState = InternalState::CONFIGURE; - } - - if(internalState == InternalState::CONFIGURE) { - if(commandExecuted) { - internalState = InternalState::CHECK_REGS; - commandExecuted = false; - } - } - - if(internalState == InternalState::CHECK_REGS) { - if(commandExecuted) { - internalState = InternalState::NORMAL; -#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 - setMode(MODE_NORMAL); -#else - setMode(_MODE_TO_ON); -#endif - commandExecuted = false; - } - } -} - -void GyroHandlerL3GD20H::doShutDown() { - setMode(_MODE_POWER_DOWN); -} - -ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - switch(internalState) { - case(InternalState::NONE): - case(InternalState::NORMAL): { - return HasReturnvaluesIF::RETURN_OK; - } - case(InternalState::CONFIGURE): { - *id = L3GD20H::CONFIGURE_CTRL_REGS; - uint8_t command [5]; - command[0] = L3GD20H::CTRL_REG_1_VAL; - command[1] = L3GD20H::CTRL_REG_2_VAL; - command[2] = L3GD20H::CTRL_REG_3_VAL; - command[3] = L3GD20H::CTRL_REG_4_VAL; - command[4] = L3GD20H::CTRL_REG_5_VAL; - return buildCommandFromCommand(*id, command, 5); - } - case(InternalState::CHECK_REGS): { - *id = L3GD20H::READ_REGS; - return buildCommandFromCommand(*id, nullptr, 0); - } - default: - /* Might be a configuration error. */ - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << - std::endl; - return HasReturnvaluesIF::RETURN_OK; - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = L3GD20H::READ_REGS; - return buildCommandFromCommand(*id, nullptr, 0); -} - -ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) { - switch(deviceCommand) { - case(L3GD20H::READ_REGS): { - commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | - L3GD20H::READ_MASK; - - std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN); - rawPacket = commandBuffer; - rawPacketLen = L3GD20H::READ_LEN + 1; - break; - } - case(L3GD20H::CONFIGURE_CTRL_REGS): { - commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK; - if(commandData == nullptr or commandDataLen != 5) { - return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; - } - - ctrlReg1Value = commandData[0]; - ctrlReg2Value = commandData[1]; - ctrlReg3Value = commandData[2]; - ctrlReg4Value = commandData[3]; - ctrlReg5Value = commandData[4]; - - bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1; - bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0; - - if(not fsH and not fsL) { - scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; - } - else if(not fsH and fsL) { - scaleFactor = static_cast(L3GD20H::RANGE_DPS_01) / INT16_MAX; - } - else { - scaleFactor = static_cast(L3GD20H::RANGE_DPS_11) / INT16_MAX; - } - - commandBuffer[1] = ctrlReg1Value; - commandBuffer[2] = ctrlReg2Value; - commandBuffer[3] = ctrlReg3Value; - commandBuffer[4] = ctrlReg4Value; - commandBuffer[5] = ctrlReg5Value; - - rawPacket = commandBuffer; - rawPacketLen = 6; - break; - } - case(L3GD20H::READ_CTRL_REGS): { - commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | - L3GD20H::READ_MASK; - - std::memset(commandBuffer + 1, 0, 5); - rawPacket = commandBuffer; - rawPacketLen = 6; - break; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, - 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; - - /* Data with SPI Interface has always this answer */ - if (start[0] == 0b11111111) { - return HasReturnvaluesIF::RETURN_OK; - } - return DeviceHandlerIF::INVALID_DATA; -} - -ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - switch(id) { - case(L3GD20H::CONFIGURE_CTRL_REGS): { - commandExecuted = true; - break; - } - case(L3GD20H::READ_CTRL_REGS): { - if(packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and - packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and - packet[5] == ctrlReg5Value) { - commandExecuted = true; - } - else { - /* Attempt reconfiguration. */ - internalState = InternalState::CONFIGURE; - return DeviceHandlerIF::DEVICE_REPLY_INVALID; - } - break; - } - case(L3GD20H::READ_REGS): { - if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and - packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and - packet[5] != ctrlReg5Value) { - return DeviceHandlerIF::DEVICE_REPLY_INVALID; - } - else { - if(internalState == InternalState::CHECK_REGS) { - commandExecuted = true; - } - } - - statusReg = packet[L3GD20H::STATUS_IDX]; - - float angVelocX = (packet[L3GD20H::OUT_X_H] << 8 | - packet[L3GD20H::OUT_X_L]) * scaleFactor; - float angVelocY = (packet[L3GD20H::OUT_Y_H] << 8 | - packet[L3GD20H::OUT_Y_L]) * scaleFactor; - float angVelocZ = (packet[L3GD20H::OUT_Z_H] << 8 | - packet[L3GD20H::OUT_Z_L]) * scaleFactor; - - int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; - float temperature = 25.0 + temperaturOffset; -#if L3GD20_GYRO_DEBUG == 1 - if(debugDivider->checkAndIncrement()) { - /* Set terminal to utf-8 if there is an issue with micro printout. */ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "GyroHandlerL3GD20H: Angular velocities in degrees per second:" << - std::endl; - sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl; - sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl; - sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl; -#else - sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n"); - sif::printInfo("X: %f " "\xC2\xB0" "T\n", angVelocX); - sif::printInfo("Y: %f " "\xC2\xB0" "T\n", angVelocY); - sif::printInfo("Z: %f " "\xC2\xB0" "T\n", angVelocZ); -#endif - } -#endif - - PoolReadGuard readSet(&dataset); - if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - dataset.angVelocX = angVelocX; - dataset.angVelocY = angVelocY; - dataset.angVelocZ = angVelocZ; - dataset.temperature = temperature; - dataset.setValidity(true, true); - } - break; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } - return result; -} - - -uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 10000; -} - -ReturnValue_t GyroHandlerL3GD20H::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})); - return HasReturnvaluesIF::RETURN_OK; -} - -void GyroHandlerL3GD20H::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); - insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); - insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); -} - -void GyroHandlerL3GD20H::modeChanged() { - internalState = InternalState::NONE; -} +//#include "GyroL3GD20Handler.h" +//#include +// +//#include +// +//GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, +// CookieIF *comCookie): +// DeviceHandlerBase(objectId, deviceCommunication, comCookie), +// dataset(this) { +//#if L3GD20_GYRO_DEBUG == 1 +// debugDivider = new PeriodicOperationDivider(5); +//#endif +//} +// +//GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} +// +//void GyroHandlerL3GD20H::doStartUp() { +// if(internalState == InternalState::NONE) { +// internalState = InternalState::CONFIGURE; +// } +// +// if(internalState == InternalState::CONFIGURE) { +// if(commandExecuted) { +// internalState = InternalState::CHECK_REGS; +// commandExecuted = false; +// } +// } +// +// if(internalState == InternalState::CHECK_REGS) { +// if(commandExecuted) { +// internalState = InternalState::NORMAL; +//#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 +// setMode(MODE_NORMAL); +//#else +// setMode(_MODE_TO_ON); +//#endif +// commandExecuted = false; +// } +// } +//} +// +//void GyroHandlerL3GD20H::doShutDown() { +// setMode(_MODE_POWER_DOWN); +//} +// +//ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { +// switch(internalState) { +// case(InternalState::NONE): +// case(InternalState::NORMAL): { +// return HasReturnvaluesIF::RETURN_OK; +// } +// case(InternalState::CONFIGURE): { +// *id = L3GD20H::CONFIGURE_CTRL_REGS; +// uint8_t command [5]; +// command[0] = L3GD20H::CTRL_REG_1_VAL; +// command[1] = L3GD20H::CTRL_REG_2_VAL; +// command[2] = L3GD20H::CTRL_REG_3_VAL; +// command[3] = L3GD20H::CTRL_REG_4_VAL; +// command[4] = L3GD20H::CTRL_REG_5_VAL; +// return buildCommandFromCommand(*id, command, 5); +// } +// case(InternalState::CHECK_REGS): { +// *id = L3GD20H::READ_REGS; +// return buildCommandFromCommand(*id, nullptr, 0); +// } +// default: +// /* Might be a configuration error. */ +// sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << +// std::endl; +// return HasReturnvaluesIF::RETURN_OK; +// } +// return HasReturnvaluesIF::RETURN_OK; +//} +// +//ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { +// *id = L3GD20H::READ_REGS; +// return buildCommandFromCommand(*id, nullptr, 0); +//} +// +//ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( +// DeviceCommandId_t deviceCommand, const uint8_t *commandData, +// size_t commandDataLen) { +// switch(deviceCommand) { +// case(L3GD20H::READ_REGS): { +// commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | +// L3GD20H::READ_MASK; +// +// std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN); +// rawPacket = commandBuffer; +// rawPacketLen = L3GD20H::READ_LEN + 1; +// break; +// } +// case(L3GD20H::CONFIGURE_CTRL_REGS): { +// commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK; +// if(commandData == nullptr or commandDataLen != 5) { +// return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; +// } +// +// ctrlReg1Value = commandData[0]; +// ctrlReg2Value = commandData[1]; +// ctrlReg3Value = commandData[2]; +// ctrlReg4Value = commandData[3]; +// ctrlReg5Value = commandData[4]; +// +// bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1; +// bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0; +// +// if(not fsH and not fsL) { +// scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; +// } +// else if(not fsH and fsL) { +// scaleFactor = static_cast(L3GD20H::RANGE_DPS_01) / INT16_MAX; +// } +// else { +// scaleFactor = static_cast(L3GD20H::RANGE_DPS_11) / INT16_MAX; +// } +// +// commandBuffer[1] = ctrlReg1Value; +// commandBuffer[2] = ctrlReg2Value; +// commandBuffer[3] = ctrlReg3Value; +// commandBuffer[4] = ctrlReg4Value; +// commandBuffer[5] = ctrlReg5Value; +// +// rawPacket = commandBuffer; +// rawPacketLen = 6; +// break; +// } +// case(L3GD20H::READ_CTRL_REGS): { +// commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | +// L3GD20H::READ_MASK; +// +// std::memset(commandBuffer + 1, 0, 5); +// rawPacket = commandBuffer; +// rawPacketLen = 6; +// break; +// } +// default: +// return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; +// } +// return HasReturnvaluesIF::RETURN_OK; +//} +// +//ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, +// 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; +// +// /* Data with SPI Interface has always this answer */ +// if (start[0] == 0b11111111) { +// return HasReturnvaluesIF::RETURN_OK; +// } +// return DeviceHandlerIF::INVALID_DATA; +//} +// +//ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, +// const uint8_t *packet) { +// ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; +// switch(id) { +// case(L3GD20H::CONFIGURE_CTRL_REGS): { +// commandExecuted = true; +// break; +// } +// case(L3GD20H::READ_CTRL_REGS): { +// if(packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and +// packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and +// packet[5] == ctrlReg5Value) { +// commandExecuted = true; +// } +// else { +// /* Attempt reconfiguration. */ +// internalState = InternalState::CONFIGURE; +// return DeviceHandlerIF::DEVICE_REPLY_INVALID; +// } +// break; +// } +// case(L3GD20H::READ_REGS): { +// if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and +// packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and +// packet[5] != ctrlReg5Value) { +// return DeviceHandlerIF::DEVICE_REPLY_INVALID; +// } +// else { +// if(internalState == InternalState::CHECK_REGS) { +// commandExecuted = true; +// } +// } +// +// statusReg = packet[L3GD20H::STATUS_IDX]; +// +// float angVelocX = (packet[L3GD20H::OUT_X_H] << 8 | +// packet[L3GD20H::OUT_X_L]) * scaleFactor; +// float angVelocY = (packet[L3GD20H::OUT_Y_H] << 8 | +// packet[L3GD20H::OUT_Y_L]) * scaleFactor; +// float angVelocZ = (packet[L3GD20H::OUT_Z_H] << 8 | +// packet[L3GD20H::OUT_Z_L]) * scaleFactor; +// +// int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; +// float temperature = 25.0 + temperaturOffset; +//#if L3GD20_GYRO_DEBUG == 1 +// if(debugDivider->checkAndIncrement()) { +// /* Set terminal to utf-8 if there is an issue with micro printout. */ +//#if FSFW_CPP_OSTREAM_ENABLED == 1 +// sif::info << "GyroHandlerL3GD20H: Angular velocities in degrees per second:" << +// std::endl; +// sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl; +// sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl; +// sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl; +//#else +// sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n"); +// sif::printInfo("X: %f " "\xC2\xB0" "T\n", angVelocX); +// sif::printInfo("Y: %f " "\xC2\xB0" "T\n", angVelocY); +// sif::printInfo("Z: %f " "\xC2\xB0" "T\n", angVelocZ); +//#endif +// } +//#endif +// +// PoolReadGuard readSet(&dataset); +// if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { +// dataset.angVelocX = angVelocX; +// dataset.angVelocY = angVelocY; +// dataset.angVelocZ = angVelocZ; +// dataset.temperature = temperature; +// dataset.setValidity(true, true); +// } +// break; +// } +// default: +// return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; +// } +// return result; +//} +// +// +//uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { +// return 10000; +//} +// +//ReturnValue_t GyroHandlerL3GD20H::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})); +// return HasReturnvaluesIF::RETURN_OK; +//} +// +//void GyroHandlerL3GD20H::fillCommandAndReplyMap() { +// insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); +// insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); +// insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); +//} +// +//void GyroHandlerL3GD20H::modeChanged() { +// internalState = InternalState::NONE; +//} diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index 7f5ee1c5..b2e03b35 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -1,80 +1,80 @@ -#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ -#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ - -#include "devicedefinitions/GyroL3GD20Definitions.h" -#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 GyroHandlerL3GD20H: public DeviceHandlerBase { -public: - GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie); - virtual ~GyroHandlerL3GD20H(); - -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; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) override; - - void fillCommandAndReplyMap() override; - void modeChanged() override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; - ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, - LocalDataPoolManager &poolManager) override; - -private: - GyroPrimaryDataset dataset; - - enum class InternalState { - NONE, - CONFIGURE, - CHECK_REGS, - NORMAL - }; - InternalState internalState = InternalState::NONE; - bool commandExecuted = false; - - uint8_t statusReg = 0; - - 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; - - uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; - - float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; - -#if L3GD20_GYRO_DEBUG == 1 - PeriodicOperationDivider* debugDivider = nullptr; -#endif -}; - - - -#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ +//#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +//#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ +// +//#include "devicedefinitions/GyroL3GD20Definitions.h" +//#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 GyroHandlerL3GD20H: public DeviceHandlerBase { +//public: +// GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, +// CookieIF* comCookie); +// virtual ~GyroHandlerL3GD20H(); +// +//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; +// ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, +// const uint8_t *packet) override; +// +// void fillCommandAndReplyMap() override; +// void modeChanged() override; +// uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; +// ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, +// LocalDataPoolManager &poolManager) override; +// +//private: +// GyroPrimaryDataset dataset; +// +// enum class InternalState { +// NONE, +// CONFIGURE, +// CHECK_REGS, +// NORMAL +// }; +// InternalState internalState = InternalState::NONE; +// bool commandExecuted = false; +// +// uint8_t statusReg = 0; +// +// 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; +// +// uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; +// +// float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; +// +//#if L3GD20_GYRO_DEBUG == 1 +// PeriodicOperationDivider* debugDivider = nullptr; +//#endif +//}; +// +// +// +//#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */