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

Merge remote-tracking branch 'origin/develop' into add_heater_info_set
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller
2023-02-17 11:21:17 +01:00
parent 0bf1df81cd d233dc9f82
commit 1e0c94246d
77 changed files with 2487 additions and 1506 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
mission/controller/AcsController.cpp
View File

@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
multipleRwUnavailableCounter{0},
parameterHelper(this),
mgmDataRaw(this),
mgmDataProcessed(this),
@ -50,6 +51,9 @@ ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
}
void AcsController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "ACS & TCS PST");
#endif
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
@ -116,10 +120,10 @@ void AcsController::performSafe() {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
// Give desired satellite rate and sun direction to align
// give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
// if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) {
@ -137,8 +141,8 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
{
PoolReadGuard pg(&ctrlValData);
@ -180,18 +184,15 @@ void AcsController::performSafe() {
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
// torqueDuration);
// }
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}
void AcsController::performDetumble() {
@ -208,8 +209,8 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -228,10 +229,6 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY);
}
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
}
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
@ -239,18 +236,15 @@ void AcsController::performDetumble() {
actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}
void AcsController::performPointingCtrl() {
@ -270,10 +264,19 @@ void AcsController::performPointingCtrl() {
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
multipleRwUnavailableCounter++;
if (multipleRwUnavailableCounter > 4) {
triggerEvent(acs::MULTIPLE_RW_INVALID);
}
return;
} else {
multipleRwUnavailableCounter = 0;
}
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
double mgtDpDes[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
@ -392,45 +395,60 @@ void AcsController::performPointingCtrl() {
}
if (enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
}
int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
}
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// acsParameters.rwHandlingParameters.rampTime);
}
ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed,
int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
uint16_t rampTime) {
{
PoolReadGuard pg(&dipoleSet);
MutexGuard mg(torquer::lazyLock());
torquer::NEW_ACTUATION_FLAG = true;
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
}
{
PoolReadGuard pg(&rw1SpeedSet);
rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
}
{
PoolReadGuard pg(&rw2SpeedSet);
rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
}
{
PoolReadGuard pg(&rw3SpeedSet);
rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
}
{
PoolReadGuard pg(&rw4SpeedSet);
rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
}
return returnvalue::OK;
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,

18
mission/controller/AcsController.h
View File

@ -5,6 +5,7 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
@ -19,6 +20,7 @@
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
#include "mission/trace.h"
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public:
@ -48,9 +50,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PtgCtrl ptgCtrl;
uint8_t detumbleCounter;
uint8_t multipleRwUnavailableCounter;
ParameterHelper parameterHelper;
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
@ -69,11 +76,20 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive);
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
/* ACS Sensor Values */
ACS::SensorValues sensorValues;
/* ACS Datasets */
/* ACS Actuation Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1);
rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2);
rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3);
rws::RwSpeedActuationSet rw4SpeedSet = rws::RwSpeedActuationSet(objects::RW4);
/* ACS Datasets */
// MGMs
acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);

13
mission/controller/ThermalController.cpp
View File

@ -9,10 +9,10 @@
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/RwDefinitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <objects/systemObjectList.h>
ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater)
@ -73,6 +73,9 @@ ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) {
}
void ThermalController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "TCS Task");
#endif
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
@ -703,7 +706,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw1 = lp_var_t<int32_t>(objects::RW1, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw1 = lp_var_t<int32_t>(objects::RW1, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl;
@ -716,7 +719,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw2 = lp_var_t<int32_t>(objects::RW2, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw2 = lp_var_t<int32_t>(objects::RW2, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl;
@ -729,7 +732,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw3 = lp_var_t<int32_t>(objects::RW3, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw3 = lp_var_t<int32_t>(objects::RW3, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl;
@ -742,7 +745,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw4 = lp_var_t<int32_t>(objects::RW4, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw4 = lp_var_t<int32_t>(objects::RW4, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl;

5
mission/controller/ThermalController.h
View File

@ -11,6 +11,7 @@
#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
#include "mission/devices/HeaterHandler.h"
#include "mission/trace.h"
#include <list>
/**
@ -155,6 +156,10 @@ class ThermalController : public ExtendedControllerBase {
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(DELAY);
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
std::array<std::pair<bool, double>, 5> sensors;
uint8_t numSensors = 0;

16
mission/controller/acs/AcsParameters.cpp
View File

@ -278,6 +278,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
case 0x5:
parameterWrapper->set(rwHandlingParameters.rampTime);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -291,17 +294,17 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(rwMatrices.pseudoInverse);
break;
case 0x2:
parameterWrapper->set(rwMatrices.without0);
break;
case 0x3:
parameterWrapper->set(rwMatrices.without1);
break;
case 0x4:
case 0x3:
parameterWrapper->set(rwMatrices.without2);
break;
case 0x5:
case 0x4:
parameterWrapper->set(rwMatrices.without3);
break;
case 0x5:
parameterWrapper->set(rwMatrices.without4);
break;
case 0x6:
parameterWrapper->set(rwMatrices.nullspace);
break;
@ -584,6 +587,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax);
break;
case 0x6:
parameterWrapper->set(magnetorquesParameter.torqueDuration);
break;
default:
return INVALID_IDENTIFIER_ID;
}

29
mission/controller/acs/AcsParameters.h
View File

@ -73,9 +73,9 @@ class AcsParameters : public HasParametersIF {
{-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}};
float mgm02variance[3] = {1, 1, 1};
float mgm13variance[3] = {1, 1, 1};
float mgm4variance[3] = {1, 1, 1};
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
} mgmHandlingParameters;
struct SusHandlingParameters {
@ -779,19 +779,21 @@ class AcsParameters : public HasParametersIF {
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = true;
} gyrHandlingParameters;
struct RwHandlingParameters {
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 100; // 80; // RPM
double stictionReleaseSpeed = 120; // RPM
double maxTrq = 0.0032; // 3.2 [mNm]
int32_t stictionSpeed = 100; // RPM
int32_t stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006;
uint16_t rampTime = 10;
} rwHandlingParameters;
struct RwMatrices {
@ -800,13 +802,13 @@ class AcsParameters : public HasParametersIF {
{0.3907, 0.3907, 0.3907, 0.3907}};
double pseudoInverse[4][3] = {
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
double without0[4][3] = {
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
double without1[4][3] = {
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
double without2[4][3] = {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
double without3[4][3] = {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double without4[4][3] = {
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
} rwMatrices;
@ -910,6 +912,7 @@ class AcsParameters : public HasParametersIF {
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double DipolMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms]
} magnetorquesParameter;
struct DetumbleParameter {

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

@ -1,10 +1,3 @@
/*
* ActuatorCmd.cpp
*
* Created on: 4 Aug 2022
* Author: Robin Marquardt
*/
#include "ActuatorCmd.h"
#include <fsfw/globalfunctions/constants.h>
@ -38,27 +31,33 @@ 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);
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, 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 +68,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:

117
mission/controller/acs/Guidance.cpp
View File

@ -610,104 +610,33 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
// under 150 arcsec ??
}
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
}
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
!(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
(sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) {
rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
}
else {
if (rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK;
} else {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one
// reaction wheel invalid the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
return returnvalue::FAILED;
}
}

2
mission/controller/acs/Guidance.h
View File

@ -67,7 +67,7 @@ class Guidance {
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"
void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private:
AcsParameters acsParameters;

9
mission/controller/acs/Igrf13Model.cpp
View File

@ -20,6 +20,7 @@ Igrf13Model::~Igrf13Model() {}
void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
const double altitude, timeval timeOfMagMeasurement,
double* magFieldModelInertial) {
double magFieldModel[3] = {0, 0, 0};
double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/
theta -= 90 * PI / 180;
@ -100,12 +101,8 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModelInertial[2] =
magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
// convert nT to uT
VectorOperations<double>::mulScalar(magFieldModelInertial, 1e-3, magFieldModelInertial, 3);
}
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {

1
mission/controller/acs/Igrf13Model.h
View File

@ -47,7 +47,6 @@ class Igrf13Model /*:public HasParametersIF*/ {
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3];
void schmidtNormalization();
private:

11
mission/controller/acs/SensorValues.h
View File

@ -1,13 +1,14 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_H_
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
@ -62,10 +63,10 @@ class SensorValues {
// bool mgt0valid;
RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1);
RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2);
RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3);
RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4);
rws::StatusSet rw1Set = rws::StatusSet(objects::RW1);
rws::StatusSet rw2Set = rws::StatusSet(objects::RW2);
rws::StatusSet rw3Set = rws::StatusSet(objects::RW3);
rws::StatusSet rw4Set = rws::StatusSet(objects::RW4);
};
} /* namespace ACS */

10
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -161,8 +161,14 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
double *torqueCommand) {
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
bool rwAvailable[4] = {
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {

5
mission/controller/acs/control/PtgCtrl.h
View File

@ -54,11 +54,10 @@ class PtgCtrl {
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low
* @param: rwAvailable Boolean Flag for all reaction wheels
* omegaRw current wheel speed of reaction wheels
* @param: sensorValues class containing all RW related values
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
private:
AcsParameters::RwHandlingParameters *rwHandlingParameters;