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

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

2
mission/controller/AcsController.h
View File

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