adis handler fixes
This commit is contained in:
parent
7bab7ffb75
commit
ccce16dd8f
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user