From 5a5d00e4e50bf81bcb2932386850724bad31e966 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 9 Feb 2023 17:11:23 +0100 Subject: [PATCH 01/69] dataPool for setting RW speeds --- mission/devices/RwHandler.cpp | 2 + mission/devices/RwHandler.h | 2 + .../devices/devicedefinitions/RwDefinitions.h | 54 +++++++++++++++---- 3 files changed, 47 insertions(+), 11 deletions(-) diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 732bb9fe..d859f9d0 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -243,6 +243,8 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry({0})); diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index f9f66bd5..43deb579 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -96,6 +96,8 @@ class RwHandler : public DeviceHandlerBase { uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; + PoolEntry rwSpeed = PoolEntry(0, false); + enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS }; InternalState internalState = InternalState::GET_RESET_STATUS; diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index 2923b41b..4016b23c 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -51,7 +51,9 @@ enum PoolIds : lp_id_t { SPI_BYTES_WRITTEN, SPI_BYTES_READ, SPI_REG_OVERRUN_ERRORS, - SPI_TOTAL_ERRORS + SPI_TOTAL_ERRORS, + + RW_SPEED, }; enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING }; @@ -75,10 +77,13 @@ static const DeviceCommandId_t SET_SPEED = 6; static const DeviceCommandId_t GET_TEMPERATURE = 8; static const DeviceCommandId_t GET_TM = 9; -static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE; -static const uint32_t STATUS_SET_ID = GET_RW_STATUS; -static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS; -static const uint32_t TM_SET_ID = GET_TM; +enum SetIds : uint32_t { + TEMPERATURE_SET_ID = GET_TEMPERATURE, + STATUS_SET_ID = GET_RW_STATUS, + LAST_RESET_ID = GET_LAST_RESET_STATUS, + TM_SET_ID = GET_TM, + RW_SPEED = 10, +}; static const size_t SIZE_GET_RESET_STATUS = 5; static const size_t SIZE_CLEAR_RESET_STATUS = 4; @@ -106,9 +111,11 @@ static const uint8_t TM_SET_ENTRIES = 24; */ class StatusSet : public StaticLocalDataSet { public: - StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, STATUS_SET_ID) {} + StatusSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, RwDefinitions::SetIds::STATUS_SET_ID) {} - StatusSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {} + StatusSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::STATUS_SET_ID)) {} lp_var_t temperatureCelcius = lp_var_t(sid.objectId, PoolIds::TEMPERATURE_C, this); @@ -124,9 +131,11 @@ class StatusSet : public StaticLocalDataSet { */ class LastResetSatus : public StaticLocalDataSet { public: - LastResetSatus(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LAST_RESET_ID) {} + LastResetSatus(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, RwDefinitions::SetIds::LAST_RESET_ID) {} - LastResetSatus(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {} + LastResetSatus(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::LAST_RESET_ID)) {} // If a reset occurs, the status code will be cached into this variable lp_var_t lastNonClearedResetStatus = @@ -143,9 +152,11 @@ class LastResetSatus : public StaticLocalDataSet { */ class TmDataset : public StaticLocalDataSet { public: - TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TM_SET_ID) {} + TmDataset(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, RwDefinitions::SetIds::TM_SET_ID) {} - TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TM_SET_ID)) {} + TmDataset(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::TM_SET_ID)) {} lp_var_t lastResetStatus = lp_var_t(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this); @@ -192,6 +203,27 @@ class TmDataset : public StaticLocalDataSet { lp_var_t(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this); }; +class RwSpeedActuationSet : StaticLocalDataSet<4> { + friend class ::RwHandler; + + public: + RwSpeedActuationSet(HasLocalDataPoolIF& owner) + : StaticLocalDataSet(&owner, RwDefinitions::SetIds::RW_SPEED) {} + RwSpeedActuationSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::RW_SPEED)) {} + + void setRwSpeed(int32_t rwSpeed_) { + if (rwSpeed.value != rwSpeed_) { + } + rwSpeed = rwSpeed_; + } + + void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; } + + private: + lp_var_t rwSpeed = lp_var_t(sid.objectId, RW_SPEED, this); +}; + } // namespace RwDefinitions #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */ From a3eaace4a55504e3a6dc13b933031e3e1b13963b Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 9 Feb 2023 17:31:26 +0100 Subject: [PATCH 02/69] added RwSpeedActuationSets to AcsController --- mission/controller/AcsController.cpp | 51 ++++++++++++++++++++++++++-- mission/controller/AcsController.h | 8 ++++- 2 files changed, 56 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1b4f54a9..bee21d71 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -189,8 +189,23 @@ void AcsController::performSafe() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], - // torqueDuration); + // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], 0); + // } + // { + // PoolReadGuard pg(&rw1SpeedSet); + // rw1SpeedSet.setRwSpeed(0); + // } + // { + // PoolReadGuard pg(&rw2SpeedSet); + // rw2SpeedSet.setRwSpeed(0); + // } + // { + // PoolReadGuard pg(&rw3SpeedSet); + // rw3SpeedSet.setRwSpeed(0); + // } + // { + // PoolReadGuard pg(&rw4SpeedSet); + // rw4SpeedSet.setRwSpeed(0); // } } @@ -251,6 +266,22 @@ void AcsController::performDetumble() { // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } + // { + // PoolReadGuard pg(&rw1SpeedSet); + // rw1SpeedSet.setRwSpeed(0); + // } + // { + // PoolReadGuard pg(&rw2SpeedSet); + // rw2SpeedSet.setRwSpeed(0); + // } + // { + // PoolReadGuard pg(&rw3SpeedSet); + // rw3SpeedSet.setRwSpeed(0); + // } + // { + // PoolReadGuard pg(&rw4SpeedSet); + // rw4SpeedSet.setRwSpeed(0); + // } } void AcsController::performPointingCtrl() { @@ -431,6 +462,22 @@ void AcsController::performPointingCtrl() { // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } + // { + // PoolReadGuard pg(&rw1SpeedSet); + // rw1SpeedSet.setRwSpeed(cmdRwSpeedInt[0]); + // } + // { + // PoolReadGuard pg(&rw2SpeedSet); + // rw2SpeedSet.setRwSpeed(cmdRwSpeedInt[1]); + // } + // { + // PoolReadGuard pg(&rw3SpeedSet); + // rw3SpeedSet.setRwSpeed(cmdRwSpeedInt[2]); + // } + // { + // PoolReadGuard pg(&rw4SpeedSet); + // rw4SpeedSet.setRwSpeed(cmdRwSpeedInt[3]); + // } } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1e017099..d427bd5b 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -17,6 +17,7 @@ #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" @@ -72,8 +73,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes /* ACS Sensor Values */ ACS::SensorValues sensorValues; - /* ACS Datasets */ + /* ACS Actuation Datasets */ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); + RwDefinitions::RwSpeedActuationSet rw1SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW1); + RwDefinitions::RwSpeedActuationSet rw2SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW2); + RwDefinitions::RwSpeedActuationSet rw3SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW3); + RwDefinitions::RwSpeedActuationSet rw4SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW4); + /* ACS Datasets */ // MGMs acsctrl::MgmDataRaw mgmDataRaw; PoolEntry mgm0VecRaw = PoolEntry(3); From 51629da4a068e1aad1364692a339048e64accc49 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 9 Feb 2023 17:55:20 +0100 Subject: [PATCH 03/69] naming fixes --- mission/devices/devicedefinitions/RwDefinitions.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index 4016b23c..a4e8e96d 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -82,7 +82,7 @@ enum SetIds : uint32_t { STATUS_SET_ID = GET_RW_STATUS, LAST_RESET_ID = GET_LAST_RESET_STATUS, TM_SET_ID = GET_TM, - RW_SPEED = 10, + SPEED_CMD_SET = 10, }; static const size_t SIZE_GET_RESET_STATUS = 5; @@ -204,13 +204,13 @@ class TmDataset : public StaticLocalDataSet { }; class RwSpeedActuationSet : StaticLocalDataSet<4> { - friend class ::RwHandler; + friend class RwHandler; public: RwSpeedActuationSet(HasLocalDataPoolIF& owner) - : StaticLocalDataSet(&owner, RwDefinitions::SetIds::RW_SPEED) {} + : StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {} RwSpeedActuationSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::RW_SPEED)) {} + : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {} void setRwSpeed(int32_t rwSpeed_) { if (rwSpeed.value != rwSpeed_) { @@ -221,7 +221,8 @@ class RwSpeedActuationSet : StaticLocalDataSet<4> { void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; } private: - lp_var_t rwSpeed = lp_var_t(sid.objectId, RW_SPEED, this); + lp_var_t rwSpeed = + lp_var_t(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this); }; } // namespace RwDefinitions From 9dfd8491d26aee8b619818c3883da58e104afdb6 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 10:25:13 +0100 Subject: [PATCH 04/69] added rampTime and torqueDuration to AcsParameters --- mission/controller/acs/AcsParameters.cpp | 5 +++++ mission/controller/acs/AcsParameters.h | 3 +++ 2 files changed, 8 insertions(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 13642fe9..30264963 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -278,6 +278,8 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x4: parameterWrapper->set(rwHandlingParameters.stictionTorque); break; + case 0x5: + parameterWrapper->set(rwHandlingParameters.rampTime); default: return INVALID_IDENTIFIER_ID; } @@ -584,6 +586,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x5: parameterWrapper->set(magnetorquesParameter.DipolMax); break; + case 0x6: + parameterWrapper->set(magnetorquesParameter.torqueDuration); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f1c0fb63..35e316f1 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -792,6 +792,8 @@ class AcsParameters : public HasParametersIF { double stictionSpeed = 100; // 80; // RPM double stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; + + uint16_t rampTime = 10; } rwHandlingParameters; struct RwMatrices { @@ -910,6 +912,7 @@ class AcsParameters : public HasParametersIF { double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double DipolMax = 0.2; // [Am^2] + uint16_t torqueDuration = 300; // [ms] } magnetorquesParameter; struct DetumbleParameter { From 9041a3376c18cae282a1afcfbcd39c3ef3b09c47 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 10:56:50 +0100 Subject: [PATCH 05/69] ActuatorCmd now output their solutions as integers as expected by the sensors --- mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++++++++++--------- mission/controller/acs/ActuatorCmd.h | 6 +++--- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 4e0052e0..cbe24e5b 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -38,27 +38,32 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { } } -void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, - const double *rwTorque, double *rwCmdSpeed) { +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double *rwTorque, int32_t *rwCmdSpeed) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; double deltaSpeed[4] = {0, 0, 0, 0}; double commandTime = acsParameters.onBoardParams.sampleTime, inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] double factor = commandTime / inertiaWheel * radToRpm; + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); - VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); + for (int i = 0; i < 4; i++) { + deltaSpeedInt[i] = std::round(deltaSpeed[i]); + } + VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) { +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { // Convert to actuator frame + double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, - dipolMoment, dipolMomentActuator, 3, 3, 1); + dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); // Scaling along largest element if dipol exceeds maximum double maxDipol = acsParameters.magnetorquesParameter.DipolMax; double maxValue = 0; @@ -69,8 +74,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu } if (maxValue > maxDipol) { double scalingFactor = maxDipol / maxValue; - VectorOperations::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3); + VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, + dipolMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3); + VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + for (int i = 0; i < 3; i++) { + dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 04e2b61f..969bd782 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -28,8 +28,8 @@ class ActuatorCmd { * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -37,7 +37,7 @@ class ActuatorCmd { * @param: dipolMoment given dipol moment in spacecraft frame * dipolMomentActuator resulting dipol moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator); + void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); protected: private: From c2079dcbbad994877c3dc3a638c25ea325699f0d Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 11:26:46 +0100 Subject: [PATCH 06/69] added rampTime and made dataSet public --- .../devices/devicedefinitions/RwDefinitions.h | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index a4e8e96d..b3cb0fe4 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -54,6 +54,7 @@ enum PoolIds : lp_id_t { SPI_TOTAL_ERRORS, RW_SPEED, + RAMP_TIME, }; enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING }; @@ -203,7 +204,7 @@ class TmDataset : public StaticLocalDataSet { lp_var_t(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this); }; -class RwSpeedActuationSet : StaticLocalDataSet<4> { +class RwSpeedActuationSet : public StaticLocalDataSet<2> { friend class RwHandler; public: @@ -212,17 +213,25 @@ class RwSpeedActuationSet : StaticLocalDataSet<4> { RwSpeedActuationSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {} - void setRwSpeed(int32_t rwSpeed_) { + void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) { if (rwSpeed.value != rwSpeed_) { + rwSpeed = rwSpeed_; + } + if (rampTime.value != rampTime_) { + rampTime = rampTime_; } - rwSpeed = rwSpeed_; } - void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; } + void getRwSpeed(int32_t& rwSpeed_, uint16_t& rampTime_) { + rwSpeed_ = rwSpeed.value; + rampTime_ = rampTime.value; + } private: lp_var_t rwSpeed = lp_var_t(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this); + lp_var_t rampTime = + lp_var_t(sid.objectId, RwDefinitions::PoolIds::RAMP_TIME, this); }; } // namespace RwDefinitions From 01d6060111ee96cedc8e40e3368aae3414a38562 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 11:27:02 +0100 Subject: [PATCH 07/69] added ramp time --- mission/devices/RwHandler.cpp | 1 + mission/devices/RwHandler.h | 1 + 2 files changed, 2 insertions(+) diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index d859f9d0..e9383c4a 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -244,6 +244,7 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime); localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 43deb579..17f2b396 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -97,6 +97,7 @@ class RwHandler : public DeviceHandlerBase { uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; PoolEntry rwSpeed = PoolEntry(0, false); + PoolEntry rampTime = PoolEntry(10, false); enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS }; From ea32d87c25955912be952335696348cb003b659a Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 11:27:25 +0100 Subject: [PATCH 08/69] function now handles actuator cmd --- mission/controller/AcsController.cpp | 163 ++++++++++----------------- mission/controller/AcsController.h | 4 + 2 files changed, 66 insertions(+), 101 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index bee21d71..29bd06b6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -116,10 +116,10 @@ void AcsController::performSafe() { navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); - // Give desired satellite rate and sun direction to align + // give desired satellite rate and sun direction to align double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // IF MEKF is working + // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; if (validMekf == returnvalue::OK) { @@ -137,8 +137,8 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); { PoolReadGuard pg(&ctrlValData); @@ -180,33 +180,15 @@ void AcsController::performSafe() { actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], 0); - // } - // { - // PoolReadGuard pg(&rw1SpeedSet); - // rw1SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw2SpeedSet); - // rw2SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw3SpeedSet); - // rw3SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw4SpeedSet); - // rw4SpeedSet.setRwSpeed(0); - // } + + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + acsParameters.rwHandlingParameters.rampTime); } void AcsController::performDetumble() { @@ -223,8 +205,8 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -243,10 +225,6 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); - } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { @@ -254,34 +232,15 @@ void AcsController::performDetumble() { actuatorCmdData.rwTargetTorque.setValid(false); std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } - // { - // PoolReadGuard pg(&rw1SpeedSet); - // rw1SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw2SpeedSet); - // rw2SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw3SpeedSet); - // rw3SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw4SpeedSet); - // rw4SpeedSet.setRwSpeed(0); - // } + + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + acsParameters.rwHandlingParameters.rampTime); } void AcsController::performPointingCtrl() { @@ -304,7 +263,7 @@ void AcsController::performPointingCtrl() { guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + double mgtDpDes[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: @@ -424,60 +383,62 @@ void AcsController::performPointingCtrl() { if (enableAntiStiction) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = { - (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value}; ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } - double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); - } - int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; - for (int i = 0; i < 4; ++i) { - cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); - } + int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; + actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.setValidity(true, true); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } - // { - // PoolReadGuard pg(&rw1SpeedSet); - // rw1SpeedSet.setRwSpeed(cmdRwSpeedInt[0]); - // } - // { - // PoolReadGuard pg(&rw2SpeedSet); - // rw2SpeedSet.setRwSpeed(cmdRwSpeedInt[1]); - // } - // { - // PoolReadGuard pg(&rw3SpeedSet); - // rw3SpeedSet.setRwSpeed(cmdRwSpeedInt[2]); - // } - // { - // PoolReadGuard pg(&rw4SpeedSet); - // rw4SpeedSet.setRwSpeed(cmdRwSpeedInt[3]); - // } + + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); +} + +ReturnValue_t AcsController::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) { + { + PoolReadGuard pg(&dipoleSet); + MutexGuard mg(torquer::lazyLock()); + torquer::NEW_ACTUATION_FLAG = true; + dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration); + } + { + PoolReadGuard pg(&rw1SpeedSet); + rw1SpeedSet.setRwSpeed(rw1Speed, rampTime); + } + { + PoolReadGuard pg(&rw2SpeedSet); + rw2SpeedSet.setRwSpeed(rw2Speed, rampTime); + } + { + PoolReadGuard pg(&rw3SpeedSet); + rw3SpeedSet.setRwSpeed(rw3Speed, rampTime); + } + { + PoolReadGuard pg(&rw4SpeedSet); + rw4SpeedSet.setRwSpeed(rw4Speed, rampTime); + } + return returnvalue::OK; } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index d427bd5b..c79e4426 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -70,6 +70,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + 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); + /* ACS Sensor Values */ ACS::SensorValues sensorValues; From 109560feb263f6078fb31db12fde99d9d091ae55 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 11:30:52 +0100 Subject: [PATCH 09/69] disabled actCmd from acsCtrl again for now --- mission/controller/AcsController.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 29bd06b6..7ed66045 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -186,9 +186,9 @@ void AcsController::performSafe() { } } - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, - acsParameters.rwHandlingParameters.rampTime); + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + // acsParameters.rwHandlingParameters.rampTime); } void AcsController::performDetumble() { @@ -238,9 +238,9 @@ void AcsController::performDetumble() { } } - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, - acsParameters.rwHandlingParameters.rampTime); + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + // acsParameters.rwHandlingParameters.rampTime); } void AcsController::performPointingCtrl() { @@ -406,10 +406,10 @@ void AcsController::performPointingCtrl() { } } - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], - cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], + // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + // acsParameters.rwHandlingParameters.rampTime); } ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, From bc4c6c3a5466e66fac8a02e2f2c1245cdd2acd35 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 12:07:45 +0100 Subject: [PATCH 10/69] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index d92d6113..1bbc760d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,11 +20,13 @@ change warranting a new major release: ## Added - First version of a TCS controller heater control loop. +- Function for the ACS controller to command MTQ and RWs called by all subroutines ## Changed - Reworked dummy handling for the TCS controller. - Generator scripts now generate files for hosted and for Q7S build. +- ActCmds now returns command vectors as integers as required by the actuators # [v1.26.2] 2023-02-08 From 33684f2ef7eeaba440382d3a5a51dfcd61d5724c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 12:48:17 +0100 Subject: [PATCH 11/69] fixed imtq setDipoles check --- mission/devices/devicedefinitions/imtqHandlerDefinitions.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h index 2abf1c21..c3bc3d36 100644 --- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h @@ -492,14 +492,14 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_, uint16_t currentTorqueDurationMs_) { if (xDipole.value != xDipole_) { + xDipole = xDipole_; } - xDipole = xDipole_; if (yDipole.value != yDipole_) { + yDipole = yDipole_; } - yDipole = yDipole_; if (zDipole.value != zDipole_) { + zDipole = zDipole_; } - zDipole = zDipole_; currentTorqueDurationMs = currentTorqueDurationMs_; } From 08df102f3684b880c295a64b374793e84eef87e0 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 13:16:50 +0100 Subject: [PATCH 12/69] rwHandler rwSpeedActuationSet handling --- mission/devices/RwHandler.cpp | 62 +++++++++++++++++++++++------------ mission/devices/RwHandler.h | 12 ++++--- 2 files changed, 49 insertions(+), 25 deletions(-) diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index e9383c4a..7a8b52f3 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -13,7 +13,8 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki enableGpio(enableGpio), statusSet(this), lastResetStatusSet(this), - tmDataset(this) { + tmDataset(this), + rwSpeedActuationSet(*this) { if (comCookie == nullptr) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } @@ -97,16 +98,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand return returnvalue::OK; } case (RwDefinitions::SET_SPEED): { - if (commandDataLen != 6) { + if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; return SET_SPEED_COMMAND_INVALID_LENGTH; } - result = checkSpeedAndRampTime(commandData, commandDataLen); + // Commands override anything which was set in the software + if (commandData != nullptr) { + rwSpeedActuationSet.setValidityBufferGeneration(false); + result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + rwSpeedActuationSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } else { + // Read set rw speed value from local pool + PoolReadGuard pg(&rwSpeedActuationSet); + } + if (ACTUATION_WIRETAPPING) { + int32_t speed; + uint16_t rampTime; + rwSpeedActuationSet.getRwSpeed(speed, rampTime); + sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime + << std::endl; + } + result = checkSpeedAndRampTime(); if (result != returnvalue::OK) { return result; } - prepareSetSpeedCmd(commandData, commandDataLen); + result = prepareSetSpeedCmd(); return result; } case (RwDefinitions::GET_TEMPERATURE): { @@ -298,17 +319,15 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { rawPacketLen = 3; } -ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) { - int32_t speed = - *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3); - +ReturnValue_t RwHandler::checkSpeedAndRampTime() { + int32_t speed = 0; + uint16_t rampTime = 0; + rwSpeedActuationSet.getRwSpeed(speed, rampTime); if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) { sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl; return INVALID_SPEED; } - uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5); - if (rampTime < 10 || rampTime > 20000) { sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl; return INVALID_RAMP_TIME; @@ -317,23 +336,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_ return returnvalue::OK; } -void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) { +ReturnValue_t RwHandler::prepareSetSpeedCmd() { commandBuffer[0] = static_cast(RwDefinitions::SET_SPEED); - - /** Speed (0.1 RPM) */ - commandBuffer[1] = *(commandData + 3); - commandBuffer[2] = *(commandData + 2); - commandBuffer[3] = *(commandData + 1); - commandBuffer[4] = *commandData; - /** Ramp time (ms) */ - commandBuffer[5] = *(commandData + 5); - commandBuffer[6] = *(commandData + 4); + uint8_t* serPtr = commandBuffer + 1; + size_t serSize = 1; + rwSpeedActuationSet.setValidityBufferGeneration(false); + ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), + SerializeIF::Endianness::LITTLE); + rwSpeedActuationSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF); commandBuffer[7] = static_cast(crc & 0xFF); - commandBuffer[8] = static_cast(crc >> 8 & 0xFF); + commandBuffer[8] = static_cast((crc >> 8) & 0xFF); rawPacket = commandBuffer; rawPacketLen = 9; + return result; } void RwHandler::handleResetStatusReply(const uint8_t* packet) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 17f2b396..af59caa1 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -9,6 +9,8 @@ #include "events/subsystemIdRanges.h" #include "returnvalues/classIds.h" +static constexpr bool ACTUATION_WIRETAPPING = false; + class GpioIF; /** @@ -93,6 +95,7 @@ class RwHandler : public DeviceHandlerBase { RwDefinitions::StatusSet statusSet; RwDefinitions::LastResetSatus lastResetStatusSet; RwDefinitions::TmDataset tmDataset; + RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet; uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; @@ -117,13 +120,14 @@ class RwHandler : public DeviceHandlerBase { * range. * @return returnvalue::OK if successful, otherwise error code. */ - ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen); + ReturnValue_t checkSpeedAndRampTime(); /** - * @brief This function prepares the set speed command from the commandData received with - * an action message. + * @brief This function prepares the set speed command from the dataSet received with + * an action message or set in the software. + * @return returnvalue::OK if successful, otherwise error code. */ - void prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen); + ReturnValue_t prepareSetSpeedCmd(); /** * @brief This function writes the last reset status retrieved with the get last reset status From 285bd7116b2a8b08472e6ccfcc39dbce2362b678 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 13:17:52 +0100 Subject: [PATCH 13/69] bump changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1bbc760d..6f1896d6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ change warranting a new major release: - First version of a TCS controller heater control loop. - Function for the ACS controller to command MTQ and RWs called by all subroutines +- RwHandler now handles commanding of RW speeds via RwSpeedActuationSet ## Changed From 35172185f9da2b55ea0e85018bf3a5543579519b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 13:29:13 +0100 Subject: [PATCH 14/69] fix --- mission/controller/acs/AcsParameters.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 30264963..44bed15f 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -280,6 +280,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, break; case 0x5: parameterWrapper->set(rwHandlingParameters.rampTime); + break; default: return INVALID_IDENTIFIER_ID; } From f4f6d1ed819f69a0291092584029fb6956a1467a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Feb 2023 10:33:08 +0100 Subject: [PATCH 15/69] added InternalState SET_SPEED to enable setting RW Speed from ACS Ctrl --- mission/devices/RwHandler.cpp | 7 ++++++- mission/devices/RwHandler.h | 10 ++++++++-- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 7a8b52f3..88857b68 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -53,7 +53,11 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { break; case InternalState::GET_RW_SATUS: *id = RwDefinitions::GET_RW_STATUS; - internalState = InternalState::GET_RESET_STATUS; + internalState = InternalState::SET_SPEED; + break; + case InternalState::SET_SPEED: + *id = RwDefinitions::SET_SPEED; + internalState = InternalState::CLEAR_RESET_STATUS; break; case InternalState::CLEAR_RESET_STATUS: *id = RwDefinitions::CLEAR_LAST_RESET_STATUS; @@ -98,6 +102,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand return returnvalue::OK; } case (RwDefinitions::SET_SPEED): { + sif::debug << "hello" << std::endl; if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index af59caa1..148a6359 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -9,7 +9,7 @@ #include "events/subsystemIdRanges.h" #include "returnvalues/classIds.h" -static constexpr bool ACTUATION_WIRETAPPING = false; +static constexpr bool ACTUATION_WIRETAPPING = true; class GpioIF; @@ -102,7 +102,13 @@ class RwHandler : public DeviceHandlerBase { PoolEntry rwSpeed = PoolEntry(0, false); PoolEntry rampTime = PoolEntry(10, false); - enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS }; + enum class InternalState { + GET_RESET_STATUS, + CLEAR_RESET_STATUS, + READ_TEMPERATURE, + SET_SPEED, + GET_RW_SATUS + }; InternalState internalState = InternalState::GET_RESET_STATUS; From 53f31ca5a4df78887aaf3e22090159eb35d8509c Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Feb 2023 10:42:05 +0100 Subject: [PATCH 16/69] lol --- .../pollingSequenceFactory.cpp | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index f9b69e45..6b184f58 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -639,6 +639,129 @@ ReturnValue_t pst::pstAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) { } if (cfg.scheduleRws) { + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + } + + if (cfg.scheduleRws) { + // this is the torquing cycle + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::PERFORM_OPERATION); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_WRITE); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, From 14c43c49dca652396a161dc083cb99acd6ed1ba8 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Feb 2023 10:58:12 +0100 Subject: [PATCH 17/69] scale rwCmdSpeed to appropriate range --- mission/controller/acs/ActuatorCmd.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index cbe24e5b..b902593f 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -57,6 +57,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, deltaSpeedInt[i] = std::round(deltaSpeed[i]); } VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); + VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); } void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { From 1d20c3b472e8e1d7e617d54204465a0c41302ebe Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Feb 2023 11:04:43 +0100 Subject: [PATCH 18/69] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1dbd2f9c..57209a3f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,8 @@ change warranting a new major release: ## Changed - ActCmds now returns command vectors as integers as required by the actuators + and scales them to the appropriate range +- All RwHandler are now polled five times per ACS cycle # [v1.27.0] 2023-02-13 From 068b31a3d6bac46dd3feaac02670af10c89250de Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Feb 2023 11:07:09 +0100 Subject: [PATCH 19/69] changelog --- CHANGELOG.md | 1 - 1 file changed, 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 57209a3f..d3d64dce 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,7 +19,6 @@ change warranting a new major release: ## Added -- First version of a TCS controller heater control loop. - Function for the ACS controller to command MTQ and RWs called by all subroutines - RwHandler now handles commanding of RW speeds via RwSpeedActuationSet From 99913594fda899dbc1b8f5a374502008d2afa66c Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Feb 2023 16:10:58 +0100 Subject: [PATCH 20/69] rfrmt --- .../pollingsequence/pollingSequenceFactory.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 31606768..2944856e 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -240,7 +240,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (enableBside) { // B side thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, @@ -293,7 +292,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg } } // SUS: 16 ms - bool addSus0 = true; bool addSus1 = true; bool addSus2 = true; @@ -441,7 +439,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus6) { thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -464,7 +461,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus7) { thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -487,7 +483,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus8) { thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -510,7 +505,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus9) { thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -522,6 +516,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_WRITE); @@ -532,7 +527,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus10) { thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -555,7 +549,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); } - if (addSus11) { thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -807,6 +800,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); } + thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0); + return returnvalue::OK; } From 66d20dc11873acca86b5c004b04703ccc8c52c0b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 10:59:35 +0100 Subject: [PATCH 21/69] add tracing for first tasks --- bsp_q7s/OBSWConfig.h.in | 1 + bsp_q7s/core/CoreController.cpp | 4 ++++ bsp_q7s/core/CoreController.h | 3 +++ bsp_q7s/core/scheduling.cpp | 8 ++++---- mission/CMakeLists.txt | 2 +- mission/controller/AcsController.cpp | 4 ++++ mission/controller/AcsController.h | 5 +++++ mission/devices/BpxBatteryHandler.cpp | 8 +++++++- mission/devices/BpxBatteryHandler.h | 6 ++++++ mission/devices/SolarArrayDeploymentHandler.cpp | 6 ++++++ mission/devices/SolarArrayDeploymentHandler.h | 3 +++ mission/trace.cpp | 10 ++++++++++ mission/trace.h | 12 ++++++++++++ 13 files changed, 66 insertions(+), 6 deletions(-) create mode 100644 mission/trace.cpp create mode 100644 mission/trace.h diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 687d9363..a86dd8c7 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -77,6 +77,7 @@ // If this is enabled, all other I2C code should be disabled #define OBSW_ADD_I2C_TEST_CODE 0 #define OBSW_ADD_UART_TEST_CODE 0 +#define OBSW_THREAD_TRACING 1 #define OBSW_TEST_ACS 0 #define OBSW_DEBUG_ACS 0 diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 116b4fc9..79dc68f5 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -9,6 +9,7 @@ #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/timemanager/Stopwatch.h" #include "fsfw/version.h" +#include "mission/trace.h" #include "watchdog/definitions.h" #if OBSW_ADD_TMTC_UDP_SERVER == 1 #include "fsfw/osal/common/UdpTmTcBridge.h" @@ -63,6 +64,9 @@ ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) { } void CoreController::performControlOperation() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "CORE CTRL"); +#endif EventMessage event; for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 9cbc6340..36cf5d27 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -222,6 +222,9 @@ class CoreController : public ExtendedControllerBase { std::string currMntPrefix; bool performOneShotSdCardOpsSwitch = false; uint8_t shortSdCardCdCounter = 0; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter; +#endif Countdown sdCardCheckCd = Countdown(INIT_SD_CARD_CHECK_TIMEOUT); /** diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index ad76d308..5abe3fd5 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -374,9 +374,9 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction #else static constexpr float acsPstPeriod = 0.8; #endif - FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask( - "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); - result = pst::pstTcsAndAcs(acsPst, cfg); + FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask( + "ACS_TCS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); + result = pst::pstTcsAndAcs(acsTcsPst, cfg); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "scheduling::initTasks: ACS PST is empty" << std::endl; @@ -384,7 +384,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction sif::error << "scheduling::initTasks: Creating ACS PST failed!" << std::endl; } } else { - taskVec.push_back(acsPst); + taskVec.push_back(acsTcsPst); } /* Polling Sequence Table Default */ diff --git a/mission/CMakeLists.txt b/mission/CMakeLists.txt index f284a675..37c4a2e2 100644 --- a/mission/CMakeLists.txt +++ b/mission/CMakeLists.txt @@ -9,4 +9,4 @@ add_subdirectory(csp) add_subdirectory(cfdp) add_subdirectory(config) -target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp) +target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp trace.cpp) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ef590b8c..72589fcc 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -4,6 +4,7 @@ #include "mission/acsDefs.h" #include "mission/config/torquer.h" +#include "mission/trace.h" AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), @@ -50,6 +51,9 @@ ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, } void AcsController::performControlOperation() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "ACS & TCS PST"); +#endif switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1e017099..14972b3e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -6,6 +6,7 @@ #include #include +#include "OBSWConfig.h" #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" #include "acs/Navigation.h" @@ -51,6 +52,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ParameterHelper parameterHelper; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; diff --git a/mission/devices/BpxBatteryHandler.cpp b/mission/devices/BpxBatteryHandler.cpp index 4e49bebe..efda26f8 100644 --- a/mission/devices/BpxBatteryHandler.cpp +++ b/mission/devices/BpxBatteryHandler.cpp @@ -2,7 +2,7 @@ #include -#include "OBSWConfig.h" +#include "mission/trace.h" BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), hkSet(this), cfgSet(this) {} @@ -280,3 +280,9 @@ void BpxBatteryHandler::setToGoToNormalMode(bool enable) { } void BpxBatteryHandler::setDebugMode(bool enable) { this->debugMode = enable; } + +void BpxBatteryHandler::performOperationHook() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "BPX BATT"); +#endif +} diff --git a/mission/devices/BpxBatteryHandler.h b/mission/devices/BpxBatteryHandler.h index ceb0ff8d..31448799 100644 --- a/mission/devices/BpxBatteryHandler.h +++ b/mission/devices/BpxBatteryHandler.h @@ -3,6 +3,7 @@ #include +#include "OBSWConfig.h" #include "devicedefinitions/BpxBatteryDefinitions.h" class BpxBatteryHandler : public DeviceHandlerBase { @@ -24,6 +25,10 @@ class BpxBatteryHandler : public DeviceHandlerBase { bool debugMode = false; bool goToNormalModeImmediately = false; uint8_t sentPingByte = BpxBattery::DEFAULT_PING_SENT_BYTE; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + BpxBatteryHk hkSet; DeviceCommandId_t lastCmd = DeviceHandlerIF::NO_COMMAND_ID; BpxBatteryCfg cfgSet; @@ -47,6 +52,7 @@ class BpxBatteryHandler : public DeviceHandlerBase { ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override; void fillCommandAndReplyMap() override; + void performOperationHook() override; ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index 2b4e22ac..2eacdea4 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -37,6 +37,12 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default; ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { using namespace std::filesystem; +#if OBSW_THREAD_TRACING == 1 + opCounter++; + if (opCounter % 5 == 0) { + sif::debug << "SA DEPL task running" << std::endl; + } +#endif if (opDivider.checkAndIncrement()) { auto activeSdc = sdcMan.getActiveSdCard(); if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h index 63f1d3de..27c466f1 100644 --- a/mission/devices/SolarArrayDeploymentHandler.h +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -172,6 +172,9 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, bool firstAutonomousCycle = true; ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID; std::optional initUptime; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter; +#endif PeriodicOperationDivider opDivider = PeriodicOperationDivider(5); uint8_t retryCounter = 3; diff --git a/mission/trace.cpp b/mission/trace.cpp new file mode 100644 index 00000000..f12e5902 --- /dev/null +++ b/mission/trace.cpp @@ -0,0 +1,10 @@ +#include "trace.h" + +#include "fsfw/serviceinterface.h" + +void trace::threadTrace(uint32_t& counter, const char* name) { + counter++; + if (counter % 5 == 0) { + sif::debug << name << " running" << std::endl; + } +} diff --git a/mission/trace.h b/mission/trace.h new file mode 100644 index 00000000..a3fabbd9 --- /dev/null +++ b/mission/trace.h @@ -0,0 +1,12 @@ +#ifndef MISSION_TRACE_H_ +#define MISSION_TRACE_H_ + +#include + +namespace trace { + +void threadTrace(uint32_t& counter, const char* name); + +} + +#endif /* MISSION_TRACE_H_ */ From 2aac3c67ee7402a8a004f5231df830ccd52b3404 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 11:10:18 +0100 Subject: [PATCH 22/69] instrumented some more tasks --- bsp_q7s/OBSWConfig.h.in | 1 - bsp_q7s/core/CoreController.cpp | 1 - bsp_q7s/core/CoreController.h | 1 + linux/devices/ploc/PlocMPSoCHelper.cpp | 4 +++- linux/devices/ploc/PlocMPSoCHelper.h | 5 +++++ linux/devices/ploc/PlocSupvUartMan.cpp | 3 +++ linux/devices/ploc/PlocSupvUartMan.h | 5 +++++ mission/controller/AcsController.cpp | 1 - mission/controller/AcsController.h | 2 +- mission/devices/BpxBatteryHandler.cpp | 2 -- mission/devices/BpxBatteryHandler.h | 2 +- mission/devices/SolarArrayDeploymentHandler.cpp | 1 + mission/devices/SolarArrayDeploymentHandler.h | 3 ++- mission/trace.h | 2 ++ 14 files changed, 24 insertions(+), 9 deletions(-) diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index a86dd8c7..687d9363 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -77,7 +77,6 @@ // If this is enabled, all other I2C code should be disabled #define OBSW_ADD_I2C_TEST_CODE 0 #define OBSW_ADD_UART_TEST_CODE 0 -#define OBSW_THREAD_TRACING 1 #define OBSW_TEST_ACS 0 #define OBSW_DEBUG_ACS 0 diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 79dc68f5..4ff4965f 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -9,7 +9,6 @@ #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/timemanager/Stopwatch.h" #include "fsfw/version.h" -#include "mission/trace.h" #include "watchdog/definitions.h" #if OBSW_ADD_TMTC_UDP_SERVER == 1 #include "fsfw/osal/common/UdpTmTcBridge.h" diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 36cf5d27..65ee20ef 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -12,6 +12,7 @@ #include "events/subsystemIdRanges.h" #include "fsfw/controller/ExtendedControllerBase.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" +#include "mission/trace.h" class Timer; class SdCardManager; diff --git a/linux/devices/ploc/PlocMPSoCHelper.cpp b/linux/devices/ploc/PlocMPSoCHelper.cpp index fcd5178b..fca2e51c 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.cpp +++ b/linux/devices/ploc/PlocMPSoCHelper.cpp @@ -3,7 +3,6 @@ #include #include -#include "OBSWConfig.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/FilesystemHelper.h" #endif @@ -34,6 +33,9 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { ReturnValue_t result = returnvalue::OK; semaphore.acquire(); while (true) { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "PLOC MPSOC Helper"); +#endif switch (internalState) { case InternalState::IDLE: { semaphore.acquire(); diff --git a/linux/devices/ploc/PlocMPSoCHelper.h b/linux/devices/ploc/PlocMPSoCHelper.h index c39cc758..dea35a82 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.h +++ b/linux/devices/ploc/PlocMPSoCHelper.h @@ -11,6 +11,7 @@ #include "fsfw/tmtcservices/SourceSequenceCounter.h" #include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" +#include "mission/trace.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" #endif @@ -116,6 +117,10 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { struct FlashWrite flashWrite; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + enum class InternalState { IDLE, FLASH_WRITE, FLASH_READ }; InternalState internalState = InternalState::IDLE; diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 1fd2b841..5113de40 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -101,6 +101,9 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { lock->unlockMutex(); semaphore->acquire(); putTaskToSleep = false; +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "PLOC SUPV Helper PST"); +#endif while (true) { if (putTaskToSleep) { performUartShutdown(); diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index e1539c97..b787815f 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -7,6 +7,7 @@ #include #include "OBSWConfig.h" +#include "mission/trace.h" #include "fsfw/container/FIFO.h" #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" @@ -15,6 +16,7 @@ #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" +#include "mission/trace.h" #include "tas/crc.h" #ifdef XIPHOS_Q7S @@ -211,6 +213,9 @@ class PlocSupvUartManager : public DeviceCommunicationIF, supv::TmBase tmReader; int serialPort = 0; struct termios tty = {}; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif struct EventBufferRequest { std::string path = ""; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 72589fcc..051fa8f0 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -4,7 +4,6 @@ #include "mission/acsDefs.h" #include "mission/config/torquer.h" -#include "mission/trace.h" AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 14972b3e..f5240b74 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -6,7 +6,6 @@ #include #include -#include "OBSWConfig.h" #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" #include "acs/Navigation.h" @@ -20,6 +19,7 @@ #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" +#include "mission/trace.h" class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: diff --git a/mission/devices/BpxBatteryHandler.cpp b/mission/devices/BpxBatteryHandler.cpp index efda26f8..922bcd3b 100644 --- a/mission/devices/BpxBatteryHandler.cpp +++ b/mission/devices/BpxBatteryHandler.cpp @@ -2,8 +2,6 @@ #include -#include "mission/trace.h" - BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie) : DeviceHandlerBase(objectId, comIF, comCookie), hkSet(this), cfgSet(this) {} diff --git a/mission/devices/BpxBatteryHandler.h b/mission/devices/BpxBatteryHandler.h index 31448799..c42ed7fd 100644 --- a/mission/devices/BpxBatteryHandler.h +++ b/mission/devices/BpxBatteryHandler.h @@ -3,8 +3,8 @@ #include -#include "OBSWConfig.h" #include "devicedefinitions/BpxBatteryDefinitions.h" +#include "mission/trace.h" class BpxBatteryHandler : public DeviceHandlerBase { public: diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index 2eacdea4..cb8658bc 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -11,6 +11,7 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw_hal/common/gpio/GpioCookie.h" +#include "mission/trace.h" static constexpr bool DEBUG_MODE = true; diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h index 27c466f1..61211ca3 100644 --- a/mission/devices/SolarArrayDeploymentHandler.h +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -19,6 +19,7 @@ #include "fsfw/timemanager/Countdown.h" #include "fsfw_hal/common/gpio/GpioIF.h" #include "mission/memory/SdCardMountedIF.h" +#include "mission/trace.h" #include "returnvalues/classIds.h" enum DeploymentChannels : uint8_t { SA_1 = 1, SA_2 = 2 }; @@ -173,7 +174,7 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID; std::optional initUptime; #if OBSW_THREAD_TRACING == 1 - uint32_t opCounter; + uint32_t opCounter = 0; #endif PeriodicOperationDivider opDivider = PeriodicOperationDivider(5); uint8_t retryCounter = 3; diff --git a/mission/trace.h b/mission/trace.h index a3fabbd9..6f20a698 100644 --- a/mission/trace.h +++ b/mission/trace.h @@ -3,6 +3,8 @@ #include +#define OBSW_THREAD_TRACING 1 + namespace trace { void threadTrace(uint32_t& counter, const char* name); From a57384f6c43a20281884cde5cd984af71ed89b0f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 11:11:06 +0100 Subject: [PATCH 23/69] trace has settable div --- mission/trace.cpp | 4 ++-- mission/trace.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/trace.cpp b/mission/trace.cpp index f12e5902..6c9af0af 100644 --- a/mission/trace.cpp +++ b/mission/trace.cpp @@ -2,9 +2,9 @@ #include "fsfw/serviceinterface.h" -void trace::threadTrace(uint32_t& counter, const char* name) { +void trace::threadTrace(uint32_t& counter, const char* name, unsigned div) { counter++; - if (counter % 5 == 0) { + if (counter % div == 0) { sif::debug << name << " running" << std::endl; } } diff --git a/mission/trace.h b/mission/trace.h index 6f20a698..59fdd99e 100644 --- a/mission/trace.h +++ b/mission/trace.h @@ -7,7 +7,7 @@ namespace trace { -void threadTrace(uint32_t& counter, const char* name); +void threadTrace(uint32_t& counter, const char* name, unsigned div = 5); } From 9b2398888dfd11a8e4e5093492067b7bbca81d5b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 11:18:51 +0100 Subject: [PATCH 24/69] add some more traces --- linux/devices/GpsHyperionLinuxController.cpp | 3 +++ linux/devices/GpsHyperionLinuxController.h | 4 ++++ linux/devices/ploc/PlocSupvUartMan.h | 1 - mission/controller/ThermalController.cpp | 3 +++ mission/controller/ThermalController.h | 7 ++++++- 5 files changed, 16 insertions(+), 2 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index afc76fcf..a6e0a1a3 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -102,6 +102,9 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) { handleQueue(); poolManager.performHkOperation(); while (true) { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "GPS CTRL"); +#endif bool callAgainImmediately = readGpsDataFromGpsd(); if (not callAgainImmediately) { handleQueue(); diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index 5d4a35ff..1e97bc29 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -6,6 +6,7 @@ #include "fsfw/controller/ExtendedControllerBase.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" +#include "mission/trace.h" #ifdef FSFW_OSAL_LINUX #include @@ -60,6 +61,9 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); bool modeCommanded = false; bool timeInit = false; +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif struct OneShotSwitches { void reset() { diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index b787815f..02bfb6c7 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -7,7 +7,6 @@ #include #include "OBSWConfig.h" -#include "mission/trace.h" #include "fsfw/container/FIFO.h" #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 0521d9aa..53b7f13d 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -72,6 +72,9 @@ ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) { } void ThermalController::performControlOperation() { +#if OBSW_THREAD_TRACING == 1 + trace::threadTrace(opCounter, "TCS Task"); +#endif switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); diff --git a/mission/controller/ThermalController.h b/mission/controller/ThermalController.h index 03c7954c..6297b175 100644 --- a/mission/controller/ThermalController.h +++ b/mission/controller/ThermalController.h @@ -11,7 +11,8 @@ #include -#include "../devices/HeaterHandler.h" +#include "mission/devices/HeaterHandler.h" +#include "mission/trace.h" /** * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit @@ -152,6 +153,10 @@ class ThermalController : public ExtendedControllerBase { // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(DELAY); +#if OBSW_THREAD_TRACING == 1 + uint32_t opCounter = 0; +#endif + std::array, 5> sensors; uint8_t numSensors = 0; From ecb22bdd85655d93e673646fe108a666daaee690 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 11:32:03 +0100 Subject: [PATCH 25/69] further cut down the number of threads --- bsp_q7s/core/scheduling.cpp | 56 ++++++++++++------------------- linux/scheduling.cpp | 66 ++++++++++++++++++------------------- linux/scheduling.h | 4 +-- 3 files changed, 56 insertions(+), 70 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 5abe3fd5..2fa8f015 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -154,6 +154,14 @@ void scheduling::initTasks() { if (result != returnvalue::OK) { scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); } + result = genericSysTask->addComponent(objects::INTERNAL_ERROR_REPORTER); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); + } + result = genericSysTask->addComponent(objects::PUS_SERVICE_17_TEST); + if (result != returnvalue::OK) { + scheduling::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); + } #if OBSW_ADD_CCSDS_IP_CORES == 1 result = genericSysTask->addComponent(objects::CCSDS_HANDLER); @@ -266,14 +274,16 @@ void scheduling::initTasks() { #endif /* OBSW_ADD_PLOC_SUPERVISOR */ PeriodicTaskIF* plTask = factory->createPeriodicTask( - "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); - scheduling::addMpsocSupvHandlers(plTask); + "PL_TASK", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); plTask->addComponent(objects::CAM_SWITCHER); + scheduling::addMpsocSupvHandlers(plTask); +#if OBSW_ADD_SCEX_DEVICE == 1 + scheduling::scheduleScexDev(plTask); +#endif #if OBSW_ADD_SCEX_DEVICE == 1 - PeriodicTaskIF* scexDevHandler; PeriodicTaskIF* scexReaderTask; - scheduling::schedulingScex(*factory, scexDevHandler, scexReaderTask); + scheduling::scheduleScexReader(*factory, scexReaderTask); #endif std::vector pusTasks; @@ -330,7 +340,6 @@ void scheduling::initTasks() { taskStarter(pstTasks, "PST task vector"); taskStarter(pusTasks, "PUS task vector"); #if OBSW_ADD_SCEX_DEVICE == 1 - scexDevHandler->startTask(); scexReaderTask->startTask(); #endif @@ -435,42 +444,28 @@ void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction std::vector& taskVec) { ReturnValue_t result = returnvalue::OK; /* PUS Services */ - PeriodicTaskIF* pusVerification = factory.createPeriodicTask( - "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); - result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); + PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( + "PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); + result = pusHighPrio->addComponent(objects::PUS_SERVICE_1_VERIFICATION); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION); } - taskVec.push_back(pusVerification); - - PeriodicTaskIF* pusEvents = factory.createPeriodicTask( - "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); - result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); + result = pusHighPrio->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } - result = pusEvents->addComponent(objects::EVENT_MANAGER); + result = pusHighPrio->addComponent(objects::EVENT_MANAGER); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } - taskVec.push_back(pusEvents); - - PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( - "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); - result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); - } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT); } - taskVec.push_back(pusHighPrio); PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); - result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_3", objects::PUS_SERVICE_3_HOUSEKEEPING); @@ -495,20 +490,11 @@ void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction if (result != returnvalue::OK) { scheduling::printAddObjectError("PUS_201", objects::PUS_SERVICE_201_HEALTH); } - // Used for connection tests, therefore use higher priority - result = pusMedPrio->addComponent(objects::PUS_SERVICE_17_TEST); + result = pusMedPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); if (result != returnvalue::OK) { - scheduling::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); + scheduling::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } taskVec.push_back(pusMedPrio); - - PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( - "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); - result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); - } - taskVec.push_back(pusLowPrio); } void scheduling::createTestTasks(TaskFactory& factory, diff --git a/linux/scheduling.cpp b/linux/scheduling.cpp index f2e6ddec..85394dea 100644 --- a/linux/scheduling.cpp +++ b/linux/scheduling.cpp @@ -8,8 +8,7 @@ #include "ObjectFactory.h" #include "eive/objects.h" -void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, - PeriodicTaskIF*& scexReaderTask) { +void scheduling::scheduleScexReader(TaskFactory& factory, PeriodicTaskIF*& scexReaderTask) { using namespace scheduling; ReturnValue_t result = returnvalue::OK; #if OBSW_PRINT_MISSED_DEADLINES == 1 @@ -17,37 +16,6 @@ void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHa #else void (*missedDeadlineFunc)(void) = nullptr; #endif - scexDevHandler = factory.createPeriodicTask( - "SCEX_DEV", 35, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc); - - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::PERFORM_OPERATION); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_WRITE); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_WRITE); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } - result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); - if (result != returnvalue::OK) { - printAddObjectError("SCEX_DEV", objects::SCEX); - } result = returnvalue::OK; scexReaderTask = factory.createPeriodicTask( @@ -79,3 +47,35 @@ void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) { plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); #endif } + +void scheduling::scheduleScexDev(PeriodicTaskIF*& scexDevHandler) { + ReturnValue_t result = + scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::PERFORM_OPERATION); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_WRITE); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_WRITE); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } +} diff --git a/linux/scheduling.h b/linux/scheduling.h index d33e9d1f..b5ec8ef2 100644 --- a/linux/scheduling.h +++ b/linux/scheduling.h @@ -3,7 +3,7 @@ #include namespace scheduling { -void schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, - PeriodicTaskIF*& scexReaderTask); +void scheduleScexDev(PeriodicTaskIF*& scexDevHandler); +void scheduleScexReader(TaskFactory& factory, PeriodicTaskIF*& scexReaderTask); void addMpsocSupvHandlers(PeriodicTaskIF* task); } // namespace scheduling From 2d4c881d3ab84914603494f6ad4d6df71e7713b9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 11:56:40 +0100 Subject: [PATCH 26/69] add missing include --- linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 63b40df2..062bf7e7 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -1,5 +1,7 @@ #include "pollingSequenceFactory.h" +#include "OBSWConfig.h" + #include #include #include From 1de832509a7d6383abe509145d2107346dbcf4dd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 13:40:07 +0100 Subject: [PATCH 27/69] do a commit --- mission/devices/ImtqHandler.cpp | 4 ++++ mission/devices/RwHandler.cpp | 5 ++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 0e75c06d..b370b7d9 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -192,6 +192,10 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma if (result != returnvalue::OK) { return result; } + result = dipoleSet.commit(); + if (result != returnvalue::OK) { + sif::error << "ImtqHandler::buildCommandFromCommand: commit failed" << std::endl; + } } else { // Read set dipole values from local pool PoolReadGuard pg(&dipoleSet); diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 88857b68..5c6914be 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -102,7 +102,6 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand return returnvalue::OK; } case (RwDefinitions::SET_SPEED): { - sif::debug << "hello" << std::endl; if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; @@ -117,6 +116,10 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand if (result != returnvalue::OK) { return result; } + result = rwSpeedActuationSet.commit(); + if (result != returnvalue::OK) { + sif::error << "RwHandler::buildCommandFromCommand: commit failed" << std::endl; + } } else { // Read set rw speed value from local pool PoolReadGuard pg(&rwSpeedActuationSet); From 650f24d0ffbeb868d76c20b226524602fa4ada9c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Feb 2023 13:54:06 +0100 Subject: [PATCH 28/69] convert Igrf13 model vector from nT to uT --- mission/controller/acs/Igrf13Model.cpp | 9 +++------ mission/controller/acs/Igrf13Model.h | 1 - 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 63992010..3f844434 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -20,6 +20,7 @@ Igrf13Model::~Igrf13Model() {} void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement, double* magFieldModelInertial) { + double magFieldModel[3] = {0, 0, 0}; double phi = longitude, theta = gcLatitude; // geocentric /* Here is the co-latitude needed*/ theta -= 90 * PI / 180; @@ -100,12 +101,8 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModelInertial[2] = magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude); - double normVecMagFieldInert[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); - - magFieldModel[0] = 0; - magFieldModel[1] = 0; - magFieldModel[2] = 0; + // convert nT to uT + VectorOperations::mulScalar(magFieldModelInertial, 1e-3, magFieldModelInertial, 3); } void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index ea2c1044..187adde7 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -47,7 +47,6 @@ class Igrf13Model /*:public HasParametersIF*/ { // Coefficient wary over year, could be updated sometimes. void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) - double magFieldModel[3]; void schmidtNormalization(); private: From 6d428859f2a7fc78c45bf09e24f657633208e5ba Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Feb 2023 13:56:30 +0100 Subject: [PATCH 29/69] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f94066d3..b772640d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ change warranting a new major release: - Remove 2 TCS threads. - Move low level polling into ACS PST, move high level device handlers into TCS system task. +- Igrf13 model vector now outputs as uT instead of nT # [v1.27.1] 2023-02-13 From e07492c85515126257dbeccdf6a7eaf230a301cf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:10:37 +0100 Subject: [PATCH 30/69] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 9b7471e9..20e107c7 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 9b7471e9097edd410995ba0c76125b626440d9be +Subproject commit 20e107c7ae373e7f36fca68957dbc36f4dd76f3b From bf70e8ece45b29eafbb7a92ad9fe975c695afab1 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Feb 2023 14:11:45 +0100 Subject: [PATCH 31/69] changed acsPst parts again to give more time to the sensors --- common/config/eive/definitions.h | 15 +- .../pollingSequenceFactory.cpp | 132 +++++++++--------- 2 files changed, 75 insertions(+), 72 deletions(-) diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index c572e21a..9e67176a 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -57,14 +57,17 @@ static constexpr uint32_t MAX_STORED_CMDS_TCP = 150; namespace acs { -static constexpr uint32_t SCHED_BLOCK_1_SENSORS_MS = 15; -static constexpr uint32_t SCHED_BLOCK_2_ACS_CTRL_MS = 40; -static constexpr uint32_t SCHED_BLOCK_3_ACTUATOR_MS = 45; +static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15; +static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; +static constexpr uint32_t SCHED_BLOCK_3_ACS_CTRL_MS = 45; +static constexpr uint32_t SCHED_BLOCK_4_ACTUATOR_MS = 50; // 15 ms for FM -static constexpr float SCHED_BLOCK_1_PERIOD = static_cast(SCHED_BLOCK_1_SENSORS_MS) / 400.0; -static constexpr float SCHED_BLOCK_2_PERIOD = static_cast(SCHED_BLOCK_2_ACS_CTRL_MS) / 400.0; -static constexpr float SCHED_BLOCK_3_PERIOD = static_cast(SCHED_BLOCK_3_ACTUATOR_MS) / 400.0; +static constexpr float SCHED_BLOCK_1_PERIOD = static_cast(SCHED_BLOCK_1_SUS_READ_MS) / 400.0; +static constexpr float SCHED_BLOCK_2_PERIOD = + static_cast(SCHED_BLOCK_2_SENSOR_READ_MS) / 400.0; +static constexpr float SCHED_BLOCK_3_PERIOD = static_cast(SCHED_BLOCK_3_ACS_CTRL_MS) / 400.0; +static constexpr float SCHED_BLOCK_4_PERIOD = static_cast(SCHED_BLOCK_4_ACTUATOR_MS) / 400.0; } // namespace acs diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 63b40df2..b91fcd7a 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -192,103 +192,103 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg if (cfg.scheduleAcsBoard) { if (enableAside) { // A side - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); } if (enableBside) { // B side - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, - length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); + length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ); } } @@ -622,66 +622,66 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg DeviceHandlerIF::GET_READ); } - thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_2_PERIOD, 0); + thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_3_PERIOD, 0); if (cfg.scheduleImtq) { // This is the torquing cycle. - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_READ); } if (cfg.scheduleRws) { - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, + thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD, DeviceHandlerIF::GET_READ); } thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0); From 35b9c7a4df590f28e8ed05c18378c74dd120c475 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:27:25 +0100 Subject: [PATCH 32/69] bump changelog --- CHANGELOG.md | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f49f4ed4..d01f4290 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,8 +19,17 @@ change warranting a new major release: ## Changed -- Remove 2 TCS threads. -- Move low level polling into ACS PST, move high level device handlers into TCS system task. +- Remove 2 TCS threads. Move low level polling into ACS PST, move high level device handlers into + TCS system task. +- Further reduce number of threads: + 1. Remove PUS low priority task, move assigned threads to the generic system task + 2. Group events and verification tasks into PUS high priority task + 3. Group all other components into PUS medium priority task + 4. Add SCEX device handler to PL task, remove dedicated thread + +## Added + +- Tracing supports which allows checking whether threads are running as usual. ## Removed From 024e06a3d33cf85303a62083fb027effef155ff7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:28:26 +0100 Subject: [PATCH 33/69] set define to 0 for PR --- mission/trace.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/trace.h b/mission/trace.h index 59fdd99e..fbc99d37 100644 --- a/mission/trace.h +++ b/mission/trace.h @@ -3,7 +3,7 @@ #include -#define OBSW_THREAD_TRACING 1 +#define OBSW_THREAD_TRACING 0 namespace trace { From 6f84099c5e5e537a312d6221101f892fc8ef83aa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:29:47 +0100 Subject: [PATCH 34/69] small update and afmt --- CMakeLists.txt | 4 ++-- linux/boardtest/UartTestClass.h | 2 +- linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp | 3 +-- mission/devices/SolarArrayDeploymentHandler.cpp | 5 +---- test/TestTask.cpp | 3 --- 5 files changed, 5 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ce19479..8a8055bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -490,8 +490,8 @@ endif() # ############################################################################## # Add libraries -target_link_libraries(${LIB_EIVE_MISSION} - PUBLIC ${LIB_FSFW_NAME} ${LIB_OS_NAME}) +target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_FSFW_NAME} + ${LIB_OS_NAME}) target_link_libraries(${LIB_DUMMIES} PUBLIC ${LIB_FSFW_NAME} ${LIB_JSON_NAME}) diff --git a/linux/boardtest/UartTestClass.h b/linux/boardtest/UartTestClass.h index 6cb0d31c..fd20e621 100644 --- a/linux/boardtest/UartTestClass.h +++ b/linux/boardtest/UartTestClass.h @@ -59,7 +59,7 @@ class UartTestClass : public TestTask { DleEncoder dleEncoder = DleEncoder(); SerialCookie* uartCookie = nullptr; size_t encodedLen = 0; - //lwgps_t gpsData = {}; + // lwgps_t gpsData = {}; struct termios tty = {}; int serialPort = 0; bool startFound = false; diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 062bf7e7..d8df8e9a 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -1,12 +1,11 @@ #include "pollingSequenceFactory.h" -#include "OBSWConfig.h" - #include #include #include #include +#include "OBSWConfig.h" #include "eive/definitions.h" #include "mission/devices/devicedefinitions/Max31865Definitions.h" diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index cb8658bc..77c826fd 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -39,10 +39,7 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default; ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { using namespace std::filesystem; #if OBSW_THREAD_TRACING == 1 - opCounter++; - if (opCounter % 5 == 0) { - sif::debug << "SA DEPL task running" << std::endl; - } + trace::threadTrace(opCounter, "SA DEPL"); #endif if (opDivider.checkAndIncrement()) { auto activeSdc = sdcMan.getActiveSdCard(); diff --git a/test/TestTask.cpp b/test/TestTask.cpp index 33af5494..2c6cb015 100644 --- a/test/TestTask.cpp +++ b/test/TestTask.cpp @@ -42,10 +42,8 @@ ReturnValue_t EiveTestTask::performOperation(uint8_t operationCode) { #include - // #include - /** * @brief Dummy data from GPS receiver. Will be replaced witgh hyperion data later. */ @@ -76,7 +74,6 @@ const char hyperion_gps_data[] = "$GNVTG,040.7,T,,M,000.0,N,000.0,K,A*10\r\n" "$GNZDA,173225.998892,27,02,2021,00,00*75\r\n"; - ReturnValue_t EiveTestTask::performOneShotAction() { #if OBSW_ADD_TEST_CODE == 1 // performLwgpsTest(); From a26924fa0461d10f48cf436ba8b4eb987abb5b31 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Feb 2023 14:32:54 +0100 Subject: [PATCH 35/69] frmt --- mission/controller/acs/ActuatorCmd.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 4e0052e0..01befd93 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -1,10 +1,3 @@ -/* - * ActuatorCmd.cpp - * - * Created on: 4 Aug 2022 - * Author: Robin Marquardt - */ - #include "ActuatorCmd.h" #include From d506b515fc63476304e2d7934096549ecce2fa13 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:33:12 +0100 Subject: [PATCH 36/69] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index d256ede8..9de6c4b3 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d256ede8c1d8e7a746d3a56d45313d2b863e0b28 +Subproject commit 9de6c4b3aa20ee63c28051d486be8a12df147f22 From 918cd7b237be07ca6ca3a3568eac58cb2eb6377d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:38:33 +0100 Subject: [PATCH 37/69] small tweak for scripts, bump fsfw --- fsfw | 2 +- scripts/q7s-em-udp-forwarding.sh | 3 +-- scripts/q7s-fm-udp-forwarding.sh | 3 +-- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/fsfw b/fsfw index d256ede8..9de6c4b3 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d256ede8c1d8e7a746d3a56d45313d2b863e0b28 +Subproject commit 9de6c4b3aa20ee63c28051d486be8a12df147f22 diff --git a/scripts/q7s-em-udp-forwarding.sh b/scripts/q7s-em-udp-forwarding.sh index 22830efe..284e7889 100755 --- a/scripts/q7s-em-udp-forwarding.sh +++ b/scripts/q7s-em-udp-forwarding.sh @@ -4,5 +4,4 @@ echo "Q7S UDP connection from local port 18000 -> TCP ssh tunnel -> EM port 7301 socat udp4-listen:18000,reuseaddr,fork tcp:localhost:18002 & ssh -L 18002:localhost:18123 eive@flatsat.eive.absatvirt.lw \ - 'CONSOLE_PREFIX="[Q7S EM UDP Tunnel]" \ - /bin/bash && socat tcp4-listen:18123,reuseaddr udp:192.168.133.10:7301' + 'socat tcp4-listen:18123,reuseaddr udp:192.168.133.10:7301' diff --git a/scripts/q7s-fm-udp-forwarding.sh b/scripts/q7s-fm-udp-forwarding.sh index 6bb2b3f2..34c8421f 100755 --- a/scripts/q7s-fm-udp-forwarding.sh +++ b/scripts/q7s-fm-udp-forwarding.sh @@ -4,5 +4,4 @@ echo "Q7S UDP connection from local port 18000 -> TCP ssh tunnel -> FM port 7301 socat udp4-listen:18000,reuseaddr,fork tcp:localhost:18002 & ssh -L 18002:localhost:18123 eive@flatsat.eive.absatvirt.lw \ - 'CONSOLE_PREFIX="[Q7S FM UDP Tunnel]" \ - /bin/bash && socat tcp4-listen:18123,reuseaddr udp:192.168.155.55:7301' + 'socat tcp4-listen:18123,reuseaddr udp:192.168.155.55:7301' From 11e06f6b21b92d3e994980496ea97f147e5fb0d0 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 14 Feb 2023 17:14:14 +0100 Subject: [PATCH 38/69] acsParam fix --- mission/controller/acs/AcsParameters.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f1c0fb63..60efe01f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -779,9 +779,9 @@ class AcsParameters : public HasParametersIF { /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ - float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms - pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms - pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms + float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms + pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms + pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = true; } gyrHandlingParameters; From 166fd77f7b6a17486d9fc43813183ab63e76cb88 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 18:41:43 +0100 Subject: [PATCH 39/69] stupid RW --- bsp_q7s/core/ObjectFactory.cpp | 4 +- .../pollingSequenceFactory.cpp | 148 +++++++++--------- mission/devices/ImtqHandler.cpp | 26 ++- mission/devices/RwHandler.cpp | 48 +++--- mission/devices/RwHandler.h | 9 +- 5 files changed, 116 insertions(+), 119 deletions(-) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2bf36743..2631a628 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -683,8 +683,8 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback::spiCallback, nullptr); auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, - rwGpioIds[idx]); - rwCookies[idx]->setCallbackArgs(rws[idx]); + rwGpioIds[idx], idx); + rwCookies[idx]->setCallbackArgs(rwHandler); #if OBSW_TEST_RW == 1 rws[idx]->setStartUpImmediately(); #endif diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index e404cdf7..7e4cafeb 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -633,80 +633,80 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg } if (cfg.scheduleRws) { - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ); + // + // thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ); + // thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ); } if (cfg.scheduleRws) { diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index b370b7d9..93e9dae8 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -183,22 +183,20 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } ReturnValue_t result; - // Commands override anything which was set in the software - if (commandData != nullptr) { - dipoleSet.setValidityBufferGeneration(false); - result = - dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK); - dipoleSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - result = dipoleSet.commit(); - if (result != returnvalue::OK) { - sif::error << "ImtqHandler::buildCommandFromCommand: commit failed" << std::endl; - } - } else { + { // Read set dipole values from local pool PoolReadGuard pg(&dipoleSet); + + // Commands override anything which was set in the software + if (commandData != nullptr) { + dipoleSet.setValidityBufferGeneration(false); + result = dipoleSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + dipoleSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } } if (ACTUATION_WIRETAPPING) { sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 5c6914be..9d4a7255 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -7,14 +7,15 @@ #include "OBSWConfig.h" RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, - GpioIF* gpioComIF, gpioId_t enableGpio) + GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx) : DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio), statusSet(this), lastResetStatusSet(this), tmDataset(this), - rwSpeedActuationSet(*this) { + rwSpeedActuationSet(*this), + rwIdx(rwIdx) { if (comCookie == nullptr) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } @@ -43,6 +44,10 @@ void RwHandler::doShutDown() { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { + case InternalState::SET_SPEED: + *id = RwDefinitions::SET_SPEED; + internalState = InternalState::GET_RESET_STATUS; + break; case InternalState::GET_RESET_STATUS: *id = RwDefinitions::GET_LAST_RESET_STATUS; internalState = InternalState::READ_TEMPERATURE; @@ -53,10 +58,6 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { break; case InternalState::GET_RW_SATUS: *id = RwDefinitions::GET_RW_STATUS; - internalState = InternalState::SET_SPEED; - break; - case InternalState::SET_SPEED: - *id = RwDefinitions::SET_SPEED; internalState = InternalState::CLEAR_RESET_STATUS; break; case InternalState::CLEAR_RESET_STATUS: @@ -107,29 +108,26 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand << " invalid length" << std::endl; return SET_SPEED_COMMAND_INVALID_LENGTH; } - // Commands override anything which was set in the software - if (commandData != nullptr) { - rwSpeedActuationSet.setValidityBufferGeneration(false); - result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, - SerializeIF::Endianness::NETWORK); - rwSpeedActuationSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - result = rwSpeedActuationSet.commit(); - if (result != returnvalue::OK) { - sif::error << "RwHandler::buildCommandFromCommand: commit failed" << std::endl; - } - } else { - // Read set rw speed value from local pool + + { PoolReadGuard pg(&rwSpeedActuationSet); + // Commands override anything which was set in the software + if (commandData != nullptr) { + rwSpeedActuationSet.setValidityBufferGeneration(false); + result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::NETWORK); + rwSpeedActuationSet.setValidityBufferGeneration(true); + if (result != returnvalue::OK) { + return result; + } + } } if (ACTUATION_WIRETAPPING) { - int32_t speed; - uint16_t rampTime; + int32_t speed = 0; + uint16_t rampTime = 0; rwSpeedActuationSet.getRwSpeed(speed, rampTime); - sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime - << std::endl; + sif::debug << "Actuating RW " << static_cast(rwIdx) << " with speed = " << speed + << " and rampTime = " << rampTime << std::endl; } result = checkSpeedAndRampTime(); if (result != returnvalue::OK) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 148a6359..ee18960d 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -9,7 +9,7 @@ #include "events/subsystemIdRanges.h" #include "returnvalues/classIds.h" -static constexpr bool ACTUATION_WIRETAPPING = true; +static constexpr bool ACTUATION_WIRETAPPING = false; class GpioIF; @@ -36,7 +36,7 @@ class RwHandler : public DeviceHandlerBase { * to high to enable the device. */ RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF, - gpioId_t enableGpio); + gpioId_t enableGpio, uint8_t rwIdx); void setDebugMode(bool enable); @@ -98,9 +98,10 @@ class RwHandler : public DeviceHandlerBase { RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet; uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; + uint8_t rwIdx; - PoolEntry rwSpeed = PoolEntry(0, false); - PoolEntry rampTime = PoolEntry(10, false); + PoolEntry rwSpeed = PoolEntry({0}); + PoolEntry rampTime = PoolEntry({10}); enum class InternalState { GET_RESET_STATUS, From 177b573cd4ae4c47724167607f8df472834b9746 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 18:54:43 +0100 Subject: [PATCH 40/69] prep v1.27.2 --- CHANGELOG.md | 7 +++++++ CMakeLists.txt | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 417f6522..a8b2ef0c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,13 @@ change warranting a new major release: # [unreleased] +# [v1.27.2] 2023-02-14 + +Reaction Wheel handling was determined to be (quasi) broken and needs to be fixed in future release +to be usable by ACS controller. + +eive-tmtc: v2.15.6 + ## Added - Function for the ACS controller to command MTQ and RWs called by all subroutines diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a8055bd..4f62c199 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 27) -set(OBSW_VERSION_REVISION 1) +set(OBSW_VERSION_REVISION 2) # set(CMAKE_VERBOSE TRUE) From 7c68de26d28bad54d0a86b1eb633092eef270bf5 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Feb 2023 11:42:00 +0100 Subject: [PATCH 41/69] added actual variances for MGM sensor fusion --- mission/controller/acs/AcsParameters.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index b237a0f2..f68b10cf 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -73,9 +73,9 @@ class AcsParameters : public HasParametersIF { {-0.007534, 1.253879, 0.006812}, {-0.037072, 0.006812, 1.313158}}; - float mgm02variance[3] = {1, 1, 1}; - float mgm13variance[3] = {1, 1, 1}; - float mgm4variance[3] = {1, 1, 1}; + float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; + float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; + float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; } mgmHandlingParameters; struct SusHandlingParameters { From f784d4f2480b192e3cd2eb736790ea89f8cf72f7 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Feb 2023 15:15:30 +0100 Subject: [PATCH 42/69] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ecbb7302..9c1319d3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,11 @@ change warranting a new major release: ## Changed - Igrf13 model vector now outputs as uT instead of nT +- Changed timings for `AcsPst` +- Added values for MGM sensor fusion + +## Fixed +- Fixed values for GYR sensor fusion # [v1.27.2] 2023-02-14 From 8b0eceb072a4edbcc45b1abb5ca6532286987461 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 17:02:22 +0100 Subject: [PATCH 43/69] continue rw refactoring --- bsp_q7s/callbacks/rwSpiCallback.cpp | 24 +- bsp_q7s/core/ObjectFactory.cpp | 8 +- dummies/RwDummy.cpp | 64 +-- linux/devices/CMakeLists.txt | 2 +- linux/devices/RwPollingTask.cpp | 401 ++++++++++++++++++ linux/devices/RwPollingTask.h | 78 ++++ mission/controller/AcsController.h | 10 +- mission/controller/ThermalController.cpp | 10 +- mission/controller/acs/SensorValues.h | 11 +- mission/devices/RwHandler.cpp | 184 ++++---- mission/devices/RwHandler.h | 30 +- .../devices/devicedefinitions/rwHelpers.cpp | 6 + .../{RwDefinitions.h => rwHelpers.h} | 123 ++++-- 13 files changed, 739 insertions(+), 212 deletions(-) create mode 100644 linux/devices/RwPollingTask.cpp create mode 100644 linux/devices/RwPollingTask.h create mode 100644 mission/devices/devicedefinitions/rwHelpers.cpp rename mission/devices/devicedefinitions/{RwDefinitions.h => rwHelpers.h} (67%) diff --git a/bsp_q7s/callbacks/rwSpiCallback.cpp b/bsp_q7s/callbacks/rwSpiCallback.cpp index 74ede602..7f71a004 100644 --- a/bsp_q7s/callbacks/rwSpiCallback.cpp +++ b/bsp_q7s/callbacks/rwSpiCallback.cpp @@ -76,7 +76,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_WRITE_FAILURE; + return rws::SPI_WRITE_FAILURE; } /** Encoding and sending command */ @@ -101,7 +101,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_WRITE_FAILURE; + return rws::SPI_WRITE_FAILURE; } idx++; } @@ -113,7 +113,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_WRITE_FAILURE; + return rws::SPI_WRITE_FAILURE; } uint8_t* rxBuf = nullptr; @@ -128,7 +128,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen // There must be a delay of at least 20 ms after sending the command. // Delay for 70 ms here and release the SPI bus for that duration. closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - usleep(RwDefinitions::SPI_REPLY_DELAY); + usleep(rws::SPI_REPLY_DELAY); result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); if (result != returnvalue::OK) { return result; @@ -143,13 +143,13 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_READ_FAILURE; + return rws::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != FLAG_BYTE) { sif::error << "Invalid data, expected start marker" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::NO_START_MARKER; + return rws::NO_START_MARKER; } } @@ -160,7 +160,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (idx == 9) { sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::NO_REPLY; + return rws::NO_REPLY; } } @@ -175,7 +175,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen byteRead = 0; if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = RwHandler::SPI_READ_FAILURE; + result = rws::SPI_READ_FAILURE; break; } } @@ -186,7 +186,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen } else if (byteRead == 0x7D) { if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = RwHandler::SPI_READ_FAILURE; + result = rws::SPI_READ_FAILURE; break; } if (byteRead == 0x5E) { @@ -200,7 +200,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen } else { sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - result = RwHandler::INVALID_SUBSTITUTE; + result = rws::INVALID_SUBSTITUTE; break; } } else { @@ -217,14 +217,14 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (decodedFrameLen == replyBufferSize) { if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; - result = RwHandler::SPI_READ_FAILURE; + result = rws::SPI_READ_FAILURE; break; } if (byteRead != FLAG_BYTE) { sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) << std::endl; decodedFrameLen--; - result = RwHandler::MISSING_END_SIGN; + result = rws::MISSING_END_SIGN; break; } } diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2631a628..1f920f6d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include @@ -94,7 +95,6 @@ #include "mission/devices/devicedefinitions/GomspaceDefinitions.h" #include "mission/devices/devicedefinitions/Max31865Definitions.h" #include "mission/devices/devicedefinitions/RadSensorDefinitions.h" -#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SyrlinksDefinitions.h" #include "mission/devices/devicedefinitions/payloadPcduDefinitions.h" #include "mission/system/objects/AcsBoardAssembly.h" @@ -679,9 +679,9 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, gpioIds::EN_RW4}; std::array rws = {}; for (uint8_t idx = 0; idx < rwCookies.size(); idx++) { - rwCookies[idx] = new SpiCookie(rwCookieParams[idx].first, rwCookieParams[idx].second, - RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, - &rwSpiCallback::spiCallback, nullptr); + rwCookies[idx] = + new SpiCookie(rwCookieParams[idx].first, rwCookieParams[idx].second, rws::MAX_REPLY_SIZE, + spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback::spiCallback, nullptr); auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, rwGpioIds[idx], idx); rwCookies[idx]->setCallbackArgs(rwHandler); diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index a21e7ab7..54e7ac83 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -1,6 +1,6 @@ #include "RwDummy.h" -#include +#include RwDummy::RwDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} @@ -37,39 +37,39 @@ uint32_t RwDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 4864e01f..7251b802 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -4,7 +4,7 @@ endif() target_sources( ${OBSW_NAME} PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp - ScexDleParser.cpp ScexHelper.cpp) + ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) add_subdirectory(ploc) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp new file mode 100644 index 00000000..ca663c07 --- /dev/null +++ b/linux/devices/RwPollingTask.cpp @@ -0,0 +1,401 @@ +#include "RwPollingTask.h" + +#include +#include +#include +#include +#include +#include + +#include "devConf.h" +#include "mission/devices/devicedefinitions/rwHelpers.h" + +RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF) + : SystemObject(objectId), spiIF(spiIF) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + while (true) { + semaphore->acquire(); + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + prepareSetSpeedCmd(idx); + writeOneRw(idx); + } + readAllRws(fd, spiLock, dev) + // writeAndReadAllRws(sendData, sendDataLen) + int bytesRead = 0; + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; } + +ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { + spiIF->getSpiDev(); + // We are in protected section, so we can use the static variable here without issues. + // We don't need to set the speed because a SPI core is used, but the mode has to be set once + // correctly for all RWs + if (not modeAndSpeedWasSet) { + auto& dev = spiIF->getSpiDev(); + int fd = open(dev.c_str(), O_RDWR); + if (fd < 0) { + sif::error << "could not open RW SPI bus" << std::endl; + return returnvalue::FAILED; + } + spiIF->setSpiSpeedAndMode(fd, spi::RW_MODE, spi::RW_SPEED); + close(fd); + modeAndSpeedWasSet = true; + } + + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + if (sendLen < 6) { + return DeviceHandlerIF::INVALID_DATA; + } + int32_t speed = 0; + uint16_t rampTime = 0; + SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); + SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; + if (sendLen == 7 and sendData[6] < rws::SpecialRwRequest::NUM_REQUESTS) { + specialRequest = static_cast(sendData[6]); + } + RwCookie* rwCookie = dynamic_cast(cookie); + { + MutexGuard mg(ipcLock); + rwCookie->currentRwSpeed = speed; + rwCookie->currentRampTime = rampTime; + rwCookie->specialRequest = specialRequest; + if (state == InternalState::IDLE and rwCookie->rwIdx == 3) { + semaphore->release(); + } + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { + RwCookie* rwCookie = dynamic_cast(cookie); + { + MutexGuard mg(ipcLock); + *buffer = rwCookie->replyBuf.data(); + *size = rwCookie->replyBuf.size(); + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen) { + // Stopwatch watch; + ReturnValue_t result = returnvalue::OK; + + int fd = 0; + const std::string& dev = spiIF->getSpiDev(); + MutexIF* spiLock = spiIF->getCsMutex(); + result = openSpi(dev, O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx], spiLock, sendData, sendDataLen); + if (result != returnvalue::OK) { + closeSpi(fd); + return returnvalue::FAILED; + } + } + + closeSpi(fd); + usleep(rws::SPI_REPLY_DELAY); + return readAllRws(fd, spiLock, dev.c_str()); +} + +ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& fd) { + fd = open(devname.c_str(), flags); + if (fd < 0) { + sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl; + return SpiComIF::OPENING_FILE_FAILED; + } + + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, + uint8_t* replyBuf) { + ReturnValue_t result = returnvalue::OK; + int fd = 0; + gpioId_t gpioId = rwCookie.getChipSelectPin(); + GpioIF& gpioIF = spiIF->getGpioInterface(); + pullCsLow(gpioId, spiLock, gpioIF); + for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { + result = openSpi(spiDev, O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + /** + * The reaction wheel responds with empty frames while preparing the reply data. + * However, receiving more than 5 empty frames will be interpreted as an error. + */ + uint8_t byteRead = 0; + for (int idx = 0; idx < 5; idx++) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + closeSpi(fd); + return rws::SPI_READ_FAILURE; + } + if (idx == 0) { + if (byteRead != FLAG_BYTE) { + sif::error << "Invalid data, expected start marker" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + closeSpi(fd); + return rws::NO_START_MARKER; + } + } + + if (byteRead != FLAG_BYTE) { + break; + } + + pullCsHigh(gpioId, spiLock, gpioIF); + closeSpi(fd); + if (idx == MAX_RETRIES_REPLY - 1) { + sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; + return rws::NO_REPLY; + } + TaskFactory::delayTask(5); + } + +#if FSFW_HAL_SPI_WIRETAPPING == 1 + sif::info << "RW start marker detected" << std::endl; +#endif + + size_t decodedFrameLen = 0; + + while (decodedFrameLen < processingBuf.size()) { + /** First byte already read in */ + if (decodedFrameLen != 0) { + byteRead = 0; + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + } + + if (byteRead == FLAG_BYTE) { + /** Reached end of frame */ + break; + } else if (byteRead == 0x7D) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead == 0x5E) { + *(processingBuf.data() + decodedFrameLen) = 0x7E; + decodedFrameLen++; + continue; + } else if (byteRead == 0x5D) { + *(processingBuf.data() + decodedFrameLen) = 0x7D; + decodedFrameLen++; + continue; + } else { + sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; + closeSpi(fd); + result = rws::INVALID_SUBSTITUTE; + break; + } + } else { + *(processingBuf.data() + decodedFrameLen) = byteRead; + decodedFrameLen++; + continue; + } + + /** + * There might be the unlikely case that each byte in a get-telemetry reply has been + * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. + * Otherwise there might be something wrong. + */ + if (decodedFrameLen == processingBuf.size()) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead != FLAG_BYTE) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " + << static_cast(FLAG_BYTE) << std::endl; + decodedFrameLen--; + result = rws::MISSING_END_SIGN; + break; + } + } + result = returnvalue::OK; + } + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) { + int fd = 0; + const std::string& dev = spiIF->getSpiDev(); + MutexIF* spiLock = spiIF->getCsMutex(); + ReturnValue_t result = openSpi(dev, O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + ReturnValue_t result = + sendOneMessage(fd, *rwCookies[rwIdx], spiLock, writeBuffer.data(), writeLen); + if (result != returnvalue::OK) { + closeSpi(fd); + return returnvalue::FAILED; + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::readAllRws(int fd, MutexIF* spiLock, const char* dev) { + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + if (spiLock == nullptr) { + sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; + return returnvalue::FAILED; + } + uint8_t* replyBuf; + readNextReply(dev, *rwCookies[idx], spiLock, replyBuf); + } + + closeSpi(fd); + return returnvalue::OK; +} + +// This closes the SPI +void RwPollingTask::closeSpi(int fd) { + // This will perform the function to close the SPI + close(fd); + // The SPI is now closed. +} + +ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, + const uint8_t* data, size_t dataLen) { + gpioId_t gpioId = rwCookie.getChipSelectPin(); + GpioIF& gpioIF = spiIF->getGpioInterface(); + if (spiLock == nullptr) { + sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; + return returnvalue::FAILED; + } + pullCsLow(gpioId, spiLock, gpioIF); + /** Sending frame start sign */ + writeBuffer[0] = FLAG_BYTE; + size_t writeSize = 1; + if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + return rws::SPI_WRITE_FAILURE; + } + /** Encoding and sending command */ + size_t idx = 0; + while (idx < dataLen) { + switch (*(data + idx)) { + case 0x7E: + writeBuffer[0] = 0x7D; + writeBuffer[1] = 0x5E; + writeSize = 2; + break; + case 0x7D: + writeBuffer[0] = 0x7D; + writeBuffer[1] = 0x5D; + writeSize = 2; + break; + default: + writeBuffer[0] = *(data + idx); + writeSize = 1; + break; + } + } + if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + return rws::SPI_WRITE_FAILURE; + } + idx++; + /** Sending frame end sign */ + writeBuffer[0] = FLAG_BYTE; + writeSize = 1; + + if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + return rws::SPI_WRITE_FAILURE; + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { + ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS); + if (result != returnvalue::OK) { + sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl; + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + ReturnValue_t result = gpioIF.pullLow(gpioId); + if (result != returnvalue::OK) { + sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl; + return result; + } + } + return returnvalue::OK; +} + +void RwPollingTask::pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { + if (gpioId != gpio::NO_GPIO) { + if (gpioIF.pullHigh(gpioId) != returnvalue::OK) { + sif::error << "closeSpi: Failed to pull chip select high" << std::endl; + } + } + if (spiLock->unlockMutex() != returnvalue::OK) { + sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl; + ; + } +} + +void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) { + writeBuffer[0] = static_cast(id); + uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF); + writeBuffer[1] = static_cast(crc & 0xFF); + writeBuffer[2] = static_cast(crc >> 8 & 0xFF); +} + +ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { + writeBuffer[0] = static_cast(rws::SET_SPEED); + uint8_t* serPtr = writeBuffer.data() + 1; + int32_t speedToSet = 0; + uint16_t rampTimeToSet = 10; + { + MutexGuard mg(ipcLock); + speedToSet = rwCookies[rwIdx]->currentRwSpeed; + rampTimeToSet = rwCookies[rwIdx]->currentRampTime; + } + size_t serLen = 0; + SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(), + SerializeIF::Endianness::LITTLE); + SerializeAdapter::serialize(&rampTimeToSet, &serPtr, &serLen, writeBuffer.size(), + SerializeIF::Endianness::LITTLE); + + uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF); + writeBuffer[7] = static_cast(crc & 0xFF); + writeBuffer[8] = static_cast((crc >> 8) & 0xFF); + return returnvalue::OK; +} diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h new file mode 100644 index 00000000..d649a66a --- /dev/null +++ b/linux/devices/RwPollingTask.h @@ -0,0 +1,78 @@ +#ifndef LINUX_DEVICES_RWPOLLINGTASK_H_ +#define LINUX_DEVICES_RWPOLLINGTASK_H_ + +#include +#include +#include +#include +#include +#include + +#include "mission/devices/devicedefinitions/rwHelpers.h" + +class RwCookie : public SpiCookie { + friend class RwPollingTask; + + public: + RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, + spi::SpiModes spiMode, uint32_t spiSpeed) + : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {} + + private: + std::array replyBuf{}; + int32_t currentRwSpeed = 0; + uint16_t currentRampTime = 0; + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::NONE; + uint8_t rwIdx; +}; + +class RwPollingTask : public SystemObject, public ExecutableObjectIF, public DeviceCommunicationIF { + public: + RwPollingTask(object_id_t objectId, SpiComIF* spiIF); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + SemaphoreIF* semaphore; + bool debugMode = false; + bool modeAndSpeedWasSet = false; + MutexIF* ipcLock; + SpiComIF* spiIF; + std::array rwCookies; + std::array writeBuffer; + size_t writeLen = 0; + std::array processingBuf; + //! This is the end and start marker of the frame datalinklayer + static constexpr uint8_t FLAG_BYTE = 0x7E; + static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t TIMEOUT_MS = 20; + static constexpr uint8_t MAX_RETRIES_REPLY = 5; + + ReturnValue_t writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen); + ReturnValue_t writeOneRw(uint8_t rwIdx); + ReturnValue_t readAllRws(int fd, MutexIF* spiLock, const char* dev); + ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, const uint8_t* data, + size_t dataLen); + ReturnValue_t readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, + uint8_t* replyBuf); + ReturnValue_t initializeInterface(CookieIF* cookie) override; + + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + ReturnValue_t openSpi(const std::string& devname, int flags, int& fd); + ReturnValue_t pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void prepareSimpleCommand(DeviceCommandId_t id); + ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx); + + void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void closeSpi(int); +}; + +#endif /* LINUX_DEVICES_RWPOLLINGTASK_H_ */ diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c0e719f2..34fa9c33 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -5,6 +5,7 @@ #include #include #include +#include #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" @@ -17,7 +18,6 @@ #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" #include "mission/trace.h" @@ -84,10 +84,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes /* ACS Actuation Datasets */ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); - RwDefinitions::RwSpeedActuationSet rw1SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW1); - RwDefinitions::RwSpeedActuationSet rw2SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW2); - RwDefinitions::RwSpeedActuationSet rw3SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW3); - RwDefinitions::RwSpeedActuationSet rw4SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW4); + rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1); + rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2); + rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3); + rws::RwSpeedActuationSet rw4SpeedSet = rws::RwSpeedActuationSet(objects::RW4); /* ACS Datasets */ // MGMs acsctrl::MgmDataRaw mgmDataRaw; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 53b7f13d..f4e46c69 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -9,10 +9,10 @@ #include #include #include -#include #include #include #include +#include #include ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater) @@ -689,7 +689,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw1 = lp_var_t(objects::RW1, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw1 = lp_var_t(objects::RW1, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl; @@ -702,7 +702,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw2 = lp_var_t(objects::RW2, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw2 = lp_var_t(objects::RW2, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl; @@ -715,7 +715,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw3 = lp_var_t(objects::RW3, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw3 = lp_var_t(objects::RW3, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl; @@ -728,7 +728,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw4 = lp_var_t(objects::RW4, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw4 = lp_var_t(objects::RW4, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index ec1795fc..92d4c5ff 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,13 +1,14 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include + #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" -#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" @@ -62,10 +63,10 @@ class SensorValues { // bool mgt0valid; - RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1); - RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2); - RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3); - RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4); + rws::StatusSet rw1Set = rws::StatusSet(objects::RW1); + rws::StatusSet rw2Set = rws::StatusSet(objects::RW2); + rws::StatusSet rw3Set = rws::StatusSet(objects::RW3); + rws::StatusSet rw4Set = rws::StatusSet(objects::RW4); }; } /* namespace ACS */ diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 9d4a7255..c5bd1e6e 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -45,23 +45,23 @@ void RwHandler::doShutDown() { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { case InternalState::SET_SPEED: - *id = RwDefinitions::SET_SPEED; + *id = rws::SET_SPEED; internalState = InternalState::GET_RESET_STATUS; break; case InternalState::GET_RESET_STATUS: - *id = RwDefinitions::GET_LAST_RESET_STATUS; + *id = rws::GET_LAST_RESET_STATUS; internalState = InternalState::READ_TEMPERATURE; break; case InternalState::READ_TEMPERATURE: - *id = RwDefinitions::GET_TEMPERATURE; + *id = rws::GET_TEMPERATURE; internalState = InternalState::GET_RW_SATUS; break; case InternalState::GET_RW_SATUS: - *id = RwDefinitions::GET_RW_STATUS; + *id = rws::GET_RW_STATUS; internalState = InternalState::CLEAR_RESET_STATUS; break; case InternalState::CLEAR_RESET_STATUS: - *id = RwDefinitions::CLEAR_LAST_RESET_STATUS; + *id = rws::CLEAR_LAST_RESET_STATUS; /** After reset status is cleared, reset status will be polled again for verification */ internalState = InternalState::GET_RESET_STATUS; break; @@ -82,27 +82,27 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { - case (RwDefinitions::RESET_MCU): { + case (rws::RESET_MCU): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::GET_LAST_RESET_STATUS): { + case (rws::GET_LAST_RESET_STATUS): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::CLEAR_LAST_RESET_STATUS): { + case (rws::CLEAR_LAST_RESET_STATUS): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::GET_RW_STATUS): { + case (rws::GET_RW_STATUS): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::INIT_RW_CONTROLLER): { + case (rws::INIT_RW_CONTROLLER): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::SET_SPEED): { + case (rws::SET_SPEED): { if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; @@ -136,11 +136,11 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand result = prepareSetSpeedCmd(); return result; } - case (RwDefinitions::GET_TEMPERATURE): { + case (rws::GET_TEMPERATURE): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::GET_TM): { + case (rws::GET_TM): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } @@ -151,60 +151,56 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand } void RwHandler::fillCommandAndReplyMap() { - this->insertInCommandMap(RwDefinitions::RESET_MCU); - this->insertInCommandAndReplyMap(RwDefinitions::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet, - RwDefinitions::SIZE_GET_RESET_STATUS); - this->insertInCommandAndReplyMap(RwDefinitions::CLEAR_LAST_RESET_STATUS, 1, nullptr, - RwDefinitions::SIZE_CLEAR_RESET_STATUS); - this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet, - RwDefinitions::SIZE_GET_RW_STATUS); - this->insertInCommandAndReplyMap(RwDefinitions::INIT_RW_CONTROLLER, 1, nullptr, - RwDefinitions::SIZE_INIT_RW); - this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, nullptr, - RwDefinitions::SIZE_GET_TEMPERATURE_REPLY); - this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr, - RwDefinitions::SIZE_SET_SPEED_REPLY); - this->insertInCommandAndReplyMap(RwDefinitions::GET_TM, 1, &tmDataset, - RwDefinitions::SIZE_GET_TELEMETRY_REPLY); + this->insertInCommandMap(rws::RESET_MCU); + this->insertInCommandAndReplyMap(rws::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet, + rws::SIZE_GET_RESET_STATUS); + this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr, + rws::SIZE_CLEAR_RESET_STATUS); + this->insertInCommandAndReplyMap(rws::GET_RW_STATUS, 1, &statusSet, rws::SIZE_GET_RW_STATUS); + this->insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 1, nullptr, rws::SIZE_INIT_RW); + this->insertInCommandAndReplyMap(rws::GET_TEMPERATURE, 1, nullptr, + rws::SIZE_GET_TEMPERATURE_REPLY); + this->insertInCommandAndReplyMap(rws::SET_SPEED, 1, nullptr, rws::SIZE_SET_SPEED_REPLY); + this->insertInCommandAndReplyMap(rws::GET_TM, 1, &tmDataset, rws::SIZE_GET_TELEMETRY_REPLY); } ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { uint8_t replyByte = *start; switch (replyByte) { - case (RwDefinitions::GET_LAST_RESET_STATUS): { - *foundLen = RwDefinitions::SIZE_GET_RESET_STATUS; - *foundId = RwDefinitions::GET_LAST_RESET_STATUS; + case (rws::GET_LAST_RESET_STATUS): { + *foundLen = rws::SIZE_GET_RESET_STATUS; + *foundId = rws::GET_LAST_RESET_STATUS; break; } - case (RwDefinitions::CLEAR_LAST_RESET_STATUS): { - *foundLen = RwDefinitions::SIZE_CLEAR_RESET_STATUS; - *foundId = RwDefinitions::CLEAR_LAST_RESET_STATUS; + case (rws::CLEAR_LAST_RESET_STATUS): { + *foundLen = rws::SIZE_CLEAR_RESET_STATUS; + *foundId = rws::CLEAR_LAST_RESET_STATUS; break; } - case (RwDefinitions::GET_RW_STATUS): { - *foundLen = RwDefinitions::SIZE_GET_RW_STATUS; - *foundId = RwDefinitions::GET_RW_STATUS; + case (rws::GET_RW_STATUS): { + *foundLen = rws::SIZE_GET_RW_STATUS; + *foundId = rws::GET_RW_STATUS; break; } - case (RwDefinitions::INIT_RW_CONTROLLER): { - *foundLen = RwDefinitions::SIZE_INIT_RW; - *foundId = RwDefinitions::INIT_RW_CONTROLLER; + case (rws::INIT_RW_CONTROLLER): { + *foundLen = rws::SIZE_INIT_RW; + *foundId = rws::INIT_RW_CONTROLLER; break; } - case (RwDefinitions::SET_SPEED): { - *foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY; - *foundId = RwDefinitions::SET_SPEED; + case (rws::SET_SPEED): { + *foundLen = rws::SIZE_SET_SPEED_REPLY; + *foundId = rws::SET_SPEED; break; } - case (RwDefinitions::GET_TEMPERATURE): { - *foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY; - *foundId = RwDefinitions::GET_TEMPERATURE; + case (rws::GET_TEMPERATURE): { + *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY; + *foundId = rws::GET_TEMPERATURE; break; } - case (RwDefinitions::GET_TM): { - *foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY; - *foundId = RwDefinitions::GET_TM; + case (rws::GET_TM): { + *foundLen = rws::SIZE_GET_TELEMETRY_REPLY; + *foundId = rws::GET_TM; break; } default: { @@ -221,7 +217,7 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { /** Check result code */ - if (*(packet + 1) == RwDefinitions::STATE_ERROR) { + if (*(packet + 1) == rws::STATE_ERROR) { sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id << std::endl; return EXECUTION_FAILED; @@ -236,24 +232,24 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ } switch (id) { - case (RwDefinitions::GET_LAST_RESET_STATUS): { + case (rws::GET_LAST_RESET_STATUS): { handleResetStatusReply(packet); break; } - case (RwDefinitions::GET_RW_STATUS): { + case (rws::GET_RW_STATUS): { handleGetRwStatusReply(packet); break; } - case (RwDefinitions::CLEAR_LAST_RESET_STATUS): - case (RwDefinitions::INIT_RW_CONTROLLER): - case (RwDefinitions::SET_SPEED): + case (rws::CLEAR_LAST_RESET_STATUS): + case (rws::INIT_RW_CONTROLLER): + case (rws::SET_SPEED): // no reply data expected break; - case (RwDefinitions::GET_TEMPERATURE): { + case (rws::GET_TEMPERATURE): { handleTemperatureReply(packet); break; } - case (RwDefinitions::GET_TM): { + case (rws::GET_TM): { handleGetTelemetryReply(packet); break; } @@ -270,43 +266,43 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed); - localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime); + localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime); - localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 5.0)); poolManager.subscribeForRegularPeriodicPacket( @@ -343,7 +339,7 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime() { } ReturnValue_t RwHandler::prepareSetSpeedCmd() { - commandBuffer[0] = static_cast(RwDefinitions::SET_SPEED); + commandBuffer[0] = static_cast(rws::SET_SPEED); uint8_t* serPtr = commandBuffer + 1; size_t serSize = 1; rwSpeedActuationSet.setValidityBufferGeneration(false); @@ -369,7 +365,7 @@ void RwHandler::handleResetStatusReply(const uint8_t* packet) { if (resetStatus != 0) { internalState = InternalState::CLEAR_RESET_STATUS; lastResetStatusSet.lastNonClearedResetStatus = resetStatus; - triggerEvent(RwDefinitions::RESET_OCCURED, resetStatus, 0); + triggerEvent(rws::RESET_OCCURED, resetStatus, 0); } lastResetStatusSet.currentResetStatus = resetStatus; if (debugMode) { @@ -408,10 +404,10 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { statusSet.setValidity(true, true); - if (statusSet.state == RwDefinitions::STATE_ERROR) { + if (statusSet.state == rws::STATE_ERROR) { // This requires the commanding of the init reaction wheel controller command to recover // from error state which must be handled by the FDIR instance. - triggerEvent(RwDefinitions::ERROR_STATE, statusSet.state.value, 0); + triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0); sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl; } diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index ee18960d..7db3ef3b 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include "events/subsystemIdRanges.h" @@ -42,24 +42,6 @@ class RwHandler : public DeviceHandlerBase { virtual ~RwHandler(); - static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; - - static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); - //! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call - static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1); - //! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing - //! start sign 0x7E - static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2); - //! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid - //! substitution combination - static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3); - //! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E - static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4); - //! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames. - static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5); - //! [EXPORT] : [COMMENT] Expected a start marker as first byte - static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6); - protected: void doStartUp() override; void doShutDown() override; @@ -92,12 +74,12 @@ class RwHandler : public DeviceHandlerBase { gpioId_t enableGpio = gpio::NO_GPIO; bool debugMode = false; - RwDefinitions::StatusSet statusSet; - RwDefinitions::LastResetSatus lastResetStatusSet; - RwDefinitions::TmDataset tmDataset; - RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet; + rws::StatusSet statusSet; + rws::LastResetSatus lastResetStatusSet; + rws::TmDataset tmDataset; + rws::RwSpeedActuationSet rwSpeedActuationSet; - uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; + uint8_t commandBuffer[rws::MAX_CMD_SIZE]; uint8_t rwIdx; PoolEntry rwSpeed = PoolEntry({0}); diff --git a/mission/devices/devicedefinitions/rwHelpers.cpp b/mission/devices/devicedefinitions/rwHelpers.cpp new file mode 100644 index 00000000..4c246eb8 --- /dev/null +++ b/mission/devices/devicedefinitions/rwHelpers.cpp @@ -0,0 +1,6 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ + +#include "rwHelpers.h" + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */ diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/rwHelpers.h similarity index 67% rename from mission/devices/devicedefinitions/RwDefinitions.h rename to mission/devices/devicedefinitions/rwHelpers.h index b3cb0fe4..743813fa 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/rwHelpers.h @@ -1,5 +1,5 @@ -#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ -#define MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ #include #include @@ -8,7 +8,83 @@ #include "events/subsystemIdRanges.h" #include "objects/systemObjectList.h" -namespace RwDefinitions { +namespace rws { + +static const size_t SIZE_GET_RESET_STATUS = 5; +static const size_t SIZE_CLEAR_RESET_STATUS = 4; +static const size_t SIZE_INIT_RW = 4; +static const size_t SIZE_GET_RW_STATUS = 14; +static const size_t SIZE_SET_SPEED_REPLY = 4; +static const size_t SIZE_GET_TEMPERATURE_REPLY = 8; +/** Max size when requesting telemetry */ +static const size_t SIZE_GET_TELEMETRY_REPLY = 91; + +enum SpecialRwRequest : uint8_t { + REQUEST_NONE = 0, + RESET_MCU = 1, + INIT_RW_CONTROLLER = 2, + GET_TM = 3, + NUM_REQUESTS +}; + +struct RwReplies { + friend class RwPollingTask; + + public: + RwReplies(const uint8_t* rawData) : rawData(rawData) { + rwStatusReply = rawData; + setSpeedReply = rawData + SIZE_GET_RW_STATUS; + getLastResetStatusReply = setSpeedReply + SIZE_SET_SPEED_REPLY; + clearLastResetStatusReply = getLastResetStatusReply + SIZE_GET_RESET_STATUS; + readTemperatureReply = clearLastResetStatusReply + SIZE_CLEAR_RESET_STATUS; + hkDataReply = readTemperatureReply + SIZE_GET_TEMPERATURE_REPLY; + initRwControllerReply = hkDataReply + SIZE_GET_TELEMETRY_REPLY; + } + + const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; } + + const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; } + + const uint8_t* getHkDataReply() const { return hkDataReply; } + + const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } + + const uint8_t* getRawData() const { return rawData; } + + const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } + + const uint8_t* getRwStatusReply() const { return rwStatusReply; } + + const uint8_t* getSetSpeedReply() const { return setSpeedReply; } + + private: + const uint8_t* rawData; + const uint8_t* rwStatusReply; + const uint8_t* setSpeedReply; + const uint8_t* getLastResetStatusReply; + const uint8_t* clearLastResetStatusReply; + const uint8_t* readTemperatureReply; + const uint8_t* hkDataReply; + const uint8_t* initRwControllerReply; +}; + +static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; + +static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); +//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call +static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1); +//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing +//! start sign 0x7E +static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2); +//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid +//! substitution combination +static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3); +//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E +static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4); +//! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames. +static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5); +//! [EXPORT] : [COMMENT] Expected a start marker as first byte +static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER; @@ -17,7 +93,8 @@ static constexpr Event ERROR_STATE = MAKE_EVENT(1, severity::HIGH); static constexpr Event RESET_OCCURED = event::makeEvent(SUBSYSTEM_ID, 2, severity::LOW); -static const uint32_t SPI_REPLY_DELAY = 70000; // us +//! Minimal delay as specified by the datasheet. +static const uint32_t SPI_REPLY_DELAY = 20000; // us enum PoolIds : lp_id_t { TEMPERATURE_C, @@ -86,15 +163,6 @@ enum SetIds : uint32_t { SPEED_CMD_SET = 10, }; -static const size_t SIZE_GET_RESET_STATUS = 5; -static const size_t SIZE_CLEAR_RESET_STATUS = 4; -static const size_t SIZE_INIT_RW = 4; -static const size_t SIZE_GET_RW_STATUS = 14; -static const size_t SIZE_SET_SPEED_REPLY = 4; -static const size_t SIZE_GET_TEMPERATURE_REPLY = 8; -/** Max size when requesting telemetry */ -static const size_t SIZE_GET_TELEMETRY_REPLY = 91; - /** Set speed command has maximum size */ static const size_t MAX_CMD_SIZE = 9; /** @@ -112,11 +180,10 @@ static const uint8_t TM_SET_ENTRIES = 24; */ class StatusSet : public StaticLocalDataSet { public: - StatusSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, RwDefinitions::SetIds::STATUS_SET_ID) {} + StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, rws::SetIds::STATUS_SET_ID) {} StatusSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::STATUS_SET_ID)) {} + : StaticLocalDataSet(sid_t(objectId, rws::SetIds::STATUS_SET_ID)) {} lp_var_t temperatureCelcius = lp_var_t(sid.objectId, PoolIds::TEMPERATURE_C, this); @@ -133,10 +200,10 @@ class StatusSet : public StaticLocalDataSet { class LastResetSatus : public StaticLocalDataSet { public: LastResetSatus(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, RwDefinitions::SetIds::LAST_RESET_ID) {} + : StaticLocalDataSet(owner, rws::SetIds::LAST_RESET_ID) {} LastResetSatus(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::LAST_RESET_ID)) {} + : StaticLocalDataSet(sid_t(objectId, rws::SetIds::LAST_RESET_ID)) {} // If a reset occurs, the status code will be cached into this variable lp_var_t lastNonClearedResetStatus = @@ -153,11 +220,9 @@ class LastResetSatus : public StaticLocalDataSet { */ class TmDataset : public StaticLocalDataSet { public: - TmDataset(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, RwDefinitions::SetIds::TM_SET_ID) {} + TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, rws::SetIds::TM_SET_ID) {} - TmDataset(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::TM_SET_ID)) {} + TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, rws::SetIds::TM_SET_ID)) {} lp_var_t lastResetStatus = lp_var_t(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this); @@ -209,9 +274,9 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { public: RwSpeedActuationSet(HasLocalDataPoolIF& owner) - : StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {} + : StaticLocalDataSet(&owner, rws::SetIds::SPEED_CMD_SET) {} RwSpeedActuationSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {} + : StaticLocalDataSet(sid_t(objectId, rws::SetIds::SPEED_CMD_SET)) {} void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) { if (rwSpeed.value != rwSpeed_) { @@ -228,12 +293,10 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { } private: - lp_var_t rwSpeed = - lp_var_t(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this); - lp_var_t rampTime = - lp_var_t(sid.objectId, RwDefinitions::PoolIds::RAMP_TIME, this); + lp_var_t rwSpeed = lp_var_t(sid.objectId, rws::PoolIds::RW_SPEED, this); + lp_var_t rampTime = lp_var_t(sid.objectId, rws::PoolIds::RAMP_TIME, this); }; -} // namespace RwDefinitions +} // namespace rws -#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */ +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ */ From 8c001b64433d18c47b2b53b7de200224eb8a4d7e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:16:42 +0100 Subject: [PATCH 44/69] heading towards completion for low level rw polling task --- linux/devices/RwPollingTask.cpp | 205 ++++++++++++------ linux/devices/RwPollingTask.h | 28 ++- mission/devices/devicedefinitions/rwHelpers.h | 89 ++++---- 3 files changed, 205 insertions(+), 117 deletions(-) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index ca663c07..657f8853 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -15,35 +15,66 @@ RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF) semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); semaphore->acquire(); ipcLock = MutexFactory::instance()->createMutex(); + spiLock = spiIF->getCsMutex(); + spiDev = spiIF->getSpiDev().c_str(); } ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { - ipcLock->lockMutex(); - state = InternalState::IDLE; - ipcLock->unlockMutex(); while (true) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); semaphore->acquire(); + int fd = 0; + ReturnValue_t result = openSpi(O_RDWR, fd); + if (result != returnvalue::OK) { + continue; + } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { prepareSetSpeedCmd(idx); - writeOneRw(idx); + if (writeOneRwCmd(idx, fd) != returnvalue::OK) { + continue; + } } - readAllRws(fd, spiLock, dev) - // writeAndReadAllRws(sendData, sendDataLen) - int bytesRead = 0; + closeSpi(fd); + usleep(rws::SPI_REPLY_DELAY); + if (readAllRws(fd, rws::SET_SPEED) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::GET_LAST_RESET_STATUS); + if (writeAndReadAllRws(rws::GET_LAST_RESET_STATUS) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::GET_RW_STATUS); + if (writeAndReadAllRws(rws::GET_RW_STATUS) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::GET_TEMPERATURE); + if (writeAndReadAllRws(rws::GET_TEMPERATURE) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::CLEAR_LAST_RESET_STATUS); + if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) { + continue; + } + } + + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::initialize() { + if (spiDev == nullptr) { + sif::error << "SPI device is invalid" << std::endl; + return returnvalue::FAILED; } return returnvalue::OK; } -ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; } - ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { - spiIF->getSpiDev(); - // We are in protected section, so we can use the static variable here without issues. // We don't need to set the speed because a SPI core is used, but the mode has to be set once // correctly for all RWs if (not modeAndSpeedWasSet) { - auto& dev = spiIF->getSpiDev(); - int fd = open(dev.c_str(), O_RDWR); + int fd = open(spiDev, O_RDWR); if (fd < 0) { sif::error << "could not open RW SPI bus" << std::endl; return returnvalue::FAILED; @@ -66,16 +97,20 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; - if (sendLen == 7 and sendData[6] < rws::SpecialRwRequest::NUM_REQUESTS) { + if (sendLen == 7 and sendData[6] < static_cast(rws::SpecialRwRequest::NUM_REQUESTS)) { specialRequest = static_cast(sendData[6]); } RwCookie* rwCookie = dynamic_cast(cookie); + if (rwCookie == nullptr) { + return returnvalue::FAILED; + } { MutexGuard mg(ipcLock); rwCookie->currentRwSpeed = speed; rwCookie->currentRampTime = rampTime; rwCookie->specialRequest = specialRequest; if (state == InternalState::IDLE and rwCookie->rwIdx == 3) { + state = InternalState::BUSY; semaphore->release(); } } @@ -98,19 +133,17 @@ ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buf return returnvalue::OK; } -ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen) { +ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) { // Stopwatch watch; ReturnValue_t result = returnvalue::OK; int fd = 0; - const std::string& dev = spiIF->getSpiDev(); - MutexIF* spiLock = spiIF->getCsMutex(); - result = openSpi(dev, O_RDWR, fd); + result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { return result; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx], spiLock, sendData, sendDataLen); + ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx]); if (result != returnvalue::OK) { closeSpi(fd); return returnvalue::FAILED; @@ -119,11 +152,11 @@ ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t closeSpi(fd); usleep(rws::SPI_REPLY_DELAY); - return readAllRws(fd, spiLock, dev.c_str()); + return readAllRws(fd, id); } -ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& fd) { - fd = open(devname.c_str(), flags); +ReturnValue_t RwPollingTask::openSpi(int flags, int& fd) { + fd = open(spiDev, flags); if (fd < 0) { sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl; return SpiComIF::OPENING_FILE_FAILED; @@ -132,15 +165,15 @@ ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& return returnvalue::OK; } -ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, - uint8_t* replyBuf) { +ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, + size_t maxReplyLen) { ReturnValue_t result = returnvalue::OK; int fd = 0; gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); pullCsLow(gpioId, spiLock, gpioIF); for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { - result = openSpi(spiDev, O_RDWR, fd); + result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { return result; } @@ -184,7 +217,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki size_t decodedFrameLen = 0; - while (decodedFrameLen < processingBuf.size()) { + while (decodedFrameLen < maxReplyLen) { /** First byte already read in */ if (decodedFrameLen != 0) { byteRead = 0; @@ -205,11 +238,11 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki break; } if (byteRead == 0x5E) { - *(processingBuf.data() + decodedFrameLen) = 0x7E; + *(replyBuf + decodedFrameLen) = 0x7E; decodedFrameLen++; continue; } else if (byteRead == 0x5D) { - *(processingBuf.data() + decodedFrameLen) = 0x7D; + *(replyBuf + decodedFrameLen) = 0x7D; decodedFrameLen++; continue; } else { @@ -219,7 +252,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki break; } } else { - *(processingBuf.data() + decodedFrameLen) = byteRead; + *(replyBuf + decodedFrameLen) = byteRead; decodedFrameLen++; continue; } @@ -229,7 +262,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. * Otherwise there might be something wrong. */ - if (decodedFrameLen == processingBuf.size()) { + if (decodedFrameLen == maxReplyLen) { if (read(fd, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; result = rws::SPI_READ_FAILURE; @@ -249,16 +282,8 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki return returnvalue::OK; } -ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) { - int fd = 0; - const std::string& dev = spiIF->getSpiDev(); - MutexIF* spiLock = spiIF->getCsMutex(); - ReturnValue_t result = openSpi(dev, O_RDWR, fd); - if (result != returnvalue::OK) { - return result; - } - ReturnValue_t result = - sendOneMessage(fd, *rwCookies[rwIdx], spiLock, writeBuffer.data(), writeLen); +ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { + ReturnValue_t result = sendOneMessage(fd, *rwCookies[rwIdx]); if (result != returnvalue::OK) { closeSpi(fd); return returnvalue::FAILED; @@ -266,20 +291,83 @@ ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) { return returnvalue::OK; } -ReturnValue_t RwPollingTask::readAllRws(int fd, MutexIF* spiLock, const char* dev) { +ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) { + ReturnValue_t result = openSpi(O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { if (spiLock == nullptr) { sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; return returnvalue::FAILED; } + // TODO: Fix buffer to write to uint8_t* replyBuf; - readNextReply(dev, *rwCookies[idx], spiLock, replyBuf); + size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf); + readNextReply(*rwCookies[idx], replyBuf, maxReadLen); } closeSpi(fd); return returnvalue::OK; } +size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** ptr) { + uint8_t* rawStart = rwCookies[rwIdx]->replyBuf.data(); + RwReplies replies(rawStart); + switch (id) { + case (rws::GET_RW_STATUS): { + *ptr = replies.rwStatusReply; + return rws::SIZE_GET_RW_STATUS; + } + case (rws::SET_SPEED): { + *ptr = replies.setSpeedReply; + return rws::SIZE_SET_SPEED_REPLY; + } + case (rws::CLEAR_LAST_RESET_STATUS): { + *ptr = replies.clearLastResetStatusReply; + return rws::SIZE_CLEAR_RESET_STATUS; + } + case (rws::GET_LAST_RESET_STATUS): { + *ptr = replies.getLastResetStatusReply; + return rws::SIZE_GET_RESET_STATUS; + } + case (rws::GET_TEMPERATURE): { + *ptr = replies.readTemperatureReply; + return rws::SIZE_GET_TEMPERATURE_REPLY; + } + case (rws::GET_TM): { + *ptr = replies.hkDataReply; + return rws::SIZE_GET_TELEMETRY_REPLY; + } + case (rws::INIT_RW_CONTROLLER): { + *ptr = replies.initRwControllerReply; + return rws::SIZE_INIT_RW; + } + default: { + sif::error << "no reply buffer for rw command " << id << std::endl; + *ptr = replies.dummyPointer; + return 0; + } + } +} + +void RwPollingTask::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen) { + encodedBuffer[0] = FLAG_BYTE; + encodedLen = 1; + for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) { + if (sourceBuf[sourceIdx] == 0x7E) { + encodedBuffer[encodedLen++] = 0x7D; + encodedBuffer[encodedLen++] = 0x5E; + } else if (sourceBuf[sourceIdx] == 0x7D) { + encodedBuffer[encodedLen++] = 0x7D; + encodedBuffer[encodedLen++] = 0x5D; + } else { + encodedBuffer[encodedLen++] = sourceBuf[sourceIdx]; + } + } + encodedBuffer[encodedLen++] = FLAG_BYTE; +} + // This closes the SPI void RwPollingTask::closeSpi(int fd) { // This will perform the function to close the SPI @@ -287,8 +375,7 @@ void RwPollingTask::closeSpi(int fd) { // The SPI is now closed. } -ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, - const uint8_t* data, size_t dataLen) { +ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); if (spiLock == nullptr) { @@ -296,15 +383,8 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* return returnvalue::FAILED; } pullCsLow(gpioId, spiLock, gpioIF); - /** Sending frame start sign */ - writeBuffer[0] = FLAG_BYTE; - size_t writeSize = 1; - if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { - sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); - return rws::SPI_WRITE_FAILURE; - } - /** Encoding and sending command */ + /* + //Encoding and sending command size_t idx = 0; while (idx < dataLen) { switch (*(data + idx)) { @@ -324,21 +404,16 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* break; } } - if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { - sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); - return rws::SPI_WRITE_FAILURE; - } - idx++; - /** Sending frame end sign */ - writeBuffer[0] = FLAG_BYTE; - writeSize = 1; - - if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + */ + // Add datalinklayer like specified in the datasheet. + size_t lenToSend = 0; + encodeHdlc(writeBuffer.data(), writeLen, lenToSend); + if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; pullCsHigh(gpioId, spiLock, gpioIF); return rws::SPI_WRITE_FAILURE; } + pullCsHigh(gpioId, spiLock, gpioIF); return returnvalue::OK; } @@ -376,6 +451,7 @@ void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) { uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF); writeBuffer[1] = static_cast(crc & 0xFF); writeBuffer[2] = static_cast(crc >> 8 & 0xFF); + writeLen = 3; } ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { @@ -397,5 +473,6 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF); writeBuffer[7] = static_cast(crc & 0xFF); writeBuffer[8] = static_cast((crc >> 8) & 0xFF); + writeLen = 9; return returnvalue::OK; } diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index d649a66a..2b7d899f 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -14,15 +14,16 @@ class RwCookie : public SpiCookie { friend class RwPollingTask; public: + static constexpr size_t REPLY_BUF_LEN = 524; RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed) : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {} private: - std::array replyBuf{}; + std::array replyBuf{}; int32_t currentRwSpeed = 0; uint16_t currentRampTime = 0; - rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::NONE; + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; uint8_t rwIdx; }; @@ -39,24 +40,26 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev bool debugMode = false; bool modeAndSpeedWasSet = false; MutexIF* ipcLock; + MutexIF* spiLock; + const char* spiDev; SpiComIF* spiIF; std::array rwCookies; std::array writeBuffer; + std::array encodedBuffer; + size_t writeLen = 0; - std::array processingBuf; //! This is the end and start marker of the frame datalinklayer static constexpr uint8_t FLAG_BYTE = 0x7E; static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static constexpr uint32_t TIMEOUT_MS = 20; static constexpr uint8_t MAX_RETRIES_REPLY = 5; - ReturnValue_t writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen); - ReturnValue_t writeOneRw(uint8_t rwIdx); - ReturnValue_t readAllRws(int fd, MutexIF* spiLock, const char* dev); - ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, const uint8_t* data, - size_t dataLen); - ReturnValue_t readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, - uint8_t* replyBuf); + ReturnValue_t writeAndReadAllRws(DeviceCommandId_t id); + ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd); + ReturnValue_t readAllRws(int fd, DeviceCommandId_t id); + + ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie); + ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen); ReturnValue_t initializeInterface(CookieIF* cookie) override; ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; @@ -66,11 +69,14 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; - ReturnValue_t openSpi(const std::string& devname, int flags, int& fd); + ReturnValue_t openSpi(int flags, int& fd); ReturnValue_t pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); void prepareSimpleCommand(DeviceCommandId_t id); ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx); + size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); + void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); + void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); void closeSpi(int); }; diff --git a/mission/devices/devicedefinitions/rwHelpers.h b/mission/devices/devicedefinitions/rwHelpers.h index 743813fa..55b17859 100644 --- a/mission/devices/devicedefinitions/rwHelpers.h +++ b/mission/devices/devicedefinitions/rwHelpers.h @@ -19,7 +19,7 @@ static const size_t SIZE_GET_TEMPERATURE_REPLY = 8; /** Max size when requesting telemetry */ static const size_t SIZE_GET_TELEMETRY_REPLY = 91; -enum SpecialRwRequest : uint8_t { +enum class SpecialRwRequest : uint8_t { REQUEST_NONE = 0, RESET_MCU = 1, INIT_RW_CONTROLLER = 2, @@ -27,47 +27,6 @@ enum SpecialRwRequest : uint8_t { NUM_REQUESTS }; -struct RwReplies { - friend class RwPollingTask; - - public: - RwReplies(const uint8_t* rawData) : rawData(rawData) { - rwStatusReply = rawData; - setSpeedReply = rawData + SIZE_GET_RW_STATUS; - getLastResetStatusReply = setSpeedReply + SIZE_SET_SPEED_REPLY; - clearLastResetStatusReply = getLastResetStatusReply + SIZE_GET_RESET_STATUS; - readTemperatureReply = clearLastResetStatusReply + SIZE_CLEAR_RESET_STATUS; - hkDataReply = readTemperatureReply + SIZE_GET_TEMPERATURE_REPLY; - initRwControllerReply = hkDataReply + SIZE_GET_TELEMETRY_REPLY; - } - - const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; } - - const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; } - - const uint8_t* getHkDataReply() const { return hkDataReply; } - - const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } - - const uint8_t* getRawData() const { return rawData; } - - const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } - - const uint8_t* getRwStatusReply() const { return rwStatusReply; } - - const uint8_t* getSetSpeedReply() const { return setSpeedReply; } - - private: - const uint8_t* rawData; - const uint8_t* rwStatusReply; - const uint8_t* setSpeedReply; - const uint8_t* getLastResetStatusReply; - const uint8_t* clearLastResetStatusReply; - const uint8_t* readTemperatureReply; - const uint8_t* hkDataReply; - const uint8_t* initRwControllerReply; -}; - static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); @@ -299,4 +258,50 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { } // namespace rws +struct RwReplies { + friend class RwPollingTask; + + public: + RwReplies(const uint8_t* rawData) : rawData(const_cast(rawData)) { initPointers(); } + + const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; } + + const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; } + + const uint8_t* getHkDataReply() const { return hkDataReply; } + + const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } + + const uint8_t* getRawData() const { return rawData; } + + const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } + + const uint8_t* getRwStatusReply() const { return rwStatusReply; } + + const uint8_t* getSetSpeedReply() const { return setSpeedReply; } + + private: + RwReplies(uint8_t* rwData) : rawData(rwData) { initPointers(); } + + void initPointers() { + rwStatusReply = rawData; + setSpeedReply = rawData + rws::SIZE_GET_RW_STATUS; + getLastResetStatusReply = setSpeedReply + rws::SIZE_SET_SPEED_REPLY; + clearLastResetStatusReply = getLastResetStatusReply + rws::SIZE_GET_RESET_STATUS; + readTemperatureReply = clearLastResetStatusReply + rws::SIZE_CLEAR_RESET_STATUS; + hkDataReply = readTemperatureReply + rws::SIZE_GET_TEMPERATURE_REPLY; + initRwControllerReply = hkDataReply + rws::SIZE_GET_TELEMETRY_REPLY; + dummyPointer = initRwControllerReply + rws::SIZE_INIT_RW; + } + uint8_t* rawData; + uint8_t* rwStatusReply; + uint8_t* setSpeedReply; + uint8_t* getLastResetStatusReply; + uint8_t* clearLastResetStatusReply; + uint8_t* readTemperatureReply; + uint8_t* hkDataReply; + uint8_t* initRwControllerReply; + uint8_t* dummyPointer; +}; + #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ */ From 655e01c2d13979111a11f226151a45d63ee42175 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:27:19 +0100 Subject: [PATCH 45/69] some more bugfixes --- linux/devices/RwPollingTask.cpp | 136 ++++++++++++++++---------------- linux/devices/RwPollingTask.h | 2 +- 2 files changed, 70 insertions(+), 68 deletions(-) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 657f8853..b300136a 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -172,6 +172,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); pullCsLow(gpioId, spiLock, gpioIF); + uint8_t byteRead = 0; for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { @@ -181,18 +182,17 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf * The reaction wheel responds with empty frames while preparing the reply data. * However, receiving more than 5 empty frames will be interpreted as an error. */ - uint8_t byteRead = 0; for (int idx = 0; idx < 5; idx++) { if (read(fd, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != FLAG_BYTE) { sif::error << "Invalid data, expected start marker" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::NO_START_MARKER; } @@ -202,7 +202,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf break; } - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); if (idx == MAX_RETRIES_REPLY - 1) { sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; @@ -210,76 +210,78 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } TaskFactory::delayTask(5); } - + } #if FSFW_HAL_SPI_WIRETAPPING == 1 - sif::info << "RW start marker detected" << std::endl; + sif::info << "RW start marker detected" << std::endl; #endif - size_t decodedFrameLen = 0; + size_t decodedFrameLen = 0; - while (decodedFrameLen < maxReplyLen) { - /** First byte already read in */ - if (decodedFrameLen != 0) { - byteRead = 0; - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - } - - if (byteRead == FLAG_BYTE) { - /** Reached end of frame */ + while (decodedFrameLen < maxReplyLen) { + // First byte already read in + if (decodedFrameLen != 0) { + byteRead = 0; + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; break; - } else if (byteRead == 0x7D) { - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - if (byteRead == 0x5E) { - *(replyBuf + decodedFrameLen) = 0x7E; - decodedFrameLen++; - continue; - } else if (byteRead == 0x5D) { - *(replyBuf + decodedFrameLen) = 0x7D; - decodedFrameLen++; - continue; - } else { - sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; - closeSpi(fd); - result = rws::INVALID_SUBSTITUTE; - break; - } - } else { - *(replyBuf + decodedFrameLen) = byteRead; + } + } + + if (byteRead == FLAG_BYTE) { + // Reached end of frame + break; + } else if (byteRead == 0x7D) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead == 0x5E) { + *(replyBuf + decodedFrameLen) = 0x7E; decodedFrameLen++; continue; + } else if (byteRead == 0x5D) { + *(replyBuf + decodedFrameLen) = 0x7D; + decodedFrameLen++; + continue; + } else { + sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; + result = rws::INVALID_SUBSTITUTE; + break; } - - /** - * There might be the unlikely case that each byte in a get-telemetry reply has been - * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. - * Otherwise there might be something wrong. - */ - if (decodedFrameLen == maxReplyLen) { - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - if (byteRead != FLAG_BYTE) { - sif::error << "rwSpiCallback::spiCallback: Missing end sign " - << static_cast(FLAG_BYTE) << std::endl; - decodedFrameLen--; - result = rws::MISSING_END_SIGN; - break; - } - } - result = returnvalue::OK; + } else { + *(replyBuf + decodedFrameLen) = byteRead; + decodedFrameLen++; + continue; } + + // Check end marker. + /** + * There might be the unlikely case that each byte in a get-telemetry reply has been + * replaced by its substitute. Then the next byte must correspond to the end sign 0x7E. + * Otherwise there might be something wrong. + */ + if (decodedFrameLen == maxReplyLen) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead != FLAG_BYTE) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) + << std::endl; + decodedFrameLen--; + result = rws::MISSING_END_SIGN; + break; + } + } + result = returnvalue::OK; } - return returnvalue::OK; + + pullCsHigh(gpioId, gpioIF); + closeSpi(fd); + return result; } ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { @@ -410,10 +412,10 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { encodeHdlc(writeBuffer.data(), writeLen, lenToSend); if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); return rws::SPI_WRITE_FAILURE; } - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); return returnvalue::OK; } @@ -434,7 +436,7 @@ ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF return returnvalue::OK; } -void RwPollingTask::pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { +void RwPollingTask::pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF) { if (gpioId != gpio::NO_GPIO) { if (gpioIF.pullHigh(gpioId) != returnvalue::OK) { sif::error << "closeSpi: Failed to pull chip select high" << std::endl; diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index 2b7d899f..701771f1 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -77,7 +77,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); - void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF); void closeSpi(int); }; From 9fdb41506b070ee0c0254eef417f307b984880a1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:58:32 +0100 Subject: [PATCH 46/69] added basic unittests for hdlc encoder --- linux/devices/RwPollingTask.cpp | 53 +++---------------- linux/devices/RwPollingTask.h | 3 -- misc/eclipse/.cproject | 35 +++++++----- .../devices/devicedefinitions/CMakeLists.txt | 2 +- .../devices/devicedefinitions/rwHelpers.cpp | 18 +++++++ mission/devices/devicedefinitions/rwHelpers.h | 8 +++ unittest/CMakeLists.txt | 1 + unittest/hdlcEncodingRw.cpp | 38 +++++++++++++ 8 files changed, 95 insertions(+), 63 deletions(-) create mode 100644 unittest/hdlcEncodingRw.cpp diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index b300136a..c8744301 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -190,7 +190,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf return rws::SPI_READ_FAILURE; } if (idx == 0) { - if (byteRead != FLAG_BYTE) { + if (byteRead != rws::FRAME_DELIMITER) { sif::error << "Invalid data, expected start marker" << std::endl; pullCsHigh(gpioId, gpioIF); closeSpi(fd); @@ -198,7 +198,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } } - if (byteRead != FLAG_BYTE) { + if (byteRead != rws::FRAME_DELIMITER) { break; } @@ -228,7 +228,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } } - if (byteRead == FLAG_BYTE) { + if (byteRead == rws::FRAME_DELIMITER) { // Reached end of frame break; } else if (byteRead == 0x7D) { @@ -268,9 +268,9 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf result = rws::SPI_READ_FAILURE; break; } - if (byteRead != FLAG_BYTE) { - sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) - << std::endl; + if (byteRead != rws::FRAME_DELIMITER) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " + << static_cast(rws::FRAME_DELIMITER) << std::endl; decodedFrameLen--; result = rws::MISSING_END_SIGN; break; @@ -353,23 +353,6 @@ size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, } } -void RwPollingTask::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen) { - encodedBuffer[0] = FLAG_BYTE; - encodedLen = 1; - for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) { - if (sourceBuf[sourceIdx] == 0x7E) { - encodedBuffer[encodedLen++] = 0x7D; - encodedBuffer[encodedLen++] = 0x5E; - } else if (sourceBuf[sourceIdx] == 0x7D) { - encodedBuffer[encodedLen++] = 0x7D; - encodedBuffer[encodedLen++] = 0x5D; - } else { - encodedBuffer[encodedLen++] = sourceBuf[sourceIdx]; - } - } - encodedBuffer[encodedLen++] = FLAG_BYTE; -} - // This closes the SPI void RwPollingTask::closeSpi(int fd) { // This will perform the function to close the SPI @@ -385,31 +368,9 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { return returnvalue::FAILED; } pullCsLow(gpioId, spiLock, gpioIF); - /* - //Encoding and sending command - size_t idx = 0; - while (idx < dataLen) { - switch (*(data + idx)) { - case 0x7E: - writeBuffer[0] = 0x7D; - writeBuffer[1] = 0x5E; - writeSize = 2; - break; - case 0x7D: - writeBuffer[0] = 0x7D; - writeBuffer[1] = 0x5D; - writeSize = 2; - break; - default: - writeBuffer[0] = *(data + idx); - writeSize = 1; - break; - } - } - */ // Add datalinklayer like specified in the datasheet. size_t lenToSend = 0; - encodeHdlc(writeBuffer.data(), writeLen, lenToSend); + rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend); if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; pullCsHigh(gpioId, gpioIF); diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index 701771f1..b9895865 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -48,8 +48,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev std::array encodedBuffer; size_t writeLen = 0; - //! This is the end and start marker of the frame datalinklayer - static constexpr uint8_t FLAG_BYTE = 0x7E; static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static constexpr uint32_t TIMEOUT_MS = 20; static constexpr uint8_t MAX_RETRIES_REPLY = 5; @@ -75,7 +73,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx); size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); - void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); void pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF); void closeSpi(int); diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 0dfd812a..0f3348a8 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,8 @@ - + + @@ -119,7 +120,8 @@ - + + @@ -187,7 +189,8 @@ - + + @@ -255,7 +258,8 @@ - + + @@ -418,7 +422,8 @@ - + + @@ -580,7 +585,8 @@ - + + @@ -680,7 +686,7 @@