adis handler continued
This commit is contained in:
parent
a73d31b0fb
commit
4a7087fa5a
@ -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) {
|
||||||
|
@ -40,6 +40,7 @@ private:
|
|||||||
|
|
||||||
enum class InternalState {
|
enum class InternalState {
|
||||||
STARTUP,
|
STARTUP,
|
||||||
|
CONFIG,
|
||||||
IDLE
|
IDLE
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -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;
|
||||||
|
Loading…
Reference in New Issue
Block a user