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

use more constants
Some checks are pending
EIVE/eive-obsw/pipeline/head This commit looks good
Details
EIVE/eive-obsw/pipeline/pr-develop Build started...
Details

Browse Source
This commit is contained in:
Robin Müller 2023-02-23 18:39:14 +01:00
parent 59b80807ba
commit 0907e8f5e5
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
2 changed files with 21 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
mission/controller/AcsController.cpp
View File

@ -433,22 +433,18 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
} }
void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) { void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
} }
void AcsController::updateActuatorCmdData(const double *rwTargetTorque, void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
const int32_t *rwTargetSpeed, const int32_t *rwTargetSpeed,
const int16_t *mtqTargetDipole) { const int16_t *mtqTargetDipole) {
{ PoolReadGuard pg(&actuatorCmdData);
PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) {
if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); actuatorCmdData.setValidity(true, true);
actuatorCmdData.setValidity(true, true);
}
} }
} }
@ -466,28 +462,22 @@ void AcsController::updateCtrlValData(double errAng) {
} }
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) { void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) {
{ PoolReadGuard pg(&ctrlValData);
PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) {
if (pg.getReadResult() == returnvalue::OK) { std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng;
ctrlValData.errAng.value = errAng; ctrlValData.setValidity(true, true);
ctrlValData.setValidity(true, true);
}
} }
} }
void AcsController::disableCtrlValData() { void AcsController::disableCtrlValData() {
double unitQuat[4] = {0, 0, 0, 1}; PoolReadGuard pg(&ctrlValData);
double errAng = 0; if (pg.getReadResult() == returnvalue::OK) {
{ std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
PoolReadGuard pg(&ctrlValData); std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
if (pg.getReadResult() == returnvalue::OK) { ctrlValData.errAng.value = 0;
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.setValidity(false, true);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(false, true);
}
} }
} }

2
mission/controller/AcsController.h
View File

@ -41,6 +41,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
private: private:
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
AcsParameters acsParameters; AcsParameters acsParameters;
SensorProcessing sensorProcessing; SensorProcessing sensorProcessing;