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

some bugfixes for adis datasets

Browse Source
This commit is contained in:
Robin Müller 2021-05-25 13:53:03 +02:00 committed by Robin Mueller
parent 1262ccd65d
commit b9bf245d68
9 changed files with 89 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
bsp_linux_board/ObjectFactory.cpp
View File

@ -123,6 +123,11 @@ void ObjectFactory::produce(){
#endif /* RPI_TEST_ACS_BOARD == 1 */
#if RPI_TEST_ADIS16507 == 1
GpioCookie* gpioCookieAcsBoard = new GpioCookie();
gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN,
"GYRO_0_ADIS", gpio::Direction::OUT, 1);
gpioIF->addGpios(gpioCookieAcsBoard);
std::string spiDev = "/dev/spidev0.0";
SpiCookie* spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev,
ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED,

2
fsfw_hal

@ -1 +1 @@
Subproject commit 2893da047bc695fcb02b0f9567cef0366a37793a
Subproject commit c2b4231802bcc25e1a2588ab4d12c8ecf64d2c47

2
linux/fsfwconfig/FSFWConfig.h
View File

@ -68,4 +68,6 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
}
#define FSFW_HAL_LINUX_SPI_WIRETAPPING 0
#endif /* CONFIG_FSFWCONFIG_H_ */

4
linux/fsfwconfig/OBSWConfig.h
View File

@ -21,8 +21,6 @@ debugging. */
#define OBSW_ADD_TEST_CODE 1
#define OBSW_ADD_TEST_PST 1
#define FSFW_HAL_LINUX_SPI_WIRETAPPING 0
#define TEST_LIBGPIOD 0
#define TEST_RADIATION_SENSOR_HANDLER 1
#define TEST_SUS_HANDLER 1
@ -37,6 +35,8 @@ debugging. */
#define ACU_DEBUG 0
#define SYRLINKS_DEBUG 0
#define IMQT_DEBUG 0
#define ADIS16507_DEBUG 1
#define L3GD20_GYRO_DEBUG 0
#define DEBUG_RAD_SENSOR 1
#define DEBUG_SUS 1

109
mission/devices/GyroADIS16507Handler.cpp
View File

@ -15,6 +15,10 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
configDataset(this) {
#if ADIS16507_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5);
#endif
#if OBSW_ADIS16507_LINUX_COM_IF == 1
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
if(cookie != nullptr) {
@ -30,6 +34,9 @@ void GyroADIS16507Handler::doStartUp() {
internalState = InternalState::IDLE;
}
}
if(internalState == InternalState::IDLE) {
setMode(MODE_NORMAL);
}
}
void GyroADIS16507Handler::doShutDown() {
@ -103,7 +110,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
switch(id) {
case(ADIS16507::READ_OUT_CONFIG): {
PoolReadGuard rg(&configDataset);
uint16_t readProdId = packet[8] << 8 | packet[9];
uint16_t readProdId = packet[10] << 8 | packet[11];
if (readProdId != ADIS16507::PROD_ID) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
@ -111,10 +118,10 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
configDataset.diagStatReg.value = packet[0] << 8 | packet[1];
configDataset.filterSetting.value = packet[2] << 8 | packet[3];
configDataset.mscCtrlReg.value = packet[4] << 8 | packet[5];
configDataset.decRateReg.value = packet[6] << 8 | packet[7];
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
configDataset.filterSetting.value = packet[4] << 8 | packet[5];
configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
configDataset.decRateReg.value = packet[8] << 8 | packet[9];
configDataset.setValidity(true, true);
if(internalState == InternalState::STARTUP) {
commandExecuted = true;
@ -162,33 +169,44 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
}
configDataset.diagStatReg.commit();
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
{
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelXRaw = packet[10] << 8 | packet[11];
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelYRaw = packet[12] << 8 | packet[13];
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelZRaw = packet[14] << 8 | packet[15];
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelXRaw = packet[10] << 8 | packet[11];
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelYRaw = packet[12] << 8 | packet[13];
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelZRaw = packet[14] << 8 | packet[15];
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t temperatureRaw = packet[16] << 8 | packet[17];
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
int16_t temperatureRaw = packet[16] << 8 | packet[17];
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
// Ignore data counter for now
primaryDataset.setValidity(true, true);
}
// Ignore data counter for now
#if ADIS16507_DEBUG == 1
if(debugDivider->checkAndIncrement()) {
sif::info << "GyroADIS16507Handler: Angular velocities in degrees per second:" <<
std::endl;
sif::info << "X: " << primaryDataset.angVelocX.value << " \xC2\xB0" << std::endl;
sif::info << "Y: " << primaryDataset.angVelocY.value << " \xC2\xB0" << std::endl;
sif::info << "Z: " << primaryDataset.angVelocZ.value << " \xC2\xB0" << std::endl;
}
#endif
primaryDataset.setValidity(true, true);
break;
}
case(BurstModes::BURST_32_BURST_SEL_0): {
@ -219,7 +237,9 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool
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(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>());
@ -294,17 +314,20 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
#endif
}
if(gpioId != gpio::NO_GPIO) {
result = mutex->lockMutex(timeoutType, timeoutMs);
if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
return result;
}
}
size_t idx = 0;
while(idx < sendLen) {
// Pull SPI CS low. For now, no support for active high given
if(gpioId != gpio::NO_GPIO) {
result = mutex->lockMutex(timeoutType, timeoutMs);
if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
return result;
}
gpioIF->pullLow(gpioId);
}
@ -321,21 +344,19 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
if(gpioId != gpio::NO_GPIO) {
gpioIF->pullHigh(gpioId);
result = mutex->unlockMutex();
if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "SpiComIF::getSendSuccess: Failed to unlock mutex" << std::endl;
#endif
return result;
}
}
idx += 2;
if(idx < sendLen) {
usleep(ADIS16507::STALL_TIME_MICROSECONDS);
}
sendData += 2;
cookie->getTransferStructHandle()->rx_buf += 2;
spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
transferStruct->tx_buf += 2;
transferStruct->rx_buf += 2;
}
if(gpioId != gpio::NO_GPIO) {
mutex->unlockMutex();
}
}
}

5
mission/devices/GyroADIS16507Handler.h
View File

@ -1,6 +1,7 @@
#ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
#define MISSION_DEVICES_GYROADIS16507HANDLER_H_
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
#include "OBSWConfig.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "devicedefinitions/GyroADIS16507Definitions.h"
@ -61,6 +62,10 @@ private:
const uint8_t *sendData, size_t sendLen, void* args);
#endif
#if ADIS16507_DEBUG == 1
PeriodicOperationDivider* debugDivider;
#endif
ReturnValue_t handleSensorData(const uint8_t* packet);
};

4
mission/devices/GyroL3GD20Handler.cpp
View File

@ -7,7 +7,7 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC
CookieIF *comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie),
dataset(this) {
#if OBSW_VERBOSE_LEVEL >= 1
#if L3GD20_GYRO_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5);
#endif
}
@ -197,7 +197,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX];
float temperature = 25.0 + temperaturOffset;
#if OBSW_VERBOSE_LEVEL >= 1
#if L3GD20_GYRO_DEBUG == 1
if(debugDivider->checkAndIncrement()) {
/* Set terminal to utf-8 if there is an issue with micro printout. */
#if FSFW_CPP_OSTREAM_ENABLED == 1

2
mission/devices/GyroL3GD20Handler.h
View File

@ -70,7 +70,7 @@ private:
float scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_00) / INT16_MAX;
#if OBSW_VERBOSE_LEVEL >= 1
#if L3GD20_GYRO_DEBUG == 1
PeriodicOperationDivider* debugDivider = nullptr;
#endif
};

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

@ -41,8 +41,8 @@ static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
static constexpr uint16_t POINT_PERCUSSION_COMPENSATION_BIT = 1 << 6;
static constexpr size_t CONFIG_READOUT_SIZE = 8;
static constexpr size_t SENSOR_READOUT_SIZE = 22;
static constexpr size_t CONFIG_READOUT_SIZE = 10 + 2;
static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;
static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;
@ -54,10 +54,7 @@ enum PrimaryPoolIds: lp_id_t {
ACCELERATION_X,
ACCELERATION_Y,
ACCELERATION_Z,
TEMPERATURE
};
enum ConfigPoolIds: lp_id_t {
TEMPERATURE,
DIAG_STAT_REGISTER,
FILTER_SETTINGS,
MSC_CTRL_REGISTER,
@ -113,11 +110,12 @@ public:
/** Constructor for data users like controllers */
AdisGyroConfigDataset(object_id_t adisId):
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_CFG_DATASET_ID)) {
setAllVariablesReadOnly();
}
lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DIAG_STAT_REG);
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);