adis handler continued

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

View File

@ -1,3 +1,4 @@
#include <fsfw/action/HasActionsIF.h>
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw_hal/linux/utility.h> #include <fsfw_hal/linux/utility.h>
@ -14,7 +15,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), breakCountdown(0) { configDataset(this), breakCountdown() {
#if ADIS16507_DEBUG == 1 #if ADIS16507_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5); debugDivider = new PeriodicOperationDivider(5);
#endif #endif
@ -29,17 +30,29 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
void GyroADIS16507Handler::doStartUp() { void GyroADIS16507Handler::doStartUp() {
if(internalState == InternalState::STARTUP) { if(internalState == InternalState::STARTUP) {
if(not commandExecuted) {
breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
commandExecuted = true;
}
if(breakCountdown.hasTimedOut()) {
internalState = InternalState::CONFIG;
}
}
if(internalState == InternalState::CONFIG) {
if(commandExecuted) { if(commandExecuted) {
commandExecuted = false; commandExecuted = false;
internalState = InternalState::IDLE; internalState = InternalState::IDLE;
} }
} }
if(internalState == InternalState::IDLE) { if(internalState == InternalState::IDLE) {
setMode(MODE_NORMAL); setMode(MODE_NORMAL);
} }
} }
void GyroADIS16507Handler::doShutDown() { void GyroADIS16507Handler::doShutDown() {
commandExecuted = false;
} }
ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
@ -80,6 +93,10 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
break; break;
} }
case(ADIS16507::READ_SENSOR_DATA): { case(ADIS16507::READ_SENSOR_DATA): {
if(breakCountdown.isBusy()) {
// A glob command is pending and sensor data can't be read currently
return NO_REPLY_EXPECTED;
}
std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(), std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
ADIS16507::BURST_READ_ENABLE.size()); ADIS16507::BURST_READ_ENABLE.size());
std::memset(commandBuffer.data() + 2, 0, 10 * 2); std::memset(commandBuffer.data() + 2, 0, 10 * 2);
@ -88,23 +105,48 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
break; break;
} }
case(ADIS16507::SELF_TEST_SENSORS): { case(ADIS16507::SELF_TEST_SENSORS): {
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00); prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00);
breakCountdown.setTimeout(ADIS16507::SELF_TEST_BREAK);
break; break;
} }
case(ADIS16507::SELF_TEST_MEMORY): { case(ADIS16507::SELF_TEST_MEMORY): {
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00); prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00);
breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_TEST_BREAK);
break; break;
} }
case(ADIS16507::UPDATE_NV_CONFIGURATION): { case(ADIS16507::UPDATE_NV_CONFIGURATION): {
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00); prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_UPDATE_BREAK);
break; break;
} }
case(ADIS16507::RESET_SENSOR_CONFIGURATION): { case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00); prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00);
breakCountdown.setTimeout(ADIS16507::FACTORY_CALIBRATION_BREAK);
break; break;
} }
case(ADIS16507::SW_RESET): { case(ADIS16507::SW_RESET): {
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00); prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00);
breakCountdown.setTimeout(ADIS16507::SW_RESET_BREAK);
break; break;
} }
case(ADIS16507::PRINT_CURRENT_CONFIGURATION): { case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
@ -127,6 +169,10 @@ void GyroADIS16507Handler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset); insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1); insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1);
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1); insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1);
insertInCommandAndReplyMap(ADIS16507::UPDATE_NV_CONFIGURATION, 1);
insertInCommandAndReplyMap(ADIS16507::RESET_SENSOR_CONFIGURATION, 1);
insertInCommandAndReplyMap(ADIS16507::SW_RESET, 1);
insertInCommandAndReplyMap(ADIS16507::PRINT_CURRENT_CONFIGURATION, 1);
} }
ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize, ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
@ -265,6 +311,8 @@ void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOn
commandBuffer[1] = valueOne; commandBuffer[1] = valueOne;
commandBuffer[2] = secondReg; commandBuffer[2] = secondReg;
commandBuffer[3] = valueTwo; commandBuffer[3] = valueTwo;
this->rawPacketLen = 4;
this->rawPacket = commandBuffer.data();
} }
void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) { void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {

View File

@ -40,6 +40,7 @@ private:
enum class InternalState { enum class InternalState {
STARTUP, STARTUP,
CONFIG,
IDLE IDLE
}; };

View File

@ -19,6 +19,13 @@ static constexpr uint16_t PROD_ID = 16507;
static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00}; static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
static constexpr dur_millis_t START_UP_TIME = 310;
static constexpr dur_millis_t SW_RESET_BREAK = 255;
static constexpr dur_millis_t FACTORY_CALIBRATION_BREAK = 136;
static constexpr dur_millis_t FLASH_MEMORY_UPDATE_BREAK = 70;
static constexpr dur_millis_t FLASH_MEMORY_TEST_BREAK = 30;
static constexpr dur_millis_t SELF_TEST_BREAK = 24;
static constexpr uint8_t DIAG_STAT_REG = 0x02; static constexpr uint8_t DIAG_STAT_REG = 0x02;
static constexpr uint8_t FILTER_CTRL_REG = 0x5c; static constexpr uint8_t FILTER_CTRL_REG = 0x5c;
static constexpr uint8_t MSC_CTRL_REG = 0x60; static constexpr uint8_t MSC_CTRL_REG = 0x60;