adis handler fixes

This commit is contained in:
Robin Müller 2021-05-26 13:41:26 +02:00 committed by Robin Mueller
parent 7bab7ffb75
commit ccce16dd8f

View File

@ -29,6 +29,7 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
}
void GyroADIS16507Handler::doStartUp() {
// Initial 310 ms start up time after power-up
if(internalState == InternalState::STARTUP) {
if(not commandExecuted) {
breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
@ -36,9 +37,11 @@ void GyroADIS16507Handler::doStartUp() {
}
if(breakCountdown.hasTimedOut()) {
internalState = InternalState::CONFIG;
commandExecuted = false;
}
}
// Read all configuration registers first
if(internalState == InternalState::CONFIG) {
if(commandExecuted) {
commandExecuted = false;
@ -48,6 +51,7 @@ void GyroADIS16507Handler::doStartUp() {
if(internalState == InternalState::IDLE) {
setMode(MODE_NORMAL);
// setMode(MODE_ON);
}
}
@ -62,11 +66,14 @@ ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *
ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch(internalState) {
case(InternalState::STARTUP): {
case(InternalState::CONFIG): {
*id = ADIS16507::READ_OUT_CONFIG;
buildCommandFromCommand(*id, nullptr, 0);
break;
}
case(InternalState::STARTUP): {
break;
}
default: {
/* Might be a configuration error. */
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
@ -202,7 +209,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
configDataset.decRateReg.value = packet[8] << 8 | packet[9];
configDataset.setValidity(true, true);
if(internalState == InternalState::STARTUP) {
if(internalState == InternalState::CONFIG) {
commandExecuted = true;
}
break;