meggert
72b5567f73
All checks were successful
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev This commit looks good
66 lines
1.9 KiB
C++
66 lines
1.9 KiB
C++
#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<adis1650x::RangMdlBitfield>((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;
|
|
}
|
|
}
|
|
}
|
|
|
|
void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf,
|
|
size_t maxLen) {
|
|
if (maxLen < 4) {
|
|
return;
|
|
}
|
|
outBuf[0] = regStart | adis1650x::WRITE_MASK;
|
|
outBuf[1] = val & 0xff;
|
|
outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK;
|
|
outBuf[3] = (val >> 8) & 0xff;
|
|
}
|