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

add handling for correct range setting
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2023-01-18 13:43:28 +01:00
parent a70570f79d
commit 3682a22021
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
4 changed files with 56 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
bsp_q7s/obsw.cpp
View File

@ -1,13 +1,13 @@
#include "obsw.h"
#include <pwd.h>
#include <sys/types.h>
#include <unistd.h>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <unistd.h>
#include <sys/types.h>
#include <pwd.h>
#include "OBSWConfig.h"
#include "commonConfig.h"
#include "core/scheduling.h"
@ -44,8 +44,8 @@ int obsw::obsw() {
const char* homedir = nullptr;
homedir = getenv("HOME");
if(homedir == nullptr) {
homedir = getpwuid(getuid())->pw_dir;
if (homedir == nullptr) {
homedir = getpwuid(getuid())->pw_dir;
}
std::filesystem::path bootDelayFile = std::filesystem::path(homedir) / "boot_delay_secs.txt";
// Init delay handling.

47
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -99,12 +99,13 @@ ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t de
switch (deviceCommand) {
case (ADIS1650X::READ_OUT_CONFIG): {
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
uint8_t regList[5] = {};
uint8_t regList[6] = {};
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;
regList[2] = ADIS1650X::RANG_MDL_REG;
regList[3] = ADIS1650X::MSC_CTRL_REG;
regList[4] = ADIS1650X::DEC_RATE_REG;
regList[5] = ADIS1650X::PROD_ID_REG;
prepareReadCommand(regList, sizeof(regList));
this->rawPacket = commandBuffer.data();
break;
@ -203,6 +204,7 @@ ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t re
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
using namespace ADIS1650X;
switch (id) {
case (ADIS1650X::READ_OUT_CONFIG): {
uint16_t readProdId = packet[10] << 8 | packet[11];
@ -220,8 +222,32 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
PoolReadGuard rg(&configDataset);
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];
uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
ADIS1650X::RangMdlBitfield bitfield =
static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
switch (bitfield) {
case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
range = GyroRange::MODEL_1BMLZ;
break;
}
case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
range = GyroRange::MODEL_2BMLZ;
break;
}
case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
range = GyroRange::MODEL_3BMLZ;
break;
}
case (RangMdlBitfield::RESERVED): {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
#endif
break;
}
}
configDataset.rangMdl.value = rangMdlRaw;
configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
configDataset.decRateReg.value = packet[10] << 8 | packet[11];
configDataset.setValidity(true, true);
if (internalState == InternalState::CONFIG) {
commandExecuted = true;
@ -271,14 +297,11 @@ ReturnValue_t GyroADIS1650XHandler::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 * ADIS1650X::GYRO_RANGE;
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX * range;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value =
static_cast<float>(angVelocYRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX * range;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value =
static_cast<float>(angVelocZRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX * range;
float accelScaling = 0;
if (adisType == ADIS1650X::Type::ADIS16507) {

2
mission/devices/GyroADIS1650XHandler.h
View File

@ -46,6 +46,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
ADIS1650X::Type adisType;
AdisGyroPrimaryDataset primaryDataset;
AdisGyroConfigDataset configDataset;
ADIS1650X::GyroRange range = ADIS1650X::GyroRange::UNSET;
bool goToNormalMode = false;
bool warningSwitch = true;

15
mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h
View File

@ -13,7 +13,15 @@ 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;
enum GyroRange { UNSET = 0, MODEL_1BMLZ = 125, MODEL_2BMLZ = 500, MODEL_3BMLZ = 2000 };
enum RangMdlBitfield {
RANGE_125_1BMLZ = 0b00,
RANGE_500_2BMLZ = 0b01,
RESERVED = 0b10,
RANGE_2000_3BMLZ = 0b11
};
static constexpr uint32_t ACCELEROMETER_RANGE_16507 = 392;
static constexpr float ACCELEROMETER_RANGE_16505 = 78.4;
@ -33,6 +41,7 @@ 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 RANG_MDL_REG = 0x5e;
static constexpr uint8_t MSC_CTRL_REG = 0x60;
static constexpr uint8_t DEC_RATE_REG = 0x64;
static constexpr uint8_t GLOB_CMD = 0x68;
@ -54,7 +63,7 @@ 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 = 10 + 2;
static constexpr size_t CONFIG_READOUT_SIZE = 12 + 2;
static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;
static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
@ -78,6 +87,7 @@ enum PrimaryPoolIds : lp_id_t {
TEMPERATURE,
DIAG_STAT_REGISTER,
FILTER_SETTINGS,
RANG_MDL,
MSC_CTRL_REGISTER,
DEC_RATE_REGISTER,
};
@ -130,6 +140,7 @@ class AdisGyroConfigDataset : public StaticLocalDataSet<5> {
lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, 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> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL);
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);