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

removed unit quats as they are confusing
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-04-11 10:55:48 +02:00
parent 18be21a310
commit c66799b24f
4 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
mission/controller/AcsController.cpp
View File

@ -576,13 +576,13 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.setValid(true);
@ -606,10 +606,10 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
void AcsController::disableCtrlValData() {
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));
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errAng.value = 0;
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.setValidity(false, true);
}
}
@ -691,6 +691,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);

7
mission/controller/AcsController.h
View File

@ -40,9 +40,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl();
private:
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
static constexpr double ZERO_VEC[3] = {0, 0, 0};
static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
bool enableHkSets = false;
@ -214,6 +214,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
// Ctrl Values
acsctrl::CtrlValData ctrlValData;
PoolEntry<uint8_t> safeStrat = PoolEntry<uint8_t>();
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>();

6
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -1105,11 +1105,9 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
double zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
std::memcpy(mekfData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double));
mekfData->quatMekf.setValid(false);
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
std::memcpy(mekfData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double));
mekfData->satRotRateMekf.setValid(false);
mekfData->mekfStatus = mekfStatus;
mekfData->setValidity(true, false);

3
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -80,6 +80,9 @@ class MultiplicativeKalmanFilter {
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
private:
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
/*Parameters*/
double quaternion_STR_SB[4];