diff --git a/CHANGELOG.md b/CHANGELOG.md index 561ca54a..62e39ed8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,20 @@ will consitute of a breaking change warranting a new major release: - Changed format of `getCurrentMountPrefix` to return a `const char*`. +## Fixed + +- ADIS1650X: Added missing MDL_RANG pool entry for configuration set + +# [v1.31.1] + +## Fixed + +- ADIS1650X configuration set was empty because the local pool variables were not registered. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/402 +- ACS Controller: Correction for size of MEKF dataset and some optimization and fixes + for actuator control which lead to a crash. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/403 + # [v1.31.0] eive-tmtc: v2.16.0 diff --git a/CMakeLists.txt b/CMakeLists.txt index b6b8e8e8..b597a566 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 1) set(OBSW_VERSION_MINOR 31) -set(OBSW_VERSION_REVISION 0) +set(OBSW_VERSION_REVISION 1) # set(CMAKE_VERBOSE TRUE) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 81e5b1ca..6b821a08 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -432,64 +432,52 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, return returnvalue::OK; } -void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) { - double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; - int32_t rwTargetSpeed[4] = {0, 0, 0, 0}; - updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole); +void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) { + updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole); } -void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], - int16_t mtqTargetDipole[3]) { - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } +void AcsController::updateActuatorCmdData(const double *rwTargetTorque, + const int32_t *rwTargetSpeed, + const int16_t *mtqTargetDipole) { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); } } void AcsController::updateCtrlValData(double errAng) { - double unitQuat[4] = {0, 0, 0, 1}; - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.errQuat.setValid(false); - ctrlValData.errAng.value = errAng; - ctrlValData.errAng.setValid(true); - ctrlValData.setValidity(true, false); - } + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); } } -void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) { - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); - std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); - ctrlValData.errAng.value = errAng; - ctrlValData.setValidity(true, true); - } +void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); + ctrlValData.errAng.value = errAng; + ctrlValData.setValidity(true, true); } } void AcsController::disableCtrlValData() { - double unitQuat[4] = {0, 0, 0, 1}; - double errAng = 0; - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.errAng.value = errAng; - ctrlValData.setValidity(false, true); - } + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double)); + ctrlValData.errAng.value = 0; + ctrlValData.setValidity(false, true); } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a36f99ee..1b57a32a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -40,6 +40,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void performPointingCtrl(); private: + static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; + static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0}; + static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0}; + AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; @@ -84,11 +88,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); - void updateActuatorCmdData(int16_t mtqTargetDipole[3]); - void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], - int16_t mtqTargetDipole[3]); + void updateActuatorCmdData(const int16_t* mtqTargetDipole); + void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed, + const int16_t* mtqTargetDipole); void updateCtrlValData(double errAng); - void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng); + void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng); void disableCtrlValData(); /* ACS Sensor Values */ @@ -187,7 +191,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry tgtQuat = PoolEntry(4); PoolEntry errQuat = PoolEntry(4); PoolEntry errAng = PoolEntry(); - PoolEntry tgtRotRate = PoolEntry(4); + PoolEntry tgtRotRate = PoolEntry(3); // Actuator CMD acsctrl::ActuatorCmdData actuatorCmdData; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 0675262c..c3509a04 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -110,7 +110,7 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; -static constexpr uint8_t MEKF_SET_ENTRIES = 2; +static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp index 0993e233..daff1b06 100644 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -385,6 +385,7 @@ ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry()); localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry()); + localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl); localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry()); localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry()); poolManager.subscribeForRegularPeriodicPacket( diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyroADIS1650XHandler.h index 34335c51..d348baa2 100644 --- a/mission/devices/GyroADIS1650XHandler.h +++ b/mission/devices/GyroADIS1650XHandler.h @@ -63,6 +63,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase { InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; + PoolEntry rangMdl = PoolEntry(); + void prepareReadCommand(uint8_t *regList, size_t len); BurstModes getBurstMode(); diff --git a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h index 89a0c918..3cfda2d4 100644 --- a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h +++ b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h @@ -147,11 +147,15 @@ class AdisGyroConfigDataset : public StaticLocalDataSet<5> { setAllVariablesReadOnly(); } - lp_var_t diagStatReg = lp_var_t(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER); - lp_var_t filterSetting = lp_var_t(sid.objectId, ADIS1650X::FILTER_SETTINGS); - lp_var_t rangMdl = lp_var_t(sid.objectId, ADIS1650X::RANG_MDL); - lp_var_t mscCtrlReg = lp_var_t(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER); - lp_var_t decRateReg = lp_var_t(sid.objectId, ADIS1650X::DEC_RATE_REGISTER); + lp_var_t diagStatReg = + lp_var_t(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this); + lp_var_t filterSetting = + lp_var_t(sid.objectId, ADIS1650X::FILTER_SETTINGS, this); + lp_var_t rangMdl = lp_var_t(sid.objectId, ADIS1650X::RANG_MDL, this); + lp_var_t mscCtrlReg = + lp_var_t(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this); + lp_var_t decRateReg = + lp_var_t(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this); private: friend class GyroADIS1650XHandler; diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 50f34b0d..ca09a434 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -17,8 +17,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { * @param maxNumberOfSequences * @param maxNumberOfTables * @param transmitterTimeout Maximum time the transmitter of the syrlinks - * will be - * enabled + * will + * be enabled */ ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, uint32_t transmitterTimeout);