From b9bf245d680daa82cd83a9d30d99ab9b09cc4f23 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 25 May 2021 13:53:03 +0200 Subject: [PATCH] some bugfixes for adis datasets --- bsp_linux_board/ObjectFactory.cpp | 5 + fsfw_hal | 2 +- linux/fsfwconfig/FSFWConfig.h | 2 + linux/fsfwconfig/OBSWConfig.h | 4 +- mission/devices/GyroADIS16507Handler.cpp | 109 +++++++++++------- mission/devices/GyroADIS16507Handler.h | 5 + mission/devices/GyroL3GD20Handler.cpp | 4 +- mission/devices/GyroL3GD20Handler.h | 2 +- .../GyroADIS16507Definitions.h | 14 +-- 9 files changed, 89 insertions(+), 58 deletions(-) diff --git a/bsp_linux_board/ObjectFactory.cpp b/bsp_linux_board/ObjectFactory.cpp index 04dd6f18..21a2ec07 100644 --- a/bsp_linux_board/ObjectFactory.cpp +++ b/bsp_linux_board/ObjectFactory.cpp @@ -123,6 +123,11 @@ void ObjectFactory::produce(){ #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, + "GYRO_0_ADIS", gpio::Direction::OUT, 1); + gpioIF->addGpios(gpioCookieAcsBoard); + std::string spiDev = "/dev/spidev0.0"; SpiCookie* 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, diff --git a/fsfw_hal b/fsfw_hal index 2893da04..c2b42318 160000 --- a/fsfw_hal +++ b/fsfw_hal @@ -1 +1 @@ -Subproject commit 2893da047bc695fcb02b0f9567cef0366a37793a +Subproject commit c2b4231802bcc25e1a2588ab4d12c8ecf64d2c47 diff --git a/linux/fsfwconfig/FSFWConfig.h b/linux/fsfwconfig/FSFWConfig.h index 8095bd12..8ec01cb9 100644 --- a/linux/fsfwconfig/FSFWConfig.h +++ b/linux/fsfwconfig/FSFWConfig.h @@ -68,4 +68,6 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6; static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124; } +#define FSFW_HAL_LINUX_SPI_WIRETAPPING 0 + #endif /* CONFIG_FSFWCONFIG_H_ */ diff --git a/linux/fsfwconfig/OBSWConfig.h b/linux/fsfwconfig/OBSWConfig.h index 5d11cb3b..c4441eef 100644 --- a/linux/fsfwconfig/OBSWConfig.h +++ b/linux/fsfwconfig/OBSWConfig.h @@ -21,8 +21,6 @@ debugging. */ #define OBSW_ADD_TEST_CODE 1 #define OBSW_ADD_TEST_PST 1 -#define FSFW_HAL_LINUX_SPI_WIRETAPPING 0 - #define TEST_LIBGPIOD 0 #define TEST_RADIATION_SENSOR_HANDLER 1 #define TEST_SUS_HANDLER 1 @@ -37,6 +35,8 @@ debugging. */ #define ACU_DEBUG 0 #define SYRLINKS_DEBUG 0 #define IMQT_DEBUG 0 +#define ADIS16507_DEBUG 1 +#define L3GD20_GYRO_DEBUG 0 #define DEBUG_RAD_SENSOR 1 #define DEBUG_SUS 1 diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp index d8ce106b..e1c4301b 100644 --- a/mission/devices/GyroADIS16507Handler.cpp +++ b/mission/devices/GyroADIS16507Handler.cpp @@ -15,6 +15,10 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, CookieIF * comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), configDataset(this) { +#if ADIS16507_DEBUG == 1 + debugDivider = new PeriodicOperationDivider(5); +#endif + #if OBSW_ADIS16507_LINUX_COM_IF == 1 SpiCookie* cookie = dynamic_cast(comCookie); if(cookie != nullptr) { @@ -30,6 +34,9 @@ void GyroADIS16507Handler::doStartUp() { internalState = InternalState::IDLE; } } + if(internalState == InternalState::IDLE) { + setMode(MODE_NORMAL); + } } void GyroADIS16507Handler::doShutDown() { @@ -103,7 +110,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id, switch(id) { case(ADIS16507::READ_OUT_CONFIG): { PoolReadGuard rg(&configDataset); - uint16_t readProdId = packet[8] << 8 | packet[9]; + uint16_t readProdId = packet[10] << 8 | packet[11]; if (readProdId != ADIS16507::PROD_ID) { #if OBSW_VERBOSE_LEVEL >= 1 sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!" @@ -111,10 +118,10 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id, #endif return HasReturnvaluesIF::RETURN_FAILED; } - configDataset.diagStatReg.value = packet[0] << 8 | packet[1]; - configDataset.filterSetting.value = packet[2] << 8 | packet[3]; - configDataset.mscCtrlReg.value = packet[4] << 8 | packet[5]; - configDataset.decRateReg.value = packet[6] << 8 | packet[7]; + configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; + configDataset.filterSetting.value = packet[4] << 8 | packet[5]; + configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7]; + configDataset.decRateReg.value = packet[8] << 8 | packet[9]; configDataset.setValidity(true, true); if(internalState == InternalState::STARTUP) { commandExecuted = true; @@ -162,33 +169,44 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) { } configDataset.diagStatReg.commit(); - PoolReadGuard pg(&primaryDataset); - int16_t angVelocXRaw = packet[4] << 8 | packet[5]; - primaryDataset.angVelocX.value = static_cast(angVelocXRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; - int16_t angVelocYRaw = packet[6] << 8 | packet[7]; - primaryDataset.angVelocY.value = static_cast(angVelocYRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; - int16_t angVelocZRaw = packet[8] << 8 | packet[9]; - primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; + { + PoolReadGuard pg(&primaryDataset); + int16_t angVelocXRaw = packet[4] << 8 | packet[5]; + primaryDataset.angVelocX.value = static_cast(angVelocXRaw) / INT16_MAX * + ADIS16507::GYRO_RANGE; + int16_t angVelocYRaw = packet[6] << 8 | packet[7]; + primaryDataset.angVelocY.value = static_cast(angVelocYRaw) / INT16_MAX * + ADIS16507::GYRO_RANGE; + int16_t angVelocZRaw = packet[8] << 8 | packet[9]; + primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) / INT16_MAX * + ADIS16507::GYRO_RANGE; - int16_t accelXRaw = packet[10] << 8 | packet[11]; - primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; - int16_t accelYRaw = packet[12] << 8 | packet[13]; - primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; - int16_t accelZRaw = packet[14] << 8 | packet[15]; - primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; + int16_t accelXRaw = packet[10] << 8 | packet[11]; + primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * + ADIS16507::GYRO_RANGE; + int16_t accelYRaw = packet[12] << 8 | packet[13]; + primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * + ADIS16507::GYRO_RANGE; + int16_t accelZRaw = packet[14] << 8 | packet[15]; + primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * + ADIS16507::GYRO_RANGE; - int16_t temperatureRaw = packet[16] << 8 | packet[17]; - primaryDataset.temperature.value = static_cast(temperatureRaw) * 0.1; + 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); + } - // Ignore data counter for now +#if ADIS16507_DEBUG == 1 + if(debugDivider->checkAndIncrement()) { + sif::info << "GyroADIS16507Handler: Angular velocities in degrees per second:" << + std::endl; + sif::info << "X: " << primaryDataset.angVelocX.value << " \xC2\xB0" << std::endl; + sif::info << "Y: " << primaryDataset.angVelocY.value << " \xC2\xB0" << std::endl; + sif::info << "Z: " << primaryDataset.angVelocZ.value << " \xC2\xB0" << std::endl; + } +#endif - primaryDataset.setValidity(true, true); break; } case(BurstModes::BURST_32_BURST_SEL_0): { @@ -219,7 +237,9 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry({0.0})); localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry({0.0})); localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS16507::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry()); localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry()); localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry()); localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry()); @@ -294,17 +314,20 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie * #endif } + if(gpioId != gpio::NO_GPIO) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != RETURN_OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; +#endif + return result; + } + } + size_t idx = 0; while(idx < sendLen) { // Pull SPI CS low. For now, no support for active high given if(gpioId != gpio::NO_GPIO) { - result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != RETURN_OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; -#endif - return result; - } gpioIF->pullLow(gpioId); } @@ -321,21 +344,19 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie * if(gpioId != gpio::NO_GPIO) { gpioIF->pullHigh(gpioId); - result = mutex->unlockMutex(); - if (result != RETURN_OK) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::getSendSuccess: Failed to unlock mutex" << std::endl; -#endif - return result; - } } idx += 2; if(idx < sendLen) { usleep(ADIS16507::STALL_TIME_MICROSECONDS); } - sendData += 2; - cookie->getTransferStructHandle()->rx_buf += 2; + spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle(); + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + } + + if(gpioId != gpio::NO_GPIO) { + mutex->unlockMutex(); } } } diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h index 45e9f153..fbb9f7bf 100644 --- a/mission/devices/GyroADIS16507Handler.h +++ b/mission/devices/GyroADIS16507Handler.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_ #define MISSION_DEVICES_GYROADIS16507HANDLER_H_ +#include #include "OBSWConfig.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "devicedefinitions/GyroADIS16507Definitions.h" @@ -61,6 +62,10 @@ private: const uint8_t *sendData, size_t sendLen, void* args); #endif +#if ADIS16507_DEBUG == 1 + PeriodicOperationDivider* debugDivider; +#endif + ReturnValue_t handleSensorData(const uint8_t* packet); }; diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 35899035..5820d930 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -7,7 +7,7 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC CookieIF *comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this) { -#if OBSW_VERBOSE_LEVEL >= 1 +#if L3GD20_GYRO_DEBUG == 1 debugDivider = new PeriodicOperationDivider(5); #endif } @@ -197,7 +197,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; -#if OBSW_VERBOSE_LEVEL >= 1 +#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 diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index 442e28f8..7f5ee1c5 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -70,7 +70,7 @@ private: float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; -#if OBSW_VERBOSE_LEVEL >= 1 +#if L3GD20_GYRO_DEBUG == 1 PeriodicOperationDivider* debugDivider = nullptr; #endif }; diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h index f58196a0..1328c99e 100644 --- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h +++ b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h @@ -41,8 +41,8 @@ static constexpr uint16_t BURST_SEL_BIT = 1 << 8; static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7; static constexpr uint16_t POINT_PERCUSSION_COMPENSATION_BIT = 1 << 6; -static constexpr size_t CONFIG_READOUT_SIZE = 8; -static constexpr size_t SENSOR_READOUT_SIZE = 22; +static constexpr size_t CONFIG_READOUT_SIZE = 10 + 2; +static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2; static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; @@ -54,10 +54,7 @@ enum PrimaryPoolIds: lp_id_t { ACCELERATION_X, ACCELERATION_Y, ACCELERATION_Z, - TEMPERATURE -}; - -enum ConfigPoolIds: lp_id_t { + TEMPERATURE, DIAG_STAT_REGISTER, FILTER_SETTINGS, MSC_CTRL_REGISTER, @@ -113,11 +110,12 @@ public: /** Constructor for data users like controllers */ AdisGyroConfigDataset(object_id_t adisId): - StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) { + StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_CFG_DATASET_ID)) { setAllVariablesReadOnly(); } - lp_var_t diagStatReg = lp_var_t(sid.objectId, ADIS16507::DIAG_STAT_REG); + lp_var_t diagStatReg = lp_var_t(sid.objectId, + ADIS16507::DIAG_STAT_REGISTER); lp_var_t filterSetting = lp_var_t(sid.objectId, ADIS16507::FILTER_SETTINGS); lp_var_t mscCtrlReg = lp_var_t(sid.objectId, ADIS16507::MSC_CTRL_REGISTER); lp_var_t decRateReg = lp_var_t(sid.objectId, ADIS16507::DEC_RATE_REGISTER);