eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

adis handler continued

Browse Source
This commit is contained in:
Robin Müller 2021-05-24 20:33:59 +02:00 committed by Robin Mueller
parent 9d312a02f7
commit fe62e38afc
7 changed files with 277 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
fsfw_hal

@ -1 +1 @@
Subproject commit d801319c12713d08cbdbc571ee2a922ce2f0c851
Subproject commit 73a427d7e0605519aecfa53fa289b94e0143394f

3
linux/fsfwconfig/OBSWConfig.h
View File

@ -38,6 +38,9 @@ debugging. */
#define DEBUG_RAD_SENSOR 1
#define DEBUG_SUS 1
// Leave at one as the BSP is linux. Used by the ADIS16507 device handler
#define OBSW_ADIS16507_LINUX_COM_IF 1
#include "OBSWVersion.h"
/* Can be used to switch device to NORMAL mode immediately */

131
mission/devices/GyroADIS16507Handler.cpp
View File

@ -1,8 +1,21 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "GyroADIS16507Handler.h"
#if OBSW_ADIS16507_LINUX_COM_IF == 1
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#endif
GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
configDataset(this) {
#if OBSW_ADIS16507_LINUX_COM_IF == 1
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
if(cookie != nullptr) {
cookie->setCallbackMode(&spiSendCallback, this);
}
#endif
}
void GyroADIS16507Handler::doStartUp() {
@ -18,15 +31,50 @@ void GyroADIS16507Handler::doShutDown() {
}
ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
return HasReturnvaluesIF::RETURN_OK;
*id = ADIS16507::READ_SENSOR_DATA;
return buildCommandFromCommand(*id, nullptr, 0);
}
ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch(internalState) {
case(InternalState::STARTUP): {
*id = ADIS16507::READ_OUT_CONFIG;
buildCommandFromCommand(*id, nullptr, 0);
break;
}
default: {
/* Might be a configuration error. */
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
"Unknown internal state!" << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData, size_t commandDataLen) {
switch(deviceCommand) {
case(ADIS16507::READ_OUT_CONFIG): {
this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
uint8_t regList[4];
regList[0] = ADIS16507::FILTER_CTRL_REG;
regList[1] = ADIS16507::MSC_CTRL_REG;
regList[2] = ADIS16507::DEC_RATE_REG;
regList[3] = ADIS16507::PROD_ID_REG;
prepareReadCommand(regList, sizeof(regList));
this->rawPacket = commandBuffer.data();
break;
}
case(ADIS16507::READ_SENSOR_DATA): {
std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
ADIS16507::BURST_READ_ENABLE.size());
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE;
this->rawPacket = commandBuffer.data();
break;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
@ -35,17 +83,90 @@ void GyroADIS16507Handler::fillCommandAndReplyMap() {
ReturnValue_t GyroADIS16507Handler::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();
*foundLen = this->rawPacketLen;
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
return HasReturnvaluesIF::RETURN_OK;
}
switch(id) {
case(ADIS16507::READ_OUT_CONFIG): {
PoolReadGuard rg(&configDataset);
uint16_t readProdId = packet[6] << 8 | packet[7];
if (readProdId != ADIS16507::PROD_ID) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
<< std::endl;
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
configDataset.filterSetting.value = packet[0] << 8 | packet[1];
configDataset.mscCtrlReg.value = packet[2] << 8 | packet[3];
configDataset.decRateReg.value = packet[4] << 8 | packet[5];
if(internalState == InternalState::STARTUP) {
commandExecuted = true;
}
break;
}
case(ADIS16507::READ_SENSOR_DATA): {
void GyroADIS16507Handler::setNormalDatapoolEntriesInvalid() {
}
}
return HasReturnvaluesIF::RETURN_OK;
}
uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
return 5000;
}
void GyroADIS16507Handler::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;
}
commandBuffer[len * 2] = 0x00;
commandBuffer[len * 2 + 1] = 0x00;
}
ReturnValue_t GyroADIS16507Handler::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::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>());
return HasReturnvaluesIF::RETURN_OK;
}
#if OBSW_ADIS16507_LINUX_COM_IF == 1
ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
const uint8_t *sendData, size_t sendLen, void *args) {
GyroADIS16507Handler* handler = reinterpret_cast<GyroADIS16507Handler*>(args);
if(handler == nullptr) {
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
<< std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
DeviceCommandId_t currentCommand = handler->getPendingCommand();
switch(currentCommand) {
case(ADIS16507::READ_SENSOR_DATA): {
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
}
case(ADIS16507::READ_OUT_CONFIG): {
}
}
return HasReturnvaluesIF::RETURN_OK;
}
#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */

26
mission/devices/GyroADIS16507Handler.h
View File

@ -1,12 +1,19 @@
#ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
#define MISSION_DEVICES_GYROADIS16507HANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include "OBSWConfig.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "devicedefinitions/GyroADIS16507Definitions.h"
#if OBSW_ADIS16507_LINUX_COM_IF == 1
class SpiComIF;
class SpiCookie;
#endif
class GyroADIS16507Handler: public DeviceHandlerBase {
public:
GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, CookieIF * comCookie);
GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF * comCookie);
/* DeviceHandlerBase abstract function implementation */
void doStartUp() override;
@ -20,10 +27,15 @@ public:
DeviceCommandId_t *foundId, size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) override;
void setNormalDatapoolEntriesInvalid() override;
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
private:
std::array<uint8_t, 32> commandBuffer;
AdisGyroPrimaryDataset primaryDataset;
AdisGyroConfigDataset configDataset;
enum class InternalState {
STARTUP,
@ -32,6 +44,14 @@ private:
InternalState internalState = InternalState::STARTUP;
bool commandExecuted = false;
void prepareReadCommand(uint8_t* regList, size_t len);
#if OBSW_ADIS16507_LINUX_COM_IF == 1
static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie,
const uint8_t *sendData, size_t sendLen, void* args);
#endif
};

4
mission/devices/GyroL3GD20Handler.cpp
View File

@ -65,8 +65,8 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
}
default:
/* Might be a configuration error. */
sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
<< "internal state!" << std::endl;
sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
return HasReturnvaluesIF::RETURN_OK;

111
mission/devices/devicedefinitions/GyroADIS16507Definitions.h
View File

@ -1,11 +1,122 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_
#include "fsfw/datapoollocal/StaticLocalDataSet.h"
#include <cstddef>
namespace ADIS16507 {
static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
static constexpr uint32_t GYRO_RANGE = 125;
static constexpr uint32_t ACCELEROMETER_RANGE = 392;
static constexpr uint16_t PROD_ID = 16507;
static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
static constexpr uint8_t DIAG_STAT_REG = 0x02;
static constexpr uint8_t FILTER_CTRL_REG = 0x5c;
static constexpr uint8_t MSC_CTRL_REG = 0x60;
static constexpr uint8_t DEC_RATE_REG = 0x64;
static constexpr uint8_t GLOB_CMD = 0x68;
static constexpr uint8_t PROD_ID_REG = 0x72;
static constexpr DeviceCommandId_t READ_SENSOR_DATA = 0;
static constexpr DeviceCommandId_t READ_OUT_CONFIG = 1;
static constexpr DeviceCommandId_t SELF_TEST_SENSORS = 2;
static constexpr DeviceCommandId_t SELF_TEST_MEMORY = 3;
static constexpr DeviceCommandId_t UPDATE_NV_CONFIGURATION = 4;
static constexpr DeviceCommandId_t SELECT_BURST_READ_MODE = 5;
static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
static constexpr DeviceCommandId_t SW_RESET = 31;
static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
static constexpr size_t CONFIG_READOUT_SIZE = 8;
static constexpr size_t SENSOR_READOUT_SIZE = 22;
static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;
enum PrimaryPoolIds: lp_id_t {
ANG_VELOC_X,
ANG_VELOC_Y,
ANG_VELOC_Z,
ACCELERATION_X,
ACCELERATION_Y,
ACCELERATION_Z,
TEMPERATURE
};
enum ConfigPoolIds: lp_id_t {
FILTER_SETTINGS,
MSC_CTRL_REGISTER,
DEC_RATE_REGISTER,
};
enum FilterSettings: uint8_t {
NO_FILTER = 0,
TWO_TAPS = 1,
FOUR_TAPS = 2,
EIGHT_TAPS = 3,
SIXTEEN_TAPS = 4,
THIRTYTWO_TAPS = 5,
SIXTYFOUR_TAPS = 6
};
}
class AdisGyroPrimaryDataset: public StaticLocalDataSet<7 * sizeof(float)> {
public:
/** Constructor for data users like controllers */
AdisGyroPrimaryDataset(object_id_t adisId):
StaticLocalDataSet(sid_t(adisId, ADIS16507::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);
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId,
ADIS16507::ANG_VELOC_Y, this);
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId,
ADIS16507::ANG_VELOC_Z, this);
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId,
ADIS16507::ACCELERATION_X, this);
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId,
ADIS16507::ACCELERATION_Y, this);
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId,
ADIS16507::ACCELERATION_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
ADIS16507::TEMPERATURE, this);
private:
friend class GyroADIS16507Handler;
/** Constructor for the data creator */
AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {}
};
class AdisGyroConfigDataset: public StaticLocalDataSet<32> {
public:
/** Constructor for data users like controllers */
AdisGyroConfigDataset(object_id_t adisId):
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
setAllVariablesReadOnly();
}
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);
private:
friend class GyroADIS16507Handler;
/** Constructor for the data creator */
AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner):
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_CFG_DATASET_ID) {}
};
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */

2
mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
View File

@ -142,7 +142,7 @@ enum MgmPoolIds: lp_id_t {
TEMPERATURE_CELCIUS
};
class MgmPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
class MgmPrimaryDataset: public StaticLocalDataSet<4 * sizeof(float)> {
public:
MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner):
StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {}