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

Merge pull request 'ACS Tweaks and Bugfix' (#403) from acs_tweaks_possibly_fixes into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source 
Reviewed-on: #403
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
This commit is contained in:
Robin Müller 2023-02-23 18:52:08 +01:00
commit ed992e495b
5 changed files with 51 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CHANGELOG.md
View File

@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [unreleased]
## Fixed
- ACS Controller: Correction for size of MEKF dataset and some optimization and possible fixes
for actuator control.
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/403
# [v1.31.0] # [v1.31.0]
eive-tmtc: v2.16.0 eive-tmtc: v2.16.0

78
mission/controller/AcsController.cpp
View File

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

14
mission/controller/AcsController.h
View File

@ -40,6 +40,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl(); void performPointingCtrl();
private: 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; AcsParameters acsParameters;
SensorProcessing sensorProcessing; SensorProcessing sensorProcessing;
Navigation navigation; Navigation navigation;
@ -84,11 +88,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 +191,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;

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

@ -110,7 +110,7 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;

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);