added a few more commands

This commit is contained in:
Robin Müller 2021-05-26 13:11:48 +02:00
parent b03c6f686e
commit a73d31b0fb
No known key found for this signature in database
GPG Key ID: FC76078F520434A5
3 changed files with 56 additions and 1 deletions

View File

@ -14,7 +14,7 @@
GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId, GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie): object_id_t deviceCommunication, CookieIF * comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
configDataset(this) { configDataset(this), breakCountdown(0) {
#if ADIS16507_DEBUG == 1 #if ADIS16507_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5); debugDivider = new PeriodicOperationDivider(5);
#endif #endif
@ -87,6 +87,37 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
this->rawPacket = commandBuffer.data(); this->rawPacket = commandBuffer.data();
break; break;
} }
case(ADIS16507::SELF_TEST_SENSORS): {
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00);
break;
}
case(ADIS16507::SELF_TEST_MEMORY): {
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00);
break;
}
case(ADIS16507::UPDATE_NV_CONFIGURATION): {
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
break;
}
case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00);
break;
}
case(ADIS16507::SW_RESET): {
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00);
break;
}
case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
#if OBSW_VERBOSE_LEVEL >= 1
PoolReadGuard pg(&configDataset);
sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) <<
std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" <<
configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" <<
configDataset.filterSetting.value << " | DEC_RATE: 0x" <<
configDataset.decRateReg.value << std::setfill(' ') << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
}
} }
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
@ -94,6 +125,8 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
void GyroADIS16507Handler::fillCommandAndReplyMap() { void GyroADIS16507Handler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset); insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset); insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1);
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1);
} }
ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize, ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
@ -223,6 +256,17 @@ uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t mode
return 5000; return 5000;
} }
void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
uint8_t valueTwo) {
uint8_t secondReg = startReg + 1;
startReg |= ADIS16507::WRITE_MASK;
secondReg |= ADIS16507::WRITE_MASK;
commandBuffer[0] = startReg;
commandBuffer[1] = valueOne;
commandBuffer[2] = secondReg;
commandBuffer[3] = valueTwo;
}
void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) { void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
for(size_t idx = 0; idx < len; idx++) { for(size_t idx = 0; idx < len; idx++) {
commandBuffer[idx * 2] = regList[idx]; commandBuffer[idx * 2] = regList[idx];

View File

@ -65,6 +65,8 @@ private:
#if ADIS16507_DEBUG == 1 #if ADIS16507_DEBUG == 1
PeriodicOperationDivider* debugDivider; PeriodicOperationDivider* debugDivider;
#endif #endif
Countdown breakCountdown;
void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
ReturnValue_t handleSensorData(const uint8_t* packet); ReturnValue_t handleSensorData(const uint8_t* packet);
}; };

View File

@ -8,6 +8,7 @@
namespace ADIS16507 { namespace ADIS16507 {
static constexpr size_t MAXIMUM_REPLY_SIZE = 64; static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
static constexpr uint8_t WRITE_MASK = 0b1000'0000;
static constexpr uint32_t GYRO_RANGE = 125; static constexpr uint32_t GYRO_RANGE = 125;
static constexpr uint32_t ACCELEROMETER_RANGE = 392; static constexpr uint32_t ACCELEROMETER_RANGE = 392;
@ -47,6 +48,14 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;
static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;
enum GlobCmds: uint8_t {
FACTORY_CALIBRATION = 0b0000'0010,
SENSOR_SELF_TEST = 0b0000'0100,
FLASH_MEMORY_UPDATE = 0b0000'1000,
FLASH_MEMORY_TEST = 0b0001'0000,
SOFTWARE_RESET = 0b1000'0000
};
enum PrimaryPoolIds: lp_id_t { enum PrimaryPoolIds: lp_id_t {
ANG_VELOC_X, ANG_VELOC_X,
ANG_VELOC_Y, ANG_VELOC_Y,