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,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTorque, double *rwCmdSpeed) {
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3,
const double *rwTorque, int32_t *rwCmdSpeed) {
using namespace Math;
// 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 commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
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
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentActuator, 3, 3, 1);
dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
@ -69,8 +74,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu
}
if (maxValue > maxDipol) {
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
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
* reaction wheel
*/
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,7 +37,7 @@ class ActuatorCmd {
* @param: dipolMoment given dipol moment in spacecraft 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:
private: