From 59b80807baa3f2236399744361c48f24136510d6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 18:34:28 +0100 Subject: [PATCH 1/8] possible fix but not sure --- mission/controller/AcsController.cpp | 30 +++++++++++++-------------- mission/controller/AcsController.h | 12 ++++++----- mission/system/objects/ComSubsystem.h | 4 ++-- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 81e5b1ca..7643edaa 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -432,14 +432,15 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, return returnvalue::OK; } -void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) { +void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) { 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(double rwTargetTorque[4], int32_t rwTargetSpeed[4], - int16_t mtqTargetDipole[3]) { +void AcsController::updateActuatorCmdData(const double *rwTargetTorque, + const int32_t *rwTargetSpeed, + const int16_t *mtqTargetDipole) { { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { @@ -452,22 +453,19 @@ void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTa } 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) { +void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) { { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a36f99ee..eaf80a23 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -40,6 +40,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void performPointingCtrl(); private: + static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; + AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; @@ -84,11 +86,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 +189,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/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); From 0907e8f5e520fdff70af0868fa6b3b12b4f5d2ae Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 18:39:14 +0100 Subject: [PATCH 2/8] use more constants --- mission/controller/AcsController.cpp | 48 +++++++++++----------------- mission/controller/AcsController.h | 2 ++ 2 files changed, 21 insertions(+), 29 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7643edaa..6b821a08 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -433,22 +433,18 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, } void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) { - double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; - int32_t rwTargetSpeed[4] = {0, 0, 0, 0}; - updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole); + updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole); } 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); - } + 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); } } @@ -466,28 +462,22 @@ void AcsController::updateCtrlValData(double errAng) { } 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); - } + 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 eaf80a23..1b57a32a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -41,6 +41,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes 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; From ee974d0e2d199c76642bbef6e743381698430fac Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 18:41:25 +0100 Subject: [PATCH 3/8] register ADIS config set pool variables --- CHANGELOG.md | 4 ++++ .../devicedefinitions/GyroADIS1650XDefinitions.h | 14 +++++++++----- mission/system/objects/ComSubsystem.h | 4 ++-- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b5e570a0..94a66058 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- ADIS1650X configuration set was empty because the local pool variables were not registered. + # [v1.31.0] eive-tmtc: v2.16.0 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); From 8da373542ed6b90736eef15257e23a96390e470b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 18:51:31 +0100 Subject: [PATCH 4/8] changelog --- CHANGELOG.md | 6 ++++++ .../controller/controllerdefinitions/AcsCtrlDefinitions.h | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b5e570a0..e032c95d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- ACS Controller: Correction for size of MEKF dataset and some optimization and possible fixes + for actuator control. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/403 + # [v1.31.0] eive-tmtc: v2.16.0 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; From 089d04646aa820c6021cbef043a3bfc4d878932a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 18:53:29 +0100 Subject: [PATCH 5/8] prep v1.31.1 --- CHANGELOG.md | 2 ++ CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e032c95d..5e921cc9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v1.31.1] + ## Fixed - ACS Controller: Correction for size of MEKF dataset and some optimization and possible fixes diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c698dfe..a0768ddd 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) From e33a0fd60bd6512b08189500fabf134af91e7c4d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 19:51:07 +0100 Subject: [PATCH 6/8] that was definitely a fix --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a294062a..dd893586 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,8 +22,8 @@ will consitute of a breaking change warranting a new major release: - 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 possible fixes - for actuator control. +- 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] From eb2a0604e9da750fcb07d848de9db12c88bdae57 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Feb 2023 01:09:00 +0100 Subject: [PATCH 7/8] rang mdl pool entry --- mission/devices/GyroADIS1650XHandler.cpp | 1 + mission/devices/GyroADIS1650XHandler.h | 2 ++ 2 files changed, 3 insertions(+) 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(); From 104d9647fece4fa2d917557e36601d0c86cdd833 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Feb 2023 01:09:40 +0100 Subject: [PATCH 8/8] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dd893586..51b7209d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- ADIS1650X: Added missing MDL_RANG pool entry for configuration set + # [v1.31.1] ## Fixed