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

ACS Tweaks and Bugfix #403

Merged
muellerr merged 3 commits from acs_tweaks_possibly_fixes into develop 2023-02-23 18:52:10 +01:00
Conversation 0 Commits 3 Files Changed 5 +21 -29
2 changed files with 21 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 0907e8f5e5 - Show all commits

18
mission/controller/AcsController.cpp
View File

@ -433,15 +433,12 @@ 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));
@ -450,7 +447,6 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
actuatorCmdData.setValidity(true, true); actuatorCmdData.setValidity(true, true);
} }
} }
}
void AcsController::updateCtrlValData(double errAng) { void AcsController::updateCtrlValData(double errAng) {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
@ -466,7 +462,6 @@ 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));
@ -475,21 +470,16 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
ctrlValData.setValidity(true, true); ctrlValData.setValidity(true, true);
} }
} }
}
void AcsController::disableCtrlValData() { void AcsController::disableCtrlValData() {
double unitQuat[4] = {0, 0, 0, 1};
double errAng = 0;
{
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.errAng.value = errAng; ctrlValData.errAng.value = 0;
ctrlValData.setValidity(false, true); ctrlValData.setValidity(false, true);
} }
} }
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {

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;