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

New Safe Mode Controller #748

Merged
muellerr merged 31 commits from acs-safe-flp-v2 into main 2023-07-26 11:08:12 +02:00
Conversation 2 Commits 31 Files Changed 15 +34 -3
4 changed files with 34 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d90fd07ba3 - Show all commits

8
mission/controller/AcsController.cpp
View File

@ -20,7 +20,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
gpsDataProcessed(this), gpsDataProcessed(this),
mekfData(this), mekfData(this),
ctrlValData(this), ctrlValData(this),
actuatorCmdData(this) {} actuatorCmdData(this),
fusedRotRateData(this) {}
ReturnValue_t AcsController::initialize() { ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize(); ReturnValue_t result = parameterHelper.initialize();
@ -716,6 +717,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0}); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
return returnvalue::OK; return returnvalue::OK;
} }

6
mission/controller/AcsController.h
View File

@ -226,6 +226,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4); PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3); PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
// Fused Rot Rate
acsctrl::FusedRotRateData fusedRotRateData;
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
// Initial delay to make sure all pool variables have been initialized their owners // Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY); Countdown initialCountdown = Countdown(INIT_DELAY);
}; };

2
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -15,7 +15,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
} }
if (not sunValid) { if (not sunValid) {
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
fusedRateOrthogonalB); // ToDo fusedRateOrthogonalB);
return; return;
} }

21
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -18,7 +18,8 @@ enum SetIds : uint32_t {
GPS_PROCESSED_DATA, GPS_PROCESSED_DATA,
MEKF_DATA, MEKF_DATA,
CTRL_VAL_DATA, CTRL_VAL_DATA,
ACTUATOR_CMD_DATA ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA,
}; };
enum PoolIds : lp_id_t { enum PoolIds : lp_id_t {
@ -103,6 +104,10 @@ enum PoolIds : lp_id_t {
RW_TARGET_TORQUE, RW_TARGET_TORQUE,
RW_TARGET_SPEED, RW_TARGET_SPEED,
MTQ_TARGET_DIPOLE, MTQ_TARGET_DIPOLE,
// Fused Rotation Rate
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
}; };
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -115,6 +120,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
/** /**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -273,6 +279,19 @@ class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
private: private:
}; };
class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
public:
FusedRotRateData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonal =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
private:
};
} // namespace acsctrl } // namespace acsctrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */ #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */