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

added Antistiction, added Nadir Pointing, added performSafe()
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Marquardt
2022-11-08 13:48:50 +01:00
parent 75ab11fc35
commit 20936faec6
9 changed files with 213 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,26 +23,27 @@ ActuatorCmd::~ActuatorCmd(){
}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
using namespace Math;
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
// Scaling the commanded torque to a maximum value
double torque[4] = {0, 0, 0, 0};
double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(torque[i]) > maxValue) {
maxValue = abs(torque[i]);
if (abs(rwTrq[i]) > maxValue) {
maxValue = abs(rwTrq[i]);
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
}
}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTorque, double *rwCmdSpeed) {
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
@ -52,7 +53,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
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;
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
}