ADIS device handler can now handle multiple types
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
- Added handling for ADIS16507 and ADIS16505 type
This commit is contained in:
parent
54e5fef8fd
commit
af32941352
@ -36,7 +36,7 @@
|
||||
#include "mission/devices/P60DockHandler.h"
|
||||
#include "mission/devices/Tmp1075Handler.h"
|
||||
#include "mission/devices/Max31865PT1000Handler.h"
|
||||
#include "mission/devices/GyroADIS16507Handler.h"
|
||||
#include "mission/devices/GyroADIS1650XHandler.h"
|
||||
#include "mission/devices/IMTQHandler.h"
|
||||
#include "mission/devices/SyrlinksHkHandler.h"
|
||||
#include "mission/devices/PlocMPSoCHandler.h"
|
||||
@ -511,10 +511,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
// Commented until ACS board V2 in in clean room again
|
||||
// Gyro 0 Side A
|
||||
spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev,
|
||||
ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
ADIS1650X::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
spi::DEFAULT_ADIS16507_SPEED);
|
||||
auto adisHandler = new GyroADIS16507Handler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie);
|
||||
auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie, ADIS1650X::Type::ADIS16505);
|
||||
adisHandler->setStartUpImmediately();
|
||||
// Gyro 1 Side A
|
||||
spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev,
|
||||
@ -527,10 +527,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
#endif
|
||||
// Gyro 2 Side B
|
||||
spiCookie = new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, spiDev,
|
||||
ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
ADIS1650X::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
spi::DEFAULT_ADIS16507_SPEED);
|
||||
adisHandler = new GyroADIS16507Handler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie);
|
||||
adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie, ADIS1650X::Type::ADIS16505);
|
||||
adisHandler->setStartUpImmediately();
|
||||
// Gyro 3 Side B
|
||||
spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, spiDev,
|
||||
|
||||
@ -78,6 +78,6 @@ static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
|
||||
#define FSFW_HAL_L3GD20_GYRO_DEBUG 0
|
||||
#define FSFW_HAL_RM3100_MGM_DEBUG 0
|
||||
#define FSFW_HAL_LIS3MDL_MGM_DEBUG 0
|
||||
#define FSFW_HAL_ADIS16507_GYRO_DEBUG 0
|
||||
#define FSFW_HAL_ADIS1650X_GYRO_DEBUG 0
|
||||
|
||||
#endif /* CONFIG_FSFWCONFIG_H_ */
|
||||
|
||||
@ -439,7 +439,7 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
|
||||
#endif
|
||||
|
||||
#if OBSW_ADD_ACS_BOARD == 1
|
||||
bool enableAside = true;
|
||||
bool enableAside = false;
|
||||
bool enableBside = true;
|
||||
if(enableAside) {
|
||||
// A side
|
||||
|
||||
@ -12,7 +12,7 @@ target_sources(${TARGET_NAME} PUBLIC
|
||||
IMTQHandler.cpp
|
||||
PlocMPSoCHandler.cpp
|
||||
RadiationSensorHandler.cpp
|
||||
GyroADIS16507Handler.cpp
|
||||
GyroADIS1650XHandler.cpp
|
||||
RwHandler.cpp
|
||||
)
|
||||
|
||||
|
||||
@ -1,8 +1,8 @@
|
||||
#include "GyroADIS16507Handler.h"
|
||||
#include "GyroADIS1650XHandler.h"
|
||||
#include <fsfw/action/HasActionsIF.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
#include "fsfw_hal/linux/utility.h"
|
||||
#include "fsfw_hal/linux/spi/SpiCookie.h"
|
||||
#include "fsfw_hal/linux/spi/SpiComIF.h"
|
||||
@ -11,15 +11,15 @@
|
||||
#include <unistd.h>
|
||||
#endif
|
||||
|
||||
GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
|
||||
object_id_t deviceCommunication, CookieIF * comCookie):
|
||||
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
|
||||
configDataset(this), breakCountdown() {
|
||||
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
|
||||
GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId,
|
||||
object_id_t deviceCommunication, CookieIF * comCookie, ADIS1650X::Type type):
|
||||
DeviceHandlerBase(objectId, deviceCommunication, comCookie), adisType(type),
|
||||
primaryDataset(this), configDataset(this), breakCountdown() {
|
||||
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
|
||||
debugDivider = new PeriodicOperationDivider(5);
|
||||
#endif
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
|
||||
if(cookie != nullptr) {
|
||||
cookie->setCallbackMode(&spiSendCallback, this);
|
||||
@ -27,11 +27,11 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
|
||||
#endif
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::doStartUp() {
|
||||
void GyroADIS1650XHandler::doStartUp() {
|
||||
// Initial 310 ms start up time after power-up
|
||||
if(internalState == InternalState::STARTUP) {
|
||||
if(not commandExecuted) {
|
||||
breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
|
||||
breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
|
||||
commandExecuted = true;
|
||||
}
|
||||
if(breakCountdown.hasTimedOut()) {
|
||||
@ -54,20 +54,20 @@ void GyroADIS16507Handler::doStartUp() {
|
||||
}
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::doShutDown() {
|
||||
void GyroADIS1650XHandler::doShutDown() {
|
||||
commandExecuted = false;
|
||||
setMode(_MODE_POWER_DOWN);
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||
*id = ADIS16507::READ_SENSOR_DATA;
|
||||
ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||
*id = ADIS1650X::READ_SENSOR_DATA;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||
ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||
switch(internalState) {
|
||||
case(InternalState::CONFIG): {
|
||||
*id = ADIS16507::READ_OUT_CONFIG;
|
||||
*id = ADIS1650X::READ_OUT_CONFIG;
|
||||
buildCommandFromCommand(*id, nullptr, 0);
|
||||
break;
|
||||
}
|
||||
@ -85,79 +85,79 @@ ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
const uint8_t *commandData, size_t commandDataLen) {
|
||||
switch(deviceCommand) {
|
||||
case(ADIS16507::READ_OUT_CONFIG): {
|
||||
this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
|
||||
case(ADIS1650X::READ_OUT_CONFIG): {
|
||||
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
|
||||
uint8_t regList[5];
|
||||
regList[0] = ADIS16507::DIAG_STAT_REG;
|
||||
regList[1] = ADIS16507::FILTER_CTRL_REG;
|
||||
regList[2] = ADIS16507::MSC_CTRL_REG;
|
||||
regList[3] = ADIS16507::DEC_RATE_REG;
|
||||
regList[4] = ADIS16507::PROD_ID_REG;
|
||||
regList[0] = ADIS1650X::DIAG_STAT_REG;
|
||||
regList[1] = ADIS1650X::FILTER_CTRL_REG;
|
||||
regList[2] = ADIS1650X::MSC_CTRL_REG;
|
||||
regList[3] = ADIS1650X::DEC_RATE_REG;
|
||||
regList[4] = ADIS1650X::PROD_ID_REG;
|
||||
prepareReadCommand(regList, sizeof(regList));
|
||||
this->rawPacket = commandBuffer.data();
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::READ_SENSOR_DATA): {
|
||||
case(ADIS1650X::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::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
|
||||
ADIS1650X::BURST_READ_ENABLE.size());
|
||||
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
|
||||
this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE;
|
||||
this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
|
||||
this->rawPacket = commandBuffer.data();
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::SELF_TEST_SENSORS): {
|
||||
case(ADIS1650X::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);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::SELF_TEST_MEMORY): {
|
||||
case(ADIS1650X::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);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::UPDATE_NV_CONFIGURATION): {
|
||||
case(ADIS1650X::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);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
|
||||
case(ADIS1650X::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);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::SW_RESET): {
|
||||
case(ADIS1650X::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);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
|
||||
case(ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
PoolReadGuard pg(&configDataset);
|
||||
sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) <<
|
||||
@ -172,18 +172,18 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::fillCommandAndReplyMap() {
|
||||
insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
|
||||
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);
|
||||
void GyroADIS1650XHandler::fillCommandAndReplyMap() {
|
||||
insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
|
||||
insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
|
||||
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
/* For SPI, the ID will always be the one of the last sent command. */
|
||||
*foundId = this->getPendingCommand();
|
||||
@ -192,16 +192,19 @@ ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t re
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
const uint8_t *packet) {
|
||||
switch(id) {
|
||||
case(ADIS16507::READ_OUT_CONFIG): {
|
||||
case(ADIS1650X::READ_OUT_CONFIG): {
|
||||
PoolReadGuard rg(&configDataset);
|
||||
uint16_t readProdId = packet[10] << 8 | packet[11];
|
||||
if (readProdId != ADIS16507::PROD_ID) {
|
||||
if(((adisType == ADIS1650X::Type::ADIS16507) and
|
||||
(readProdId != ADIS1650X::PROD_ID_16507)) or
|
||||
((adisType == ADIS1650X::Type::ADIS16505) and
|
||||
(readProdId != ADIS1650X::PROD_ID_16505))) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
|
||||
<< std::endl;
|
||||
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID "
|
||||
<< readProdId << std::endl;
|
||||
#endif
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
@ -215,7 +218,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
}
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::READ_SENSOR_DATA): {
|
||||
case(ADIS1650X::READ_SENSOR_DATA): {
|
||||
return handleSensorData(packet);
|
||||
}
|
||||
|
||||
@ -224,7 +227,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
|
||||
BurstModes burstMode = getBurstMode();
|
||||
switch(burstMode) {
|
||||
case(BurstModes::BURST_16_BURST_SEL_1):
|
||||
@ -260,23 +263,32 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
PoolReadGuard pg(&primaryDataset);
|
||||
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
|
||||
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
|
||||
ADIS16507::GYRO_RANGE;
|
||||
ADIS1650X::GYRO_RANGE;
|
||||
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
|
||||
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
|
||||
ADIS16507::GYRO_RANGE;
|
||||
ADIS1650X::GYRO_RANGE;
|
||||
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
|
||||
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
|
||||
ADIS16507::GYRO_RANGE;
|
||||
ADIS1650X::GYRO_RANGE;
|
||||
|
||||
float accelScaling = 0;
|
||||
if(adisType == ADIS1650X::Type::ADIS16507) {
|
||||
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
|
||||
} else if(adisType == ADIS1650X::Type::ADIS16505) {
|
||||
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
|
||||
} else {
|
||||
sif::warning << "GyroADIS16507Handler::handleSensorData: "
|
||||
"Unknown ADIS type" << std::endl;
|
||||
}
|
||||
int16_t accelXRaw = packet[10] << 8 | packet[11];
|
||||
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
|
||||
ADIS16507::ACCELEROMETER_RANGE;
|
||||
accelScaling;
|
||||
int16_t accelYRaw = packet[12] << 8 | packet[13];
|
||||
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
|
||||
ADIS16507::ACCELEROMETER_RANGE;
|
||||
accelScaling;
|
||||
int16_t accelZRaw = packet[14] << 8 | packet[15];
|
||||
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
|
||||
ADIS16507::ACCELEROMETER_RANGE;
|
||||
accelScaling;
|
||||
|
||||
int16_t temperatureRaw = packet[16] << 8 | packet[17];
|
||||
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
|
||||
@ -284,7 +296,7 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
primaryDataset.setValidity(true, true);
|
||||
}
|
||||
|
||||
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
|
||||
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
|
||||
if(debugDivider->checkAndIncrement()) {
|
||||
sif::info << "GyroADIS16507Handler: Angular velocities in deg / s" << std::endl;
|
||||
sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
|
||||
@ -306,15 +318,15 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||
return 5000;
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
||||
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
||||
uint8_t valueTwo) {
|
||||
uint8_t secondReg = startReg + 1;
|
||||
startReg |= ADIS16507::WRITE_MASK;
|
||||
secondReg |= ADIS16507::WRITE_MASK;
|
||||
startReg |= ADIS1650X::WRITE_MASK;
|
||||
secondReg |= ADIS1650X::WRITE_MASK;
|
||||
commandBuffer[0] = startReg;
|
||||
commandBuffer[1] = valueOne;
|
||||
commandBuffer[2] = secondReg;
|
||||
@ -323,7 +335,7 @@ void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOn
|
||||
this->rawPacket = commandBuffer.data();
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
|
||||
void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
|
||||
for(size_t idx = 0; idx < len; idx++) {
|
||||
commandBuffer[idx * 2] = regList[idx];
|
||||
commandBuffer[idx * 2 + 1] = 0x00;
|
||||
@ -332,29 +344,29 @@ void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
|
||||
commandBuffer[len * 2 + 1] = 0x00;
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
LocalDataPoolManager &poolManager) {
|
||||
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||
|
||||
localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
||||
localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
|
||||
GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
|
||||
configDataset.mscCtrlReg.read();
|
||||
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
|
||||
configDataset.mscCtrlReg.commit();
|
||||
if((currentCtrlReg & ADIS16507::BURST_32_BIT) == ADIS16507::BURST_32_BIT) {
|
||||
if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
|
||||
if((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
|
||||
if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
|
||||
return BurstModes::BURST_32_BURST_SEL_1;
|
||||
}
|
||||
else {
|
||||
@ -362,7 +374,7 @@ GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
|
||||
}
|
||||
}
|
||||
else {
|
||||
if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
|
||||
if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
|
||||
return BurstModes::BURST_16_BURST_SEL_1;
|
||||
}
|
||||
else {
|
||||
@ -371,11 +383,11 @@ GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
|
||||
}
|
||||
}
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
||||
ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
||||
const uint8_t *sendData, size_t sendLen, void *args) {
|
||||
GyroADIS16507Handler* handler = reinterpret_cast<GyroADIS16507Handler*>(args);
|
||||
GyroADIS1650XHandler* handler = reinterpret_cast<GyroADIS1650XHandler*>(args);
|
||||
if(handler == nullptr) {
|
||||
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
||||
<< std::endl;
|
||||
@ -383,10 +395,10 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
|
||||
}
|
||||
DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
||||
switch(currentCommand) {
|
||||
case(ADIS16507::READ_SENSOR_DATA): {
|
||||
case(ADIS1650X::READ_SENSOR_DATA): {
|
||||
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
||||
}
|
||||
case(ADIS16507::READ_OUT_CONFIG):
|
||||
case(ADIS1650X::READ_OUT_CONFIG):
|
||||
default: {
|
||||
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
||||
int retval = 0;
|
||||
@ -451,7 +463,7 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
|
||||
|
||||
idx += 2;
|
||||
if(idx < sendLen) {
|
||||
usleep(ADIS16507::STALL_TIME_MICROSECONDS);
|
||||
usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
|
||||
}
|
||||
spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
|
||||
transferStruct->tx_buf += 2;
|
||||
@ -466,4 +478,4 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */
|
||||
#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */
|
||||
@ -3,7 +3,7 @@
|
||||
|
||||
#include "OBSWConfig.h"
|
||||
#include "FSFWConfig.h"
|
||||
#include "devicedefinitions/GyroADIS16507Definitions.h"
|
||||
#include "devicedefinitions/GyroADIS1650XDefinitions.h"
|
||||
|
||||
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
|
||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
@ -19,10 +19,10 @@ class SpiCookie;
|
||||
* Flight manual:
|
||||
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
|
||||
*/
|
||||
class GyroADIS16507Handler: public DeviceHandlerBase {
|
||||
class GyroADIS1650XHandler: public DeviceHandlerBase {
|
||||
public:
|
||||
GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication,
|
||||
CookieIF * comCookie);
|
||||
GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||||
CookieIF* comCookie, ADIS1650X::Type type);
|
||||
|
||||
/* DeviceHandlerBase abstract function implementation */
|
||||
void doStartUp() override;
|
||||
@ -42,7 +42,7 @@ public:
|
||||
|
||||
private:
|
||||
std::array<uint8_t, 32> commandBuffer;
|
||||
|
||||
ADIS1650X::Type adisType;
|
||||
AdisGyroPrimaryDataset primaryDataset;
|
||||
AdisGyroConfigDataset configDataset;
|
||||
|
||||
@ -71,7 +71,7 @@ private:
|
||||
const uint8_t *sendData, size_t sendLen, void* args);
|
||||
#endif
|
||||
|
||||
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
|
||||
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
|
||||
PeriodicOperationDivider* debugDivider;
|
||||
#endif
|
||||
Countdown breakCountdown;
|
||||
@ -6,17 +6,24 @@
|
||||
|
||||
#include <cstddef>
|
||||
|
||||
namespace ADIS16507 {
|
||||
namespace ADIS1650X {
|
||||
|
||||
enum class Type {
|
||||
ADIS16505,
|
||||
ADIS16507
|
||||
};
|
||||
|
||||
static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
|
||||
static constexpr uint8_t WRITE_MASK = 0b1000'0000;
|
||||
|
||||
static constexpr uint32_t GYRO_RANGE = 125;
|
||||
static constexpr uint32_t ACCELEROMETER_RANGE = 392;
|
||||
static constexpr uint32_t ACCELEROMETER_RANGE_16507 = 392;
|
||||
static constexpr float ACCELEROMETER_RANGE_16505 = 78.4;
|
||||
|
||||
static constexpr uint32_t STALL_TIME_MICROSECONDS = 16;
|
||||
|
||||
static constexpr uint16_t PROD_ID = 16507;
|
||||
static constexpr uint16_t PROD_ID_16507 = 16507;
|
||||
static constexpr uint16_t PROD_ID_16505 = 16505;
|
||||
|
||||
static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
|
||||
|
||||
@ -95,31 +102,31 @@ public:
|
||||
|
||||
/** Constructor for data users like controllers */
|
||||
AdisGyroPrimaryDataset(object_id_t adisId):
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) {
|
||||
setAllVariablesReadOnly();
|
||||
}
|
||||
|
||||
/* Angular velocities in degrees per second (DPS) */
|
||||
lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ANG_VELOC_X, this);
|
||||
ADIS1650X::ANG_VELOC_X, this);
|
||||
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ANG_VELOC_Y, this);
|
||||
ADIS1650X::ANG_VELOC_Y, this);
|
||||
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ANG_VELOC_Z, this);
|
||||
ADIS1650X::ANG_VELOC_Z, this);
|
||||
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ACCELERATION_X, this);
|
||||
ADIS1650X::ACCELERATION_X, this);
|
||||
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ACCELERATION_Y, this);
|
||||
ADIS1650X::ACCELERATION_Y, this);
|
||||
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ACCELERATION_Z, this);
|
||||
ADIS1650X::ACCELERATION_Z, this);
|
||||
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
|
||||
ADIS16507::TEMPERATURE, this);
|
||||
ADIS1650X::TEMPERATURE, this);
|
||||
private:
|
||||
|
||||
friend class GyroADIS16507Handler;
|
||||
friend class GyroADIS1650XHandler;
|
||||
/** Constructor for the data creator */
|
||||
AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
|
||||
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {}
|
||||
StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {}
|
||||
};
|
||||
|
||||
class AdisGyroConfigDataset: public StaticLocalDataSet<5> {
|
||||
@ -127,20 +134,20 @@ public:
|
||||
|
||||
/** Constructor for data users like controllers */
|
||||
AdisGyroConfigDataset(object_id_t adisId):
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_CFG_DATASET_ID)) {
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) {
|
||||
setAllVariablesReadOnly();
|
||||
}
|
||||
|
||||
lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId,
|
||||
ADIS16507::DIAG_STAT_REGISTER);
|
||||
lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS16507::FILTER_SETTINGS);
|
||||
lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::MSC_CTRL_REGISTER);
|
||||
lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DEC_RATE_REGISTER);
|
||||
ADIS1650X::DIAG_STAT_REGISTER);
|
||||
lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS);
|
||||
lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER);
|
||||
lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER);
|
||||
private:
|
||||
friend class GyroADIS16507Handler;
|
||||
friend class GyroADIS1650XHandler;
|
||||
/** Constructor for the data creator */
|
||||
AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner):
|
||||
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_CFG_DATASET_ID) {}
|
||||
StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {}
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */
|
||||
Loading…
Reference in New Issue
Block a user