From f2bc374f0ffd349c3c02552885433735eb632f92 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 18:12:59 +0200 Subject: [PATCH] Device handler updates --- .../fsfw_hal/devicehandlers/CMakeLists.txt | 2 + .../devicehandlers/GyroL3GD20Handler.cpp | 43 ++++- .../devicehandlers/GyroL3GD20Handler.h | 21 ++- .../devicedefinitions/MgmLIS3HandlerDefs.h | 178 ++++++++++++++++++ .../devicedefinitions/MgmRM3100HandlerDefs.h | 132 +++++++++++++ 5 files changed, 364 insertions(+), 12 deletions(-) create mode 100644 hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h create mode 100644 hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h diff --git a/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt b/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt index cf542d8d..94e67c72 100644 --- a/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt +++ b/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt @@ -1,3 +1,5 @@ target_sources(${LIB_FSFW_NAME} PRIVATE GyroL3GD20Handler.cpp + MgmRM3100Handler.cpp + MgmLIS3MDLHandler.cpp ) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 4a492e5d..70ba49eb 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -3,11 +3,11 @@ #include "fsfw/datapool/PoolReadGuard.h" GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, uint8_t switchId, uint32_t transitionDelayMs): + CookieIF *comCookie, uint32_t transitionDelayMs): DeviceHandlerBase(objectId, deviceCommunication, comCookie), - switchId(switchId), transitionDelayMs(transitionDelayMs), dataset(this) { + transitionDelayMs(transitionDelayMs), dataset(this) { #if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(5); + debugDivider = new PeriodicOperationDivider(3); #endif } @@ -215,11 +215,32 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, PoolReadGuard readSet(&dataset); if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - dataset.angVelocX = angVelocX; - dataset.angVelocY = angVelocY; - dataset.angVelocZ = angVelocZ; + if(std::abs(angVelocX) < this->absLimitX) { + dataset.angVelocX = angVelocX; + dataset.angVelocX.setValid(true); + } + else { + dataset.angVelocX.setValid(false); + } + + if(std::abs(angVelocY) < this->absLimitY) { + dataset.angVelocY = angVelocY; + dataset.angVelocY.setValid(true); + } + else { + dataset.angVelocY.setValid(false); + } + + if(std::abs(angVelocZ) < this->absLimitZ) { + dataset.angVelocZ = angVelocZ; + dataset.angVelocZ.setValid(true); + } + else { + dataset.angVelocZ.setValid(false); + } + dataset.temperature = temperature; - dataset.setValidity(true, true); + dataset.temperature.setValid(true); } break; } @@ -234,7 +255,7 @@ uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { return this->transitionDelayMs; } -void GyroHandlerL3GD20H::setGoNormalModeAtStartup() { +void GyroHandlerL3GD20H::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; } @@ -256,3 +277,9 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() { void GyroHandlerL3GD20H::modeChanged() { internalState = InternalState::NONE; } + +void GyroHandlerL3GD20H::setAbsoluteLimits(float limitX, float limitY, float limitZ) { + this->absLimitX = limitX; + this->absLimitY = limitY; + this->absLimitZ = limitZ; +} diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index bc1d9c1c..870f551d 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -19,13 +19,22 @@ class GyroHandlerL3GD20H: public DeviceHandlerBase { public: GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelayMs = 10000); + CookieIF* comCookie, uint32_t transitionDelayMs); virtual ~GyroHandlerL3GD20H(); + /** + * Set the absolute limit for the values on the axis in degrees per second. + * The dataset values will be marked as invalid if that limit is exceeded + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float limitX, float limitY, float limitZ); + /** * @brief Configure device handler to go to normal mode immediately */ - void setGoNormalModeAtStartup(); + void setToGoToNormalMode(bool enable); protected: /* DeviceHandlerBase overrides */ @@ -40,12 +49,12 @@ protected: size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; void fillCommandAndReplyMap() override; void modeChanged() override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; @@ -54,6 +63,10 @@ private: uint32_t transitionDelayMs = 0; GyroPrimaryDataset dataset; + float absLimitX = L3GD20H::RANGE_DPS_00; + float absLimitY = L3GD20H::RANGE_DPS_00; + float absLimitZ = L3GD20H::RANGE_DPS_00; + enum class InternalState { NONE, CONFIGURE, diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h new file mode 100644 index 00000000..b6f3fd84 --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h @@ -0,0 +1,178 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ + +#include +#include +#include +#include + +namespace MGMLIS3MDL { + +enum Set { + ON, OFF +}; +enum OpMode { + LOW, MEDIUM, HIGH, ULTRA +}; + +enum Sensitivies: uint8_t { + GAUSS_4 = 4, + GAUSS_8 = 8, + GAUSS_12 = 12, + GAUSS_16 = 16 +}; + +/* Actually 15, we just round up a bit */ +static constexpr size_t MAX_BUFFER_SIZE = 16; + +/* Field data register scaling */ +static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100; +static constexpr float FIELD_LSB_PER_GAUSS_4_SENS = 1.0 / 6842.0; +static constexpr float FIELD_LSB_PER_GAUSS_8_SENS = 1.0 / 3421.0; +static constexpr float FIELD_LSB_PER_GAUSS_12_SENS = 1.0 / 2281.0; +static constexpr float FIELD_LSB_PER_GAUSS_16_SENS = 1.0 / 1711.0; + +static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00; +static const DeviceCommandId_t SETUP_MGM = 0x01; +static const DeviceCommandId_t READ_TEMPERATURE = 0x02; +static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03; +static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04; +static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x05; + +/* Number of all control registers */ +static const uint8_t NR_OF_CTRL_REGISTERS = 5; +/* Number of registers in the MGM */ +static const uint8_t NR_OF_REGISTERS = 19; +/* Total number of adresses for all registers */ +static const uint8_t TOTAL_NR_OF_ADRESSES = 52; +static const uint8_t NR_OF_DATA_AND_CFG_REGISTERS = 14; +static const uint8_t TEMPERATURE_REPLY_LEN = 3; +static const uint8_t SETUP_REPLY_LEN = 6; + +/*------------------------------------------------------------------------*/ +/* Register adresses */ +/*------------------------------------------------------------------------*/ +/* Register adress returns identifier of device with default 0b00111101 */ +static const uint8_t IDENTIFY_DEVICE_REG_ADDR = 0b00001111; +static const uint8_t DEVICE_ID = 0b00111101; // Identifier for Device + +/* Register adress to access register 1 */ +static const uint8_t CTRL_REG1 = 0b00100000; +/* Register adress to access register 2 */ +static const uint8_t CTRL_REG2 = 0b00100001; +/* Register adress to access register 3 */ +static const uint8_t CTRL_REG3 = 0b00100010; +/* Register adress to access register 4 */ +static const uint8_t CTRL_REG4 = 0b00100011; +/* Register adress to access register 5 */ +static const uint8_t CTRL_REG5 = 0b00100100; + +/* Register adress to access status register */ +static const uint8_t STATUS_REG_IDX = 8; +static const uint8_t STATUS_REG = 0b00100111; + +/* Register adress to access low byte of x-axis */ +static const uint8_t X_LOWBYTE_IDX = 9; +static const uint8_t X_LOWBYTE = 0b00101000; +/* Register adress to access high byte of x-axis */ +static const uint8_t X_HIGHBYTE_IDX = 10; +static const uint8_t X_HIGHBYTE = 0b00101001; +/* Register adress to access low byte of y-axis */ +static const uint8_t Y_LOWBYTE_IDX = 11; +static const uint8_t Y_LOWBYTE = 0b00101010; +/* Register adress to access high byte of y-axis */ +static const uint8_t Y_HIGHBYTE_IDX = 12; +static const uint8_t Y_HIGHBYTE = 0b00101011; +/* Register adress to access low byte of z-axis */ +static const uint8_t Z_LOWBYTE_IDX = 13; +static const uint8_t Z_LOWBYTE = 0b00101100; +/* Register adress to access high byte of z-axis */ +static const uint8_t Z_HIGHBYTE_IDX = 14; +static const uint8_t Z_HIGHBYTE = 0b00101101; + +/* Register adress to access low byte of temperature sensor */ +static const uint8_t TEMP_LOWBYTE = 0b00101110; +/* Register adress to access high byte of temperature sensor */ +static const uint8_t TEMP_HIGHBYTE = 0b00101111; + +/*------------------------------------------------------------------------*/ +/* Initialize Setup Register set bits */ +/*------------------------------------------------------------------------*/ +/* General transfer bits */ +// Read=1 / Write=0 Bit +static const uint8_t RW_BIT = 7; +// Continous Read/Write Bit, increment adress +static const uint8_t MS_BIT = 6; + +/* CTRL_REG1 bits */ +static const uint8_t ST = 0; // Self test enable bit, enabled = 1 +// Enable rates higher than 80 Hz enabled = 1 +static const uint8_t FAST_ODR = 1; +static const uint8_t DO0 = 2; // Output data rate bit 2 +static const uint8_t DO1 = 3; // Output data rate bit 3 +static const uint8_t DO2 = 4; // Output data rate bit 4 +static const uint8_t OM0 = 5; // XY operating mode bit 5 +static const uint8_t OM1 = 6; // XY operating mode bit 6 +static const uint8_t TEMP_EN = 7; // Temperature sensor enable enabled = 1 +static const uint8_t CTRL_REG1_DEFAULT = (1 << TEMP_EN) | (1 << OM1) | + (1 << DO0) | (1 << DO1) | (1 << DO2); + +/* CTRL_REG2 bits */ +//reset configuration registers and user registers +static const uint8_t SOFT_RST = 2; +static const uint8_t REBOOT = 3; //reboot memory content +static const uint8_t FSO = 5; //full-scale selection bit 5 +static const uint8_t FS1 = 6; //full-scale selection bit 6 +static const uint8_t CTRL_REG2_DEFAULT = 0; + +/* CTRL_REG3 bits */ +static const uint8_t MD0 = 0; //Operating mode bit 0 +static const uint8_t MD1 = 1; //Operating mode bit 1 +//SPI serial interface mode selection enabled = 3-wire-mode +static const uint8_t SIM = 2; +static const uint8_t LP = 5; //low-power mode +static const uint8_t CTRL_REG3_DEFAULT = 0; + +/* CTRL_REG4 bits */ +//big/little endian data selection enabled = MSb at lower adress +static const uint8_t BLE = 1; +static const uint8_t OMZ0 = 2; //Z operating mode bit 2 +static const uint8_t OMZ1 = 3; //Z operating mode bit 3 +static const uint8_t CTRL_REG4_DEFAULT = (1 << OMZ1); + +/* CTRL_REG5 bits */ +static const uint8_t BDU = 6; //Block data update +static const uint8_t FAST_READ = 7; //Fast read enabled = 1 +static const uint8_t CTRL_REG5_DEFAULT = 0; + +static const uint32_t MGM_DATA_SET_ID = READ_CONFIG_AND_DATA; + +enum MgmPoolIds: lp_id_t { + FIELD_STRENGTH_X, + FIELD_STRENGTH_Y, + FIELD_STRENGTH_Z, + TEMPERATURE_CELCIUS +}; + +class MgmPrimaryDataset: public StaticLocalDataSet<5> { +public: + MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {} + + MgmPrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, MGM_DATA_SET_ID)) {} + + lp_var_t fieldStrengthX = lp_var_t(sid.objectId, + FIELD_STRENGTH_X, this); + lp_var_t fieldStrengthY = lp_var_t(sid.objectId, + FIELD_STRENGTH_Y, this); + lp_var_t fieldStrengthZ = lp_var_t(sid.objectId, + FIELD_STRENGTH_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + TEMPERATURE_CELCIUS, this); +}; + +} + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ */ diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h new file mode 100644 index 00000000..08f80dd9 --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h @@ -0,0 +1,132 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ + +#include +#include +#include +#include +#include + +namespace RM3100 { + +/* Actually 10, we round up a little bit */ +static constexpr size_t MAX_BUFFER_SIZE = 12; + +static constexpr uint8_t READ_MASK = 0x80; + +/*----------------------------------------------------------------------------*/ +/* CMM Register */ +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t SET_CMM_CMZ = 1 << 6; +static constexpr uint8_t SET_CMM_CMY = 1 << 5; +static constexpr uint8_t SET_CMM_CMX = 1 << 4; +static constexpr uint8_t SET_CMM_DRDM = 1 << 2; +static constexpr uint8_t SET_CMM_START = 1; +static constexpr uint8_t CMM_REGISTER = 0x01; + +static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX | + SET_CMM_DRDM | SET_CMM_START; + +/*----------------------------------------------------------------------------*/ +/* Cycle count register */ +/*----------------------------------------------------------------------------*/ +// Default value (200) +static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8; + +static constexpr float DEFAULT_GAIN = static_cast(CYCLE_COUNT_VALUE) / + 100 * 38; +static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04; + +/*----------------------------------------------------------------------------*/ +/* TMRC register */ +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t TMRC_150HZ_VALUE = 0x94; +static constexpr uint8_t TMRC_75HZ_VALUE = 0x95; +static constexpr uint8_t TMRC_DEFAULT_37HZ_VALUE = 0x96; + +static constexpr uint8_t TMRC_REGISTER = 0x0B; +static constexpr uint8_t TMRC_DEFAULT_VALUE = TMRC_DEFAULT_37HZ_VALUE; + +static constexpr uint8_t MEASUREMENT_REG_START = 0x24; +static constexpr uint8_t BIST_REGISTER = 0x33; +static constexpr uint8_t DATA_READY_VAL = 0b1000'0000; +static constexpr uint8_t STATUS_REGISTER = 0x34; +static constexpr uint8_t REVID_REGISTER = 0x36; + +// Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM) +static constexpr uint16_t RANGE = 800; + +static constexpr DeviceCommandId_t READ_DATA = 0; + +static constexpr DeviceCommandId_t CONFIGURE_CMM = 1; +static constexpr DeviceCommandId_t READ_CMM = 2; + +static constexpr DeviceCommandId_t CONFIGURE_TMRC = 3; +static constexpr DeviceCommandId_t READ_TMRC = 4; + +static constexpr DeviceCommandId_t CONFIGURE_CYCLE_COUNT = 5; +static constexpr DeviceCommandId_t READ_CYCLE_COUNT = 6; + +class CycleCountCommand: public SerialLinkedListAdapter { +public: + CycleCountCommand(bool oneCycleCount = true): oneCycleCount(oneCycleCount) { + setLinks(oneCycleCount); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override { + ReturnValue_t result = SerialLinkedListAdapter::deSerialize(buffer, + size, streamEndianness); + if(oneCycleCount) { + cycleCountY = cycleCountX; + cycleCountZ = cycleCountX; + } + return result; + } + + SerializeElement cycleCountX; + SerializeElement cycleCountY; + SerializeElement cycleCountZ; + +private: + void setLinks(bool oneCycleCount) { + setStart(&cycleCountX); + if(not oneCycleCount) { + cycleCountX.setNext(&cycleCountY); + cycleCountY.setNext(&cycleCountZ); + } + } + + bool oneCycleCount; +}; + +static constexpr uint32_t MGM_DATASET_ID = READ_DATA; + +enum MgmPoolIds: lp_id_t { + FIELD_STRENGTH_X, + FIELD_STRENGTH_Y, + FIELD_STRENGTH_Z, +}; + +class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {} + + Rm3100PrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, MGM_DATASET_ID)) {} + + // Field strengths in micro Tesla. + lp_var_t fieldStrengthX = lp_var_t(sid.objectId, + FIELD_STRENGTH_X, this); + lp_var_t fieldStrengthY = lp_var_t(sid.objectId, + FIELD_STRENGTH_Y, this); + lp_var_t fieldStrengthZ = lp_var_t(sid.objectId, + FIELD_STRENGTH_Z, this); +}; + +} + + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */