better reply result handling
This commit is contained in:
parent
fa94c67e99
commit
d42b6798e0
@ -24,6 +24,7 @@ will consitute of a breaking change warranting a new major release:
|
|||||||
SD card switch from 0 to 1 and vice-versa works without errors from persistent TM stores now.
|
SD card switch from 0 to 1 and vice-versa works without errors from persistent TM stores now.
|
||||||
- Add a way for the SUS polling to detect broken or off devices by checking the retrieved
|
- Add a way for the SUS polling to detect broken or off devices by checking the retrieved
|
||||||
temperature for the all-ones value (0x0fff).
|
temperature for the all-ones value (0x0fff).
|
||||||
|
- Better reply result handling for the ACS board devices.
|
||||||
|
|
||||||
## Changed
|
## Changed
|
||||||
|
|
||||||
|
@ -127,6 +127,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
|
|||||||
adis.ownReply.cfgWasSet = false;
|
adis.ownReply.cfgWasSet = false;
|
||||||
adis.ownReply.dataWasSet = false;
|
adis.ownReply.dataWasSet = false;
|
||||||
}
|
}
|
||||||
|
adis.replyResult = returnvalue::FAILED;
|
||||||
adis.mode = req->mode;
|
adis.mode = req->mode;
|
||||||
}
|
}
|
||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
@ -145,6 +146,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
|
|||||||
} else {
|
} else {
|
||||||
gyro.ownReply.cfgWasSet = false;
|
gyro.ownReply.cfgWasSet = false;
|
||||||
}
|
}
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
gyro.mode = req->mode;
|
gyro.mode = req->mode;
|
||||||
}
|
}
|
||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
@ -163,6 +165,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
|
|||||||
mgm.ownReply.dataWasSet = false;
|
mgm.ownReply.dataWasSet = false;
|
||||||
mgm.ownReply.temperatureWasSet = false;
|
mgm.ownReply.temperatureWasSet = false;
|
||||||
}
|
}
|
||||||
|
mgm.replyResult = returnvalue::FAILED;
|
||||||
mgm.mode = req->mode;
|
mgm.mode = req->mode;
|
||||||
}
|
}
|
||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
@ -180,6 +183,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
|
|||||||
} else {
|
} else {
|
||||||
mgm.ownReply.dataWasRead = false;
|
mgm.ownReply.dataWasRead = false;
|
||||||
}
|
}
|
||||||
|
mgm.replyResult = returnvalue::FAILED;
|
||||||
mgm.mode = req->mode;
|
mgm.mode = req->mode;
|
||||||
}
|
}
|
||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
@ -309,18 +313,18 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
|
|||||||
std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
|
std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
|
||||||
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
||||||
if (result != returnvalue::OK) {
|
if (result != returnvalue::OK) {
|
||||||
l3g.replyResult = returnvalue::OK;
|
l3g.replyResult = result;
|
||||||
}
|
}
|
||||||
// Ignore useless reply and red config
|
// Ignore useless reply and red config
|
||||||
cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
|
cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
|
||||||
std::memset(cmdBuf.data() + 1, 0, 5);
|
std::memset(cmdBuf.data() + 1, 0, 5);
|
||||||
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
||||||
if (result != returnvalue::OK) {
|
if (result != returnvalue::OK) {
|
||||||
l3g.replyResult = returnvalue::OK;
|
l3g.replyResult = result;
|
||||||
}
|
}
|
||||||
result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
|
result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
|
||||||
if (result != returnvalue::OK) {
|
if (result != returnvalue::OK) {
|
||||||
l3g.replyResult = returnvalue::OK;
|
l3g.replyResult = result;
|
||||||
}
|
}
|
||||||
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
||||||
// Cross check configuration as verification that communication is working
|
// Cross check configuration as verification that communication is working
|
||||||
@ -331,6 +335,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
l3g.replyResult = returnvalue::OK;
|
||||||
l3g.performStartup = false;
|
l3g.performStartup = false;
|
||||||
l3g.ownReply.cfgWasSet = true;
|
l3g.ownReply.cfgWasSet = true;
|
||||||
l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
|
l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
|
||||||
@ -357,6 +362,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
l3g.replyResult = returnvalue::OK;
|
||||||
l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
|
l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
|
||||||
l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
|
l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
|
||||||
l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
|
l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
|
||||||
@ -495,6 +501,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
|
|||||||
gyro.ownReply.cfg.prodId = prodId;
|
gyro.ownReply.cfg.prodId = prodId;
|
||||||
gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
|
gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
|
||||||
gyro.performStartup = false;
|
gyro.performStartup = false;
|
||||||
|
gyro.replyResult = returnvalue::OK;
|
||||||
}
|
}
|
||||||
// Read regular registers
|
// Read regular registers
|
||||||
std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
|
std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
|
||||||
@ -533,6 +540,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
||||||
|
gyro.replyResult = returnvalue::OK;
|
||||||
gyro.ownReply.dataWasSet = true;
|
gyro.ownReply.dataWasSet = true;
|
||||||
gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
|
gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
|
||||||
gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
|
gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
|
||||||
@ -590,6 +598,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
|
|||||||
}
|
}
|
||||||
// Done here. We can always read back config and data during periodic handling
|
// Done here. We can always read back config and data during periodic handling
|
||||||
mgm.performStartup = false;
|
mgm.performStartup = false;
|
||||||
|
mgm.replyResult = returnvalue::OK;
|
||||||
}
|
}
|
||||||
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
|
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
|
||||||
std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
|
std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
|
||||||
@ -607,7 +616,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
|
|||||||
// Verify communication by re-checking config
|
// Verify communication by re-checking config
|
||||||
if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
|
if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
|
||||||
rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
|
rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
|
||||||
mgm.replyResult = result;
|
mgm.replyResult = returnvalue::FAILED;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
@ -634,6 +643,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
||||||
|
mgm.replyResult = returnvalue::OK;
|
||||||
mgm.ownReply.temperatureWasSet = true;
|
mgm.ownReply.temperatureWasSet = true;
|
||||||
mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
|
mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
|
||||||
}
|
}
|
||||||
@ -704,6 +714,7 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
mgm.performStartup = false;
|
mgm.performStartup = false;
|
||||||
|
mgm.replyResult = returnvalue::OK;
|
||||||
}
|
}
|
||||||
// Regular read operation
|
// Regular read operation
|
||||||
cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
|
cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
|
||||||
@ -725,6 +736,7 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
|
|||||||
mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
|
mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
|
||||||
}
|
}
|
||||||
mgm.ownReply.dataWasRead = true;
|
mgm.ownReply.dataWasRead = true;
|
||||||
|
mgm.replyResult = returnvalue::OK;
|
||||||
// Bitshift trickery to account for 24 bit signed value.
|
// Bitshift trickery to account for 24 bit signed value.
|
||||||
mgm.ownReply.mgmValuesRaw[0] =
|
mgm.ownReply.mgmValuesRaw[0] =
|
||||||
((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;
|
((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;
|
||||||
|
@ -96,7 +96,7 @@ ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer
|
|||||||
if (susIdx < 0) {
|
if (susIdx < 0) {
|
||||||
return FAILED;
|
return FAILED;
|
||||||
}
|
}
|
||||||
if(susDevs[susIdx].replyResult != returnvalue::OK) {
|
if (susDevs[susIdx].replyResult != returnvalue::OK) {
|
||||||
return susDevs[susIdx].replyResult;
|
return susDevs[susIdx].replyResult;
|
||||||
}
|
}
|
||||||
MutexGuard mg(ipcLock);
|
MutexGuard mg(ipcLock);
|
||||||
@ -170,7 +170,7 @@ ReturnValue_t SusPolling::handleSusPolling() {
|
|||||||
susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1];
|
susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1];
|
||||||
// Reply is all ones. Sensor is probably off or faulty when
|
// Reply is all ones. Sensor is probably off or faulty when
|
||||||
// it should not be.
|
// it should not be.
|
||||||
if(susDevs[idx].ownReply.tempRaw == 0x0fff) {
|
if (susDevs[idx].ownReply.tempRaw == 0x0fff) {
|
||||||
susDevs[idx].replyResult = returnvalue::FAILED;
|
susDevs[idx].replyResult = returnvalue::FAILED;
|
||||||
} else {
|
} else {
|
||||||
susDevs[idx].replyResult = returnvalue::OK;
|
susDevs[idx].replyResult = returnvalue::OK;
|
||||||
|
Loading…
Reference in New Issue
Block a user