diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp index 0965e265..e3e418f7 100644 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -20,10 +20,6 @@ GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t dev primaryDataset(this), configDataset(this), breakCountdown() { -#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(5); -#endif - #if OBSW_ADIS1650X_LINUX_COM_IF == 1 SpiCookie *cookie = dynamic_cast(comCookie); if (cookie != nullptr) { @@ -101,7 +97,7 @@ ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t de switch (deviceCommand) { case (ADIS1650X::READ_OUT_CONFIG): { this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE; - uint8_t regList[5]; + uint8_t regList[5] = {}; regList[0] = ADIS1650X::DIAG_STAT_REG; regList[1] = ADIS1650X::FILTER_CTRL_REG; regList[2] = ADIS1650X::MSC_CTRL_REG; @@ -305,18 +301,18 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { primaryDataset.setValidity(true, true); } -#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1 - 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; + 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; + } } -#endif break; } @@ -446,6 +442,9 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * } 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) { @@ -471,11 +470,12 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * if (idx < sendLen) { usleep(ADIS1650X::STALL_TIME_MICROSECONDS); } - spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle(); + transferStruct->tx_buf += 2; transferStruct->rx_buf += 2; } - + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; if (gpioId != gpio::NO_GPIO) { mutex->unlockMutex(); } @@ -486,4 +486,9 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * 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/GyroADIS1650XHandler.h b/mission/devices/GyroADIS1650XHandler.h index 1db0f3a8..55de1477 100644 --- a/mission/devices/GyroADIS1650XHandler.h +++ b/mission/devices/GyroADIS1650XHandler.h @@ -23,6 +23,7 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, ADIS1650X::Type type); + void enablePeriodicPrintouts(bool enable, uint8_t divider); void setToGoToNormalModeImmediately(); // DeviceHandlerBase abstract function implementation @@ -69,13 +70,13 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { size_t sendLen, void *args); #endif -#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1 - PeriodicOperationDivider *debugDivider; -#endif Countdown breakCountdown; void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo); ReturnValue_t handleSensorData(const uint8_t *packet); + + bool periodicPrintout = false; + PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); }; #endif /* MISSION_DEVICES_GYROADIS16507HANDLER_H_ */