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 +23 -23
3 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 59b80807ba - Show all commits

30
mission/controller/AcsController.cpp
View File

@ -432,14 +432,15 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
return returnvalue::OK; return returnvalue::OK;
} }
void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) { void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
int32_t rwTargetSpeed[4] = {0, 0, 0, 0}; int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole); updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
} }
void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
int16_t mtqTargetDipole[3]) { const int32_t *rwTargetSpeed,
const int16_t *mtqTargetDipole) {
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -452,22 +453,19 @@ void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTa
} }
void AcsController::updateCtrlValData(double errAng) { void AcsController::updateCtrlValData(double errAng) {
double unitQuat[4] = {0, 0, 0, 1}; PoolReadGuard pg(&ctrlValData);
{ if (pg.getReadResult() == returnvalue::OK) {
PoolReadGuard pg(&ctrlValData); std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
if (pg.getReadResult() == returnvalue::OK) { ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false); ctrlValData.errQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng;
ctrlValData.errQuat.setValid(false); ctrlValData.errAng.setValid(true);
ctrlValData.errAng.value = errAng; ctrlValData.setValidity(true, false);
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
} }
} }
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], 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) {

12
mission/controller/AcsController.h
View File

@ -40,6 +40,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl(); void performPointingCtrl();
private: private:
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
AcsParameters acsParameters; AcsParameters acsParameters;
SensorProcessing sensorProcessing; SensorProcessing sensorProcessing;
Navigation navigation; Navigation navigation;
@ -84,11 +86,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
void updateActuatorCmdData(int16_t mtqTargetDipole[3]); void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
int16_t mtqTargetDipole[3]); const int16_t* mtqTargetDipole);
void updateCtrlValData(double errAng); void updateCtrlValData(double errAng);
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng); void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng);
void disableCtrlValData(); void disableCtrlValData();
/* ACS Sensor Values */ /* ACS Sensor Values */
@ -187,7 +189,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> tgtQuat = PoolEntry<double>(4); PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4); PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>(); PoolEntry<double> errAng = PoolEntry<double>();
PoolEntry<double> tgtRotRate = PoolEntry<double>(4); PoolEntry<double> tgtRotRate = PoolEntry<double>(3);
// Actuator CMD // Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData; acsctrl::ActuatorCmdData actuatorCmdData;

4
mission/system/objects/ComSubsystem.h
View File

@ -17,8 +17,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
* @param maxNumberOfSequences * @param maxNumberOfSequences
* @param maxNumberOfTables * @param maxNumberOfTables
* @param transmitterTimeout Maximum time the transmitter of the syrlinks * @param transmitterTimeout Maximum time the transmitter of the syrlinks
* will be * will
* enabled * be enabled
*/ */
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables,
uint32_t transmitterTimeout); uint32_t transmitterTimeout);