eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 118 Wiki Activity

adis handler continued

Browse Source
This commit is contained in:
Robin Müller 2021-05-26 13:30:12 +02:00 committed by Robin Mueller
parent cfcf2b9f46
commit 7bab7ffb75
3 changed files with 57 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
mission/devices/GyroADIS16507Handler.cpp
View File

@ -1,3 +1,4 @@
#include <fsfw/action/HasActionsIF.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw_hal/linux/utility.h>
@ -14,7 +15,7 @@
GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
configDataset(this), breakCountdown(0) {
configDataset(this), breakCountdown() {
#if ADIS16507_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5);
#endif
@ -29,17 +30,29 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
void GyroADIS16507Handler::doStartUp() {
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) {
commandExecuted = false;
internalState = InternalState::IDLE;
}
}
if(internalState == InternalState::IDLE) {
setMode(MODE_NORMAL);
}
}
void GyroADIS16507Handler::doShutDown() {
commandExecuted = false;
}
ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
@ -80,6 +93,10 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
break;
}
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(),
ADIS16507::BURST_READ_ENABLE.size());
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
@ -88,23 +105,48 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
break;
}
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);
breakCountdown.setTimeout(ADIS16507::SELF_TEST_BREAK);
break;
}
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);
breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_TEST_BREAK);
break;
}
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);
breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_UPDATE_BREAK);
break;
}
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);
breakCountdown.setTimeout(ADIS16507::FACTORY_CALIBRATION_BREAK);
break;
}
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);
breakCountdown.setTimeout(ADIS16507::SW_RESET_BREAK);
break;
}
case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
@ -127,6 +169,10 @@ void GyroADIS16507Handler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 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,
@ -265,6 +311,8 @@ void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOn
commandBuffer[1] = valueOne;
commandBuffer[2] = secondReg;
commandBuffer[3] = valueTwo;
this->rawPacketLen = 4;
this->rawPacket = commandBuffer.data();
}
void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {

1
mission/devices/GyroADIS16507Handler.h
View File

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

7
mission/devices/devicedefinitions/GyroADIS16507Definitions.h
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 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 FILTER_CTRL_REG = 0x5c;
static constexpr uint8_t MSC_CTRL_REG = 0x60;