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

ActuatorCmd now output their solutions as integers as expected by the sensors
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-10 10:56:50 +01:00
parent 9dfd8491d2
commit 9041a3376c
2 changed files with 21 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,27 +38,32 @@ 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, double *rwCmdSpeed) { const double *rwTorque, int32_t *rwCmdSpeed) {
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
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*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, double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; 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 = commandTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4); for (int i = 0; i < 4; i++) {
deltaSpeedInt[i] = std::round(deltaSpeed[i]);
}
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
} }
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) { void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {
// Convert to actuator frame // Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentActuator, 3, 3, 1); dipolMoment, 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 = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0; double maxValue = 0;
@ -69,8 +74,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu
} }
if (maxValue > maxDipol) { if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue; double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3); VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3);
} }
// scale dipole from 1 Am^2 to 1e^-4 Am^2 // scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3); VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
for (int i = 0; i < 3; i++) {
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
}
} }

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

@ -28,8 +28,8 @@ class ActuatorCmd {
* rwCmdSpeed output revolutions per minute for every * rwCmdSpeed output revolutions per minute for every
* 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, double *rwCmdSpeed); const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,7 +37,7 @@ 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, double *dipolMomentActuator); void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator);
protected: protected:
private: private: