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

ACS Ctrl Bug Bash #439

Merged
muellerr merged 42 commits from acs-bug-bash into develop 2023-03-10 19:21:19 +01:00
Conversation 0 Commits 42 Files Changed 23 +21 -16
2 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit fd4be08796 - Show all commits

25
mission/controller/acs/ActuatorCmd.cpp
View File

@ -10,13 +10,14 @@
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled,
AcsParameters::RwHandlingParameters *rwHandlingParameters) {
// Scaling the commanded torque to a maximum value // Scaling the commanded torque to a maximum value
double maxTrq = acsParameters.rwHandlingParameters.maxTrq; double maxTrq = rwHandlingParameters->maxTrq;
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ? for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
@ -33,17 +34,17 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3, const int32_t speedRw2, const int32_t speedRw3,
const double *rwTorque, int32_t *rwCmdSpeed) { const double *rwTorque, int32_t *rwCmdSpeed,
AcsParameters *acsParameters) {
using namespace Math; using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel // Calculating the commanded speed in RPM for every reaction wheel
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0}; double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s] // W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm; double factor = acsParameters->onBoardParams.sampleTime /
acsParameters->rwHandlingParameters.inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
@ -53,13 +54,15 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
} }
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
AcsParameters::MagnetorquerParameter *magnetorquerParameter) {
sif::debug << magnetorquerParameter->dipolMax << std::endl;
// Convert to actuator frame // Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0}; double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, MatrixOperations<double>::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment,
dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); dipolMomentActuatorDouble, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum // Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax; double maxDipol = magnetorquerParameter->dipolMax;
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (abs(dipolMomentActuator[i]) > maxDipol) { if (abs(dipolMomentActuator[i]) > maxDipol) {

12
mission/controller/acs/ActuatorCmd.h
View File

@ -8,7 +8,7 @@
class ActuatorCmd { class ActuatorCmd {
public: public:
ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? ActuatorCmd(); // Input mode ?
virtual ~ActuatorCmd(); virtual ~ActuatorCmd();
/* /*
@ -17,7 +17,8 @@ class ActuatorCmd {
* @param: rwTrq given torque for reaction wheels * @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque * rwTrqScaled possible scaled torque
*/ */
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled,
AcsParameters::RwHandlingParameters *rwHandlingParameters);
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
@ -29,7 +30,8 @@ class ActuatorCmd {
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
AcsParameters *acsParameters);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,11 +39,11 @@ class ActuatorCmd {
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame * dipolMomentActuator resulting dipol moment in actuator reference frame
*/ */
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
AcsParameters::MagnetorquerParameter *magnetorquerParameter);
protected: protected:
private: private:
AcsParameters acsParameters;
}; };
#endif /* ACTUATORCMD_H_ */ #endif /* ACTUATORCMD_H_ */