diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp index 1413bfca..08a0b3ef 100644 --- a/mission/devices/GyroADIS16507Handler.cpp +++ b/mission/devices/GyroADIS16507Handler.cpp @@ -6,6 +6,8 @@ #include "fsfw_hal/linux/spi/SpiComIF.h" #endif +#include + GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, CookieIF * comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), @@ -79,6 +81,8 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de } void GyroADIS16507Handler::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset); + insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset); } ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize, @@ -112,6 +116,35 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id, break; } case(ADIS16507::READ_SENSOR_DATA): { + BurstModes burstMode = getBurstMode(); + switch(burstMode) { + case(BurstModes::BURST_16_BURST_SEL_1): + case(BurstModes::BURST_32_BURST_SEL_1): { + sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1" + " not implemented!" << std::endl; + return HasReturnvaluesIF::RETURN_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 < 22; idx ++) { + calcChecksum += packet[idx]; + } + if(checksum != calcChecksum) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::warning << "GyroADIS16507Handler::interpretDeviceReply: " + "Invalid checksum detected!" << std::endl; +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + + } + case(BurstModes::BURST_32_BURST_SEL_0): { + + } + } } @@ -147,6 +180,28 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool return HasReturnvaluesIF::RETURN_OK; } +GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() { + configDataset.mscCtrlReg.read(); + uint16_t currentCtrlReg = configDataset.mscCtrlReg.value; + configDataset.mscCtrlReg.commit(); + if((currentCtrlReg & ADIS16507::BURST_32_BIT) == ADIS16507::BURST_32_BIT) { + if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) { + return BurstModes::BURST_32_BURST_SEL_1; + } + else { + return BurstModes::BURST_32_BURST_SEL_0; + } + } + else { + if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) { + return BurstModes::BURST_16_BURST_SEL_1; + } + else { + return BurstModes::BURST_16_BURST_SEL_0; + } + } +} + #if OBSW_ADIS16507_LINUX_COM_IF == 1 ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, @@ -163,7 +218,7 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie * return comIf->performRegularSendOperation(cookie, sendData, sendLen); } case(ADIS16507::READ_OUT_CONFIG): { - + usleep(ADIS16507::STALL_TIME_MICROSECONDS); } } return HasReturnvaluesIF::RETURN_OK; diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h index a387675f..dc5cf189 100644 --- a/mission/devices/GyroADIS16507Handler.h +++ b/mission/devices/GyroADIS16507Handler.h @@ -42,11 +42,20 @@ private: IDLE }; + enum class BurstModes { + BURST_16_BURST_SEL_0, + BURST_16_BURST_SEL_1, + BURST_32_BURST_SEL_0, + BURST_32_BURST_SEL_1 + }; + InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; void prepareReadCommand(uint8_t* regList, size_t len); + BurstModes getBurstMode(); + #if OBSW_ADIS16507_LINUX_COM_IF == 1 static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData, size_t sendLen, void* args); diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h index bacc29cf..1a2d25a9 100644 --- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h +++ b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h @@ -12,6 +12,8 @@ static constexpr size_t MAXIMUM_REPLY_SIZE = 64; static constexpr uint32_t GYRO_RANGE = 125; static constexpr uint32_t ACCELEROMETER_RANGE = 392; +static constexpr uint32_t STALL_TIME_MICROSECONDS = 16; + static constexpr uint16_t PROD_ID = 16507; static constexpr std::array BURST_READ_ENABLE = {0x68, 0x00}; @@ -34,6 +36,11 @@ 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 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; +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;