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

Merge pull request 'Final Version of the ACS Controller' (#367) from eggert/acs into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source 
Reviewed-on: #367
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Robin Müller 2023-02-08 13:50:10 +01:00
commit 6dcec02377
27 changed files with 2011 additions and 1234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
CHANGELOG.md
View File

@ -20,6 +20,23 @@ change warranting a new major release:
## Changed
- Readded calibration matrices for MGM calibration.
- Added calculation of satellite velocity vector from GPS position data
- Added detumble mode using GYR values
- Added inertial pointing mode
- Added nadir pointing mode
- Added ground station target mode
- Added antistiction for RWs
- Added `sunTargetSafe` differentiation for LEOP
- Added check for existance of `SD_0_SKEWED_PTG_FILE` and `SD_1_SKEWED_PTG_FILE` to determine
which `sunTargetSafe` to use
- Added `gpsVelocity` and `gpsPosition` to `gpsProcessed`
- Removed deprecated `OutputValues`
- Added `HasParametersIF` to `AcsParameters`
- Added `ReceivesParameterMessagesIF` and `ParameterHelper` to `AcsController`
- Updated `AcsParameters` with actual values and changed structure
- Sun vector model and magnetic field vector model calculations are always executed now
- `domainId` is now used as identifier for parameter structs
- Changed onboard GYR value handling from deg/s to rad/s
## Fixed
@ -32,6 +49,15 @@ change warranting a new major release:
- Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being
transformed in body RF. However, the calibration values are in the body RF. Also fixed the
validity flag of 'mgmVecTotDerivative'.
- Fixed calculation of model sun vector
- Fixed calculation of model magnetic field vector
- Fixed MEKF algorithm
- Fixed several variable initializations
- Fixed several variable types
- Fixed use of `sunMagAngleMin` for safe mode
- Fixed MEKF not using correct `sampleTime`
- Fixed assignment of `SUS0` and `SUS6` calibration matrices due to wiring being mixed up
- Various smaller bugfixes
# [v1.25.0] 2023-02-06

6
mission/acsDefs.h
View File

@ -11,11 +11,11 @@ enum AcsMode {
OFF = HasModesIF::MODE_OFF,
SAFE = 10,
DETUMBLE = 11,
IDLE = 12,
PTG_TARGET_NADIR = 13,
PTG_IDLE = 12,
PTG_NADIR = 13,
PTG_TARGET = 14,
PTG_TARGET_GS = 15,
PTG_TARGET_INERTIAL = 16,
PTG_INERTIAL = 16,
};
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;

194
mission/controller/AcsController.cpp
View File

@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
parameterHelper(this),
mgmDataRaw(this),
mgmDataProcessed(this),
susDataRaw(this),
@ -27,7 +28,25 @@ AcsController::AcsController(object_id_t objectId)
actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
return returnvalue::OK;
ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) {
return result;
}
result = parameterHelper.handleParameterMessage(message);
if (result == returnvalue::OK) {
return result;
}
return result;
}
MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues,
uint16_t startAtIndex) {
return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues,
startAtIndex);
}
void AcsController::performControlOperation() {
@ -52,9 +71,11 @@ void AcsController::performControlOperation() {
case acs::DETUMBLE:
performDetumble();
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_NADIR:
case acs::PTG_TARGET_INERTIAL:
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl();
break;
}
@ -86,10 +107,6 @@ void AcsController::performControlOperation() {
}
void AcsController::performSafe() {
// Concept: SAFE MODE WITH MEKF
// -do the sensor processing, maybe is does make more sense do call this class function in
// another place since we have to do it for every mode regardless of safe or not
ACS::SensorValues sensorValues;
timeval now;
@ -128,10 +145,10 @@ void AcsController::performSafe() {
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
double zeroQuat[4] = {0, 0, 0, 0};
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
double unitQuat[4] = {0, 0, 0, 1};
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
@ -174,7 +191,8 @@ void AcsController::performSafe() {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
// torqueDuration);
// }
}
@ -252,29 +270,146 @@ void AcsController::performPointingCtrl() {
&mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
double quatRef[4] = {0, 0, 0, 0};
uint8_t enableAntiStiction = true;
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
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), torquePtgRws,
rwTrqNs, cmdSpeedRws);
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
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
switch (submode) {
case acs::PTG_IDLE:
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
case acs::PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
4 * sizeof(double));
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break;
}
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);
}
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), mgtDpDes);
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
@ -371,6 +506,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
// MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@ -426,7 +563,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE;
}
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_TARGET_INERTIAL)) {
if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
@ -488,7 +625,6 @@ void AcsController::copyMgmData() {
void AcsController::copySusData() {
{
PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
6 * sizeof(uint16_t));

13
mission/controller/AcsController.h
View File

@ -3,6 +3,8 @@
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
@ -18,12 +20,17 @@
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
class AcsController : public ExtendedControllerBase {
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public:
static constexpr dur_millis_t INIT_DELAY = 500;
AcsController(object_id_t objectId);
MessageQueueId_t getCommandQueue() const;
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
uint16_t startAtIndex) override;
protected:
void performSafe();
void performDetumble();
@ -42,6 +49,8 @@ class AcsController : public ExtendedControllerBase {
uint8_t detumbleCounter;
ParameterHelper parameterHelper;
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
@ -134,6 +143,8 @@ class AcsController : public ExtendedControllerBase {
acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
// MEKF
acsctrl::MekfData mekfData;

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

@ -3,25 +3,23 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <stddef.h>
AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) :
// parameterModuleId(parameterModuleId) {}
AcsParameters::AcsParameters() {}
AcsParameters::~AcsParameters() {}
/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId,
ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
if (domainId == parameterModuleId) {
switch (parameterId >> 8) {
switch (domainId) {
case 0x0: // direct members
switch (parameterId & 0xFF) {
switch (parameterId) {
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x1: // OnBoardParams
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(onBoardParams.sampleTime);
break;
@ -30,19 +28,28 @@ AcsParameters::~AcsParameters() {}
}
break;
case 0x2: // InertiaEIVE
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(inertiaEIVE.inertiaMatrix);
break;
case 0x1:
parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse);
parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed);
break;
case 0x2:
parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed);
break;
case 0x3:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1);
break;
case 0x4:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x3: // MgmHandlingParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
break;
@ -88,12 +95,21 @@ AcsParameters::~AcsParameters() {}
case 0xE:
parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
break;
case 0xF:
parameterWrapper->set(mgmHandlingParameters.mgm02variance);
break;
case 0x10:
parameterWrapper->set(mgmHandlingParameters.mgm13variance);
break;
case 0x11:
parameterWrapper->set(mgmHandlingParameters.mgm4variance);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x4: // SusHandlingParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
break;
@ -202,18 +218,12 @@ AcsParameters::~AcsParameters() {}
case 0x23:
parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
break;
case 0x24:
parameterWrapper->set(susHandlingParameters.filterAlpha);
break;
case 0x25:
parameterWrapper->set(susHandlingParameters.sunThresh);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x5): // GyrHandlingParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
break;
@ -227,38 +237,53 @@ AcsParameters::~AcsParameters() {}
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight);
parameterWrapper->set(gyrHandlingParameters.gyr0bias);
break;
case 0x5:
parameterWrapper->set(gyrHandlingParameters.gyr1bias);
break;
case 0x6:
parameterWrapper->set(gyrHandlingParameters.gyr2bias);
break;
case 0x7:
parameterWrapper->set(gyrHandlingParameters.gyr3bias);
break;
case 0x8:
parameterWrapper->set(gyrHandlingParameters.gyr02variance);
break;
case 0x9:
parameterWrapper->set(gyrHandlingParameters.gyr13variance);
break;
case 0xA:
parameterWrapper->set(gyrHandlingParameters.preferAdis);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x6): // RwHandlingParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(rwHandlingParameters.inertiaWheel);
break;
case 0x5:
case 0x1:
parameterWrapper->set(rwHandlingParameters.maxTrq);
break;
case 0x2:
parameterWrapper->set(rwHandlingParameters.stictionSpeed);
break;
case 0x3:
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
break;
case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x7): // RwMatrices
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(rwMatrices.alignmentMatrix);
break;
@ -277,12 +302,15 @@ AcsParameters::~AcsParameters() {}
case 0x5:
parameterWrapper->set(rwMatrices.without3);
break;
case 0x6:
parameterWrapper->set(rwMatrices.nullspace);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x8): // SafeModeControllerParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(safeModeControllerParameters.k_rate_mekf);
break;
@ -299,92 +327,170 @@ AcsParameters::~AcsParameters() {}
parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
break;
case 0x5:
parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop);
break;
case 0x6:
parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
break;
case 0x7:
parameterWrapper->set(safeModeControllerParameters.satRateRef);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x9): // DetumbleCtrlParameters
switch (parameterId & 0xFF) {
case (0x9): // TargetModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(detumbleCtrlParameters.gainD);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xA): // PointingModeControllerParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.updtFlag);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.A_rw);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.refDirection);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.quatRef);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.zeta);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.zetaLow);
break;
case 0xB:
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.omLow);
break;
case 0xD:
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
break;
case 0xE:
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
break;
case 0xF:
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x10:
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
break;
case 0x11:
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break;
case 0x12:
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.omegaEarth);
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xB): // StrParameters
switch (parameterId & 0xFF) {
case (0xA): // NadirModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(nadirModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(nadirModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(nadirModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(nadirModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(nadirModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(nadirModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(nadirModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xB): // InertialModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(inertialModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(inertialModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(inertialModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(inertialModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(inertialModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(inertialModeControllerParameters.tgtQuat);
break;
case 0xA:
parameterWrapper->set(inertialModeControllerParameters.refRotRate);
break;
case 0xC:
parameterWrapper->set(inertialModeControllerParameters.quatRef);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xC): // StrParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(strParameters.exclusionAngle);
break;
@ -395,60 +501,39 @@ AcsParameters::~AcsParameters() {}
return INVALID_IDENTIFIER_ID;
}
break;
case (0xC): // GpsParameters
switch (parameterId & 0xFF) {
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xD): // GroundStationParameters
switch (parameterId & 0xFF) {
case (0xD): // GpsParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(groundStationParameters.latitudeGs);
break;
case 0x1:
parameterWrapper->set(groundStationParameters.longitudeGs);
break;
case 0x2:
parameterWrapper->set(groundStationParameters.altitudeGs);
break;
case 0x3:
parameterWrapper->set(groundStationParameters.earthRadiusEquat);
break;
case 0x4:
parameterWrapper->set(groundStationParameters.earthRadiusPolar);
parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xE): // SunModelParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(sunModelParameters.useSunModel);
break;
case 0x1:
parameterWrapper->set(sunModelParameters.domega);
break;
case 0x2:
case 0x1:
parameterWrapper->set(sunModelParameters.omega_0);
break;
case 0x3:
case 0x2:
parameterWrapper->set(sunModelParameters.m_0);
break;
case 0x4:
case 0x3:
parameterWrapper->set(sunModelParameters.dm);
break;
case 0x5:
case 0x4:
parameterWrapper->set(sunModelParameters.e);
break;
case 0x6:
case 0x5:
parameterWrapper->set(sunModelParameters.e1);
break;
case 0x7:
case 0x6:
parameterWrapper->set(sunModelParameters.p1);
break;
case 0x8:
case 0x7:
parameterWrapper->set(sunModelParameters.p2);
break;
default:
@ -456,39 +541,23 @@ AcsParameters::~AcsParameters() {}
}
break;
case (0xF): // KalmanFilterParameters
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter);
break;
case 0x1:
parameterWrapper->set(kalmanFilterParameters.requestResetFlag);
break;
case 0x2:
parameterWrapper->set(
kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps);
break;
case 0x3:
parameterWrapper->set(kalmanFilterParameters.processNoiseOmega);
break;
case 0x4:
parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion);
break;
case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
break;
case 0x6:
case 0x1:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
break;
case 0x7:
case 0x2:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
break;
case 0x8:
case 0x3:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
break;
case 0x9:
case 0x4:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
break;
case 0xA:
case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
break;
default:
@ -496,7 +565,7 @@ AcsParameters::~AcsParameters() {}
}
break;
case (0x10): // MagnetorquesParameter
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
break;
@ -520,7 +589,7 @@ AcsParameters::~AcsParameters() {}
}
break;
case (0x11): // DetumbleParameter
switch (parameterId & 0xFF) {
switch (parameterId) {
case 0x0:
parameterWrapper->set(detumbleParameter.detumblecounter);
break;
@ -530,15 +599,15 @@ AcsParameters::~AcsParameters() {}
case 0x2:
parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
break;
case 0x3:
parameterWrapper->set(detumbleParameter.gainD);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
default:
return INVALID_IDENTIFIER_ID;
}
return returnvalue::OK;
} else {
return INVALID_DOMAIN_ID;
}
}*/
return returnvalue::OK;
}

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

@ -12,22 +12,36 @@
typedef unsigned char uint8_t;
class AcsParameters /*: public HasParametersIF*/ {
class AcsParameters : public HasParametersIF {
public:
AcsParameters();
virtual ~AcsParameters();
/*
virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues, uint16_t startAtIndex);
*/
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
uint16_t startAtIndex) override;
struct OnBoardParams {
double sampleTime = 0.1; // [s]
double sampleTime = 0.4; // [s]
} onBoardParams;
struct InertiaEIVE {
double inertiaMatrix[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.5, 1.0}};
double inertiaMatrixInverse[3][3];
double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021
// Possible inertia matrices
double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021
double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008},
{-0.0001798426, 0.162240, 0.000475596},
{-0.005008, 0.000475596, 0.060136}}; // 19.11.2021
double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207},
{-0.0001836122, 0.16619787, 0.0083537},
{-0.00501207, 0.0083537, 0.07192588}}; // 19.11.2021
double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767},
{-0.000178376, 0.166172, -0.007403},
{-0.005009767, -0.007403, 0.07195314}};
} inertiaEIVE;
struct MgmHandlingParameters {
@ -62,17 +76,16 @@ class AcsParameters /*: public HasParametersIF*/ {
float mgm02variance[3] = {1, 1, 1};
float mgm13variance[3] = {1, 1, 1};
float mgm4variance[3] = {1, 1, 1};
} mgmHandlingParameters;
struct SusHandlingParameters {
float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07
float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10
float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM06
float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM13
float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM14
float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM05
float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM02
float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10
float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07
float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM01
float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM03
float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11
@ -80,61 +93,61 @@ class AcsParameters /*: public HasParametersIF*/ {
float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08
float sus0coeffAlpha[9][10] = {
{10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707,
-0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396,
-0.000124096561459262, 0.00040790566176981},
{6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417,
0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415,
1.63114413539632e-05, -2.0187452317724e-05},
{-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334,
0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995,
-0.000249094601806692, 0.000231466668650876},
{-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384,
-0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05,
-9.78534772816957e-06, -4.72083745991256e-05},
{0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541,
-0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463,
0.00010150571088124, -0.000367705461590854},
{3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526,
-0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05,
6.97244631313601e-05, 2.50519145070895e-05},
{-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191,
-0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928,
-1.78466217018177e-05, -0.00058976234038192},
{1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089,
-0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06,
-2.73407888222758e-05, 5.45131881032866e-06},
{43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675,
0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078,
-0.000889196131314391, -0.000509766491897672}};
{13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293,
0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146,
0.00091428505258307, 0.000259857066722932},
{1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859,
0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05,
-1.06065583714065e-05, -1.43899892666699e-05},
{8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577,
0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412,
-4.88501228194031e-06, -0.000434861113274231},
{-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253,
0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05,
6.09619017310129e-05, 4.23908862836885e-05},
{-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607,
0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257,
0.000150795563555828, -0.000279246491927943},
{7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557,
-0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05,
9.13233708460098e-05, -0.000206717750924323},
{16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216,
-0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597,
-0.000532190902882765, 9.36018171121253e-05},
{-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645,
-0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05,
1.02609175615251e-05, -9.64717658954667e-06},
{-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358,
-0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05,
0.000868163357631254, 0.00120099606910313}};
float sus0coeffBeta[9][10] = {
{1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235,
-0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05,
-5.52178824394723e-06, 5.73935295303589e-05},
{3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133,
0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05,
-7.58501977656551e-05, -8.79809730730992e-05},
{14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518,
0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341,
-0.00020861690864945},
{1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335,
-0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06,
-6.00508568552101e-05, 0.000101583822589461},
{-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729,
-0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05,
0.000316835483508523, 0.000151442890664899},
{-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231,
0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05,
-9.83662017231755e-05, 1.87078667116619e-05},
{27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016,
-0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772,
-0.000285438191474895, -5.71327627917386e-05},
{-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328,
-0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05,
-9.77555098179959e-05, 2.09624089449761e-05},
{-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461,
-0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688,
0.000141998709314758, 0.000101051304611037}};
{12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917,
0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05,
0.000821479971871302, -4.5330528329465e-05},
{1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823,
0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05,
-4.04514487800062e-05, -7.6296149087237e-05},
{5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941,
-0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412,
0.0011723869613426, 0.000122882034141316},
{2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172,
-0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05,
-5.87601932564404e-05, -3.92033314627704e-05},
{2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898,
0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264,
0.000259925974231328, 0.000222437797823852},
{1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167,
0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05,
-4.33518182614169e-05, 4.66993119419691e-05},
{-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108,
0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429,
0.000401546410974577, -0.000147563886502726},
{1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231,
0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05,
-5.3309466243918e-05, 6.20289310356821e-06},
{1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785,
0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741,
-0.000186887396585446, 0.00119710704771344}};
float sus1coeffAlpha[9][10] = {
{-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084,
-0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176,
@ -416,61 +429,61 @@ class AcsParameters /*: public HasParametersIF*/ {
-0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245,
-0.000169538153750085, -0.000336529204350355}};
float sus6coeffAlpha[9][10] = {
{13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293,
0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146,
0.00091428505258307, 0.000259857066722932},
{1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859,
0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05,
-1.06065583714065e-05, -1.43899892666699e-05},
{8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577,
0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412,
-4.88501228194031e-06, -0.000434861113274231},
{-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253,
0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05,
6.09619017310129e-05, 4.23908862836885e-05},
{-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607,
0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257,
0.000150795563555828, -0.000279246491927943},
{7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557,
-0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05,
9.13233708460098e-05, -0.000206717750924323},
{16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216,
-0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597,
-0.000532190902882765, 9.36018171121253e-05},
{-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645,
-0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05,
1.02609175615251e-05, -9.64717658954667e-06},
{-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358,
-0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05,
0.000868163357631254, 0.00120099606910313}};
{10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707,
-0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396,
-0.000124096561459262, 0.00040790566176981},
{6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417,
0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415,
1.63114413539632e-05, -2.0187452317724e-05},
{-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334,
0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995,
-0.000249094601806692, 0.000231466668650876},
{-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384,
-0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05,
-9.78534772816957e-06, -4.72083745991256e-05},
{0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541,
-0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463,
0.00010150571088124, -0.000367705461590854},
{3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526,
-0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05,
6.97244631313601e-05, 2.50519145070895e-05},
{-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191,
-0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928,
-1.78466217018177e-05, -0.00058976234038192},
{1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089,
-0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06,
-2.73407888222758e-05, 5.45131881032866e-06},
{43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675,
0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078,
-0.000889196131314391, -0.000509766491897672}};
float sus6coeffBeta[9][10] = {
{12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917,
0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05,
0.000821479971871302, -4.5330528329465e-05},
{1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823,
0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05,
-4.04514487800062e-05, -7.6296149087237e-05},
{5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941,
-0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412,
0.0011723869613426, 0.000122882034141316},
{2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172,
-0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05,
-5.87601932564404e-05, -3.92033314627704e-05},
{2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898,
0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264,
0.000259925974231328, 0.000222437797823852},
{1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167,
0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05,
-4.33518182614169e-05, 4.66993119419691e-05},
{-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108,
0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429,
0.000401546410974577, -0.000147563886502726},
{1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231,
0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05,
-5.3309466243918e-05, 6.20289310356821e-06},
{1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785,
0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741,
-0.000186887396585446, 0.00119710704771344}};
{1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235,
-0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05,
-5.52178824394723e-06, 5.73935295303589e-05},
{3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133,
0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05,
-7.58501977656551e-05, -8.79809730730992e-05},
{14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518,
0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341,
-0.00020861690864945},
{1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335,
-0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06,
-6.00508568552101e-05, 0.000101583822589461},
{-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729,
-0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05,
0.000316835483508523, 0.000151442890664899},
{-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231,
0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05,
-9.83662017231755e-05, 1.87078667116619e-05},
{27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016,
-0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772,
-0.000285438191474895, -5.71327627917386e-05},
{-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328,
-0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05,
-9.77555098179959e-05, 2.09624089449761e-05},
{-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461,
-0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688,
0.000141998709314758, 0.000101051304611037}};
float sus7coeffAlpha[9][10] = {
{-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523,
-0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283,
@ -751,9 +764,6 @@ class AcsParameters /*: public HasParametersIF*/ {
{116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
-0.000889232196185857, -0.00168429567131815}};
float filterAlpha;
float sunThresh;
} susHandlingParameters;
struct GyrHandlingParameters {
@ -761,110 +771,119 @@ class AcsParameters /*: public HasParametersIF*/ {
double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
// 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
double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004};
double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359};
double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
/* 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 gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
enum PreferAdis { NO = 0, YES = 1 };
uint8_t preferAdis = PreferAdis::YES;
uint8_t preferAdis = true;
} gyrHandlingParameters;
struct RwHandlingParameters {
double rw0orientationMatrix[3][3];
double rw1orientationMatrix[3][3];
double rw2orientationMatrix[3][3];
double rw3orientationMatrix[3][3];
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 100; // 80; // RPM
double stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006;
} rwHandlingParameters;
struct RwMatrices {
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
{0.0000, -0.9205, 0.0000, 0.9205},
{0.3907, 0.3907, 0.3907, 0.3907}};
double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597},
{0.2136, -0.3317, 1.0123},
{-0.8672, -0.1406, 0.1778},
{0.6426, 0.4794, 1.3603}};
double without0[4][3];
double without1[4][3];
double without2[4][3];
double without3[4][3];
double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649};
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}};
double without2[4][3] = {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double without3[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;
struct SafeModeControllerParameters {
double k_rate_mekf = 0.00059437;
double k_align_mekf = 0.000056875;
double k_rate_no_mekf;
double k_align_no_mekf;
double sunMagAngleMin;
double k_rate_no_mekf = 0.00059437;
double k_align_no_mekf = 0.000056875;
double sunTargetDir[3] = {1, 0, 0}; // Body frame
double satRateRef[3]; // Body frame
double sunMagAngleMin = 5 * M_PI / 180;
double sunTargetDirLeop[3] = {0, .5, .5};
double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0};
} safeModeControllerParameters;
struct DetumbleCtrlParameters {
double gainD = pow(10.0, -3.3);
} detumbleCtrlParameters;
// ToDo: mutiple structs for different pointing mode controllers?
struct PointingModeControllerParameters {
double updtFlag;
double A_rw[3][12];
double refDirection[3] = {1, 0, 0};
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
struct PointingLawParameters {
double zeta = 0.3;
double zetaLow;
double om = 0.3;
double omLow;
double omMax = 1 * M_PI / 180;
double qiMin = 0.1;
double gainNullspace = 0.01;
double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000;
bool desatOn = true;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
} pointingLawParameters;
double omegaEarth = 0.000072921158553;
struct TargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
// give this as an input- currently en calculation is done
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
} inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeTgt = 500; // [m]
// For one-axis control:
uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
} targetModeControllerParameters;
struct NadirModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters;
struct InertialModeControllerParameters : PointingLawParameters {
double tgtQuat[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
} inertialModeControllerParameters;
struct StrParameters {
double exclusionAngle = 20 * M_PI / 180;
// double strOrientationMatrix[3][3];
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // in body/geometry frame
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
} strParameters;
struct GpsParameters {
double timeDiffVelocityMax = 30; //[s]
} gpsParameters;
struct GroundStationParameters {
double latitudeGs = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeGs = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeGs = 500; // [m] Altitude
double earthRadiusEquat = 6378137; // [m]
double earthRadiusPolar = 6356752.314; // [m]
} groundStationParameters; // Stuttgart
struct SunModelParameters {
enum UseSunModel { NO = 0, YES = 3 };
uint8_t useSunModel;
float domega = 36000.771;
float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee
float m_0 = 357.5256; // coefficients for mean anomaly
float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of
// perigee
float m_0 = 357.5277; // coefficients for mean anomaly
float dm = 35999.049; // coefficients for mean anomaly
float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis
float e1 = 0.74508 * M_PI / 180.;
@ -874,19 +893,13 @@ class AcsParameters /*: public HasParametersIF*/ {
} sunModelParameters;
struct KalmanFilterParameters {
uint8_t activateKalmanFilter;
uint8_t requestResetFlag;
double maxToleratedTimeBetweenKalmanFilterExecutionSteps;
double processNoiseOmega[3];
double processNoiseQuaternion[4];
double sensorNoiseSTR = 0.1 * M_PI / 180;
double sensorNoiseSS = 8 * M_PI / 180;
double sensorNoiseMAG = 4 * M_PI / 180;
double sensorNoiseRMU[3];
double sensorNoiseGYR = 0.1 * M_PI / 180;
double sensorNoiseArwRmu; // Angular Random Walk
double sensorNoiseBsRMU; // Bias Stability
double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters;
struct MagnetorquesParameter {
@ -901,8 +914,8 @@ class AcsParameters /*: public HasParametersIF*/ {
struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2;
double omegaDetumbleEnd = 0.4;
double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4 * M_PI / 180;
double gainD = pow(10.0, -3.3);
} detumbleParameter;
};

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

@ -21,26 +21,27 @@ ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsPa
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);
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] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
@ -50,7 +51,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);
}

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

@ -1,10 +1,3 @@
/*
* ActuatorCmd.h
*
* Created on: 4 Aug 2022
* Author: Robin Marquardt
*/
#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_
@ -18,6 +11,14 @@ class ActuatorCmd {
ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
virtual ~ActuatorCmd();
/*
* @brief: scalingTorqueRws() scales the torque via maximum part in case this part is
* higher then the maximum torque
* @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque
*/
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
/*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
* wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
@ -28,8 +29,7 @@ class ActuatorCmd {
* reaction wheel
*/
void 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);
const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques

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

@ -13,6 +13,8 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <filesystem>
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
@ -22,57 +24,53 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete
Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) {
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
}
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now,
void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
acsParameters.groundStationParameters.longitudeGs,
acsParameters.groundStationParameters.altitudeGs,
groundStationCart);
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
sensorValues->gpsSet.longitude.value,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::dcmEJ(now, *dcmEJ);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
// TEST SECTION !
// double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix,
// dcmTEST, dcmTEST, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
@ -111,9 +109,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0};
velSatE[0] = 0.0; // sensorValues->gps0Velocity[0];
velSatE[1] = 0.0; // sensorValues->gps0Velocity[1];
velSatE[2] = 0.0; // sensorValues->gps0Velocity[2];
std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
@ -134,10 +130,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
// Calculation of reference rotation rate in case of star tracker blinding
//-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirJ[3] = {0, 0, 0};
double sunDirB[3] = {0, 0, 0};
if (susDataProcessed->sunIjkModel.isValid()) {
double sunDirJ[3] = {0, 0, 0};
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else {
@ -183,14 +179,414 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
}
}
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
(timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
if (timeElapsed < timeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[2] = omegaRefNew[2];
} else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
}
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// Transform velocity into inertial frame
double velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
// orbital normal vector
double orbitalNormalJ[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-Axis completes RHS
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get Sun Vector Model in ECI
double sunJ[3];
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
VectorOperations<double>::normalize(sunJ, sunJ, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
double sunParallel[3], zAxis[3];
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// calculate y-axis
double yAxis[3];
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
double quatBJ[4] = {0, 0, 0, 0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
if (susDataProcessed->sunIjkModel.isValid()) {
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else if (susDataProcessed->susVecTot.isValid()) {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
} else {
return;
}
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// positive z-Axis of EIVE in direction of sun
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
// Assign helper vector (north pole inertial)
double helperVec[3] = {0, 0, 1};
//
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
//
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
// Transformation matrix to Sun, no further transforamtions, reference is already
// the EIVE body frame
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatSun[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatSun);
targetQuat[0] = quatSun[0];
targetQuat[1] = quatSun[1];
targetQuat[2] = quatSun[2];
targetQuat[3] = quatSun[3];
//----------------------------------------------------------------------------
// Calculation of reference rotation rate
//----------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4],
double refSatRate[3]) { // old version of Nadir Pointing
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Target Direction in the body frame
double targetDirJ[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
// negative x-Axis aligned with target (Camera position)
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// z-Axis parallel to long side of picture resolution
double zAxis[3] = {0, 0, 0}, velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
// Complete transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
4 * sizeof(double));
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
3 * sizeof(double));
}
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]) {
double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
@ -210,8 +606,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
quatError[1] = quatErrorComplete[1];
quatError[2] = quatErrorComplete[2];
// target flag in matlab, importance, does look like it only gives
// feedback if pointing control is under 150 arcsec ??
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
}
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
@ -300,7 +696,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *
// @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 the pointing control is destined to fail.
// 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];

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

@ -21,16 +21,49 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
// Function to get the target quaternion and refence rotation rate from gps position and position
// of the ground station
void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
// Function to get the target quaternion and refence rotation rate from gps position and
// position of the ground station
void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]);
void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]);
void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground
// station
void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing
void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]);
void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial
// pointing
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]);
void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"
@ -39,6 +72,12 @@ class Guidance {
private:
AcsParameters acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";
};
#endif /* ACS_GUIDANCE_H_ */

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

@ -1,22 +1,19 @@
/*
* Igrf13Model.cpp
*
* Created on: 10 Mar 2022
* Author: Robin Marquardt
*/
#include "Igrf13Model.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <cmath>
#include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {}
@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
double* magFieldModelInertial) {
double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/
theta -= 90 * Math::PI / 180;
theta -= 90 * PI / 180;
theta *= (-1);
double rE = 6371200.0; // radius earth [m]
@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
/* Calculation of Legendre Polynoms (normalised) */
if (n == m) {
P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 - cos(theta) * P11;
dP2 = sin(theta) * dP11 + cos(theta) * P11;
P11 = P2;
P10 = P11;
P20 = 0;
@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[0] +=
pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
/* gradient of scalar potential towards phi */
/* gradient of scalar potential towards theta */
magFieldModel[1] +=
pow(rE / (altitude + rE), (n + 2)) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
/* gradient of scalar potential towards theta */
/* gradient of scalar potential towards phi */
magFieldModel[2] +=
pow(rE / (altitude + rE), (n + 2)) *
((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
@ -85,31 +82,30 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta));
/* Next step: transform into inertial KOS (IJK)*/
// Julean Centuries
double JD2000Floor = 0;
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
JD2000Floor = floor(JD2000);
double JC2000 = JD2000Floor / 36525;
double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
0.000000026 * pow(JC2000, 3); // greenwich sidereal time
gst *= PI / 180; // convert to radians
double sec =
(JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ?
double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
gst += omega0 * sec;
double UT1 = JD2000 / 36525.;
double gst =
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
gst = std::fmod(gst, 360.);
gst *= PI / 180.;
double lst = gst + longitude; // local sidereal time [rad]
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) +
magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst);
magFieldModelInertial[1] = magFieldModel[0] * cos(theta) +
magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst);
magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
magFieldModelInertial[0] =
(magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
magFieldModel[2] * sin(lst);
magFieldModelInertial[1] =
(magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) +
magFieldModel[2] * cos(lst);
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;
}
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
@ -123,3 +119,34 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
}
}
}
void Igrf13Model::schmidtNormalization() {
double kronDelta = 0;
schmidtFactors[0][0] = 1;
for (int n = 1; n <= igrfOrder; n++) {
if (n == 1) {
schmidtFactors[0][n - 1] = 1;
} else {
schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n;
}
for (int m = 1; m <= igrfOrder; m++) {
if (m == 1) {
kronDelta = 1;
} else {
kronDelta = 0;
}
schmidtFactors[m][n - 1] =
schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m));
}
}
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {
coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j];
coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j];
svG[i][j] = schmidtFactors[i][j] * svG[i][j];
svH[i][j] = schmidtFactors[i][j] * svH[i][j];
}
}
}

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

@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ {
* - timeOfMagMeasurement: time of actual measurement [s]
*
* Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
* - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3];
void schmidtNormalization();
private:
const double coeffG[14][13] = {
double coeffG[14][13] = {
{-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
{-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
{0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT
const double coeffH[14][13] = {
double coeffH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0},
{4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
{0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT
const double svG[14][13] = {
double svG[14][13] = {
{5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
const double svH[14][13] = {
double svH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -114,6 +115,16 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
double schmidtFactors[14][13] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
;
double updatedG[14][13];
double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation

328
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -1,10 +1,3 @@
/*
* MultiplicativeKalmanFilter.cpp
*
* Created on: 4 Feb 2022
* Author: rooob
*/
#include "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h>
@ -14,6 +7,8 @@
#include <stdio.h>
#include <string.h>
#include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
@ -29,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
}
void MultiplicativeKalmanFilter::reset() {}
@ -41,14 +36,11 @@ void MultiplicativeKalmanFilter::init(
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = 0.1 * 3.141 / 180; // acs parameters
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -136,7 +128,6 @@ void MultiplicativeKalmanFilter::init(
matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
/* vector*transpose(vector)*/
MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
@ -199,8 +190,6 @@ void MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2];
// initialCovarianceMatrix[][] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},
//{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
} else {
// no initialisation possible, no valid measurements
validInit = false;
@ -208,14 +197,15 @@ void MultiplicativeKalmanFilter::init(
}
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
const double *rateGYRs_, const bool validGYRs_,
const double *magneticField_,
const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ,
const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime,
acsctrl::MekfData *mekfData) {
// Check for GYR Measurements
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
{
@ -960,10 +950,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
// double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
// double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = 3 * 3.141 / 180 / 3600;
double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -977,170 +965,135 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// Discrete Process Noise Covariance Q
double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double covQ1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < 3.141 / 10) {
double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3);
MatrixOperations<double>::transpose(*covQ2, *transCovQ2, 3);
double fact3 = sampleTime * pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3);
double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
discProcessNoiseCov[0][0] = covQ1[0][0];
discProcessNoiseCov[0][1] = covQ1[0][1];
discProcessNoiseCov[0][2] = covQ1[0][2];
discProcessNoiseCov[0][3] = covQ2[0][0];
discProcessNoiseCov[0][4] = covQ2[0][1];
discProcessNoiseCov[0][5] = covQ2[0][2];
discProcessNoiseCov[1][0] = covQ1[1][0];
discProcessNoiseCov[1][1] = covQ1[1][1];
discProcessNoiseCov[1][2] = covQ1[1][2];
discProcessNoiseCov[1][3] = covQ2[1][0];
discProcessNoiseCov[1][4] = covQ2[1][1];
discProcessNoiseCov[1][5] = covQ2[1][2];
discProcessNoiseCov[2][0] = covQ1[2][0];
discProcessNoiseCov[2][1] = covQ1[2][1];
discProcessNoiseCov[2][2] = covQ1[2][2];
discProcessNoiseCov[2][3] = covQ2[2][0];
discProcessNoiseCov[2][4] = covQ2[2][1];
discProcessNoiseCov[2][5] = covQ2[2][2];
discProcessNoiseCov[3][0] = transCovQ2[0][0];
discProcessNoiseCov[3][1] = transCovQ2[0][1];
discProcessNoiseCov[3][2] = transCovQ2[0][2];
discProcessNoiseCov[3][3] = covQ3[0][0];
discProcessNoiseCov[3][4] = covQ3[0][1];
discProcessNoiseCov[3][5] = covQ3[0][2];
discProcessNoiseCov[4][0] = transCovQ2[1][0];
discProcessNoiseCov[4][1] = transCovQ2[1][1];
discProcessNoiseCov[4][2] = transCovQ2[1][2];
discProcessNoiseCov[4][3] = covQ3[1][0];
discProcessNoiseCov[4][4] = covQ3[1][1];
discProcessNoiseCov[4][5] = covQ3[1][2];
discProcessNoiseCov[5][0] = transCovQ2[2][0];
discProcessNoiseCov[5][1] = transCovQ2[2][1];
discProcessNoiseCov[5][2] = transCovQ2[2][2];
discProcessNoiseCov[5][3] = covQ3[2][0];
discProcessNoiseCov[5][4] = covQ3[2][1];
discProcessNoiseCov[5][5] = covQ3[2][2];
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
double fact22 = pow(sigmaU, 2) * sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else {
// double fact1 = sampleTime*pow(sigmaV,2);
double covQ11[3][3], covQ12[3][3], covQ13[3][3];
// MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) -
pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3);
double fact2 = pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3);
MatrixOperations<double>::subtract(*covQ12, *covQ11, *covQ11, 3, 3);
double fact3 = sampleTime * pow(sigmaV, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3);
MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3);
double fact22 = pow(sigmaU, 2) * sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
double covQ21[3][3], covQ22[3][3], covQ23[3][3];
double fact4 =
(0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3);
double fact5 = 0.5 * pow(sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3);
MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3);
double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3);
MatrixOperations<double>::subtract(*covQ23, *covQ21, *covQ21, 3, 3);
double fact7 = pow(sigmaU, 2);
MatrixOperations<double>::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
MatrixOperations<double>::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3);
MatrixOperations<double>::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3);
MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3);
discProcessNoiseCov[0][0] = covQ1[0][0];
discProcessNoiseCov[0][1] = covQ1[0][1];
discProcessNoiseCov[0][2] = covQ1[0][2];
discProcessNoiseCov[0][3] = covQ2[0][0];
discProcessNoiseCov[0][4] = covQ2[0][1];
discProcessNoiseCov[0][5] = covQ2[0][2];
discProcessNoiseCov[1][0] = covQ1[1][0];
discProcessNoiseCov[1][1] = covQ1[1][1];
discProcessNoiseCov[1][2] = covQ1[1][2];
discProcessNoiseCov[1][3] = covQ2[1][0];
discProcessNoiseCov[1][4] = covQ2[1][1];
discProcessNoiseCov[1][5] = covQ2[1][2];
discProcessNoiseCov[2][0] = covQ1[2][0];
discProcessNoiseCov[2][1] = covQ1[2][1];
discProcessNoiseCov[2][2] = covQ1[2][2];
discProcessNoiseCov[2][3] = covQ2[2][0];
discProcessNoiseCov[2][4] = covQ2[2][1];
discProcessNoiseCov[2][5] = covQ2[2][2];
discProcessNoiseCov[3][0] = covQ2[0][0];
discProcessNoiseCov[3][1] = covQ2[0][1];
discProcessNoiseCov[3][2] = covQ2[0][2];
discProcessNoiseCov[3][3] = covQ3[0][0];
discProcessNoiseCov[3][4] = covQ3[0][1];
discProcessNoiseCov[3][5] = covQ3[0][2];
discProcessNoiseCov[4][0] = covQ2[1][0];
discProcessNoiseCov[4][1] = covQ2[1][1];
discProcessNoiseCov[4][2] = covQ2[1][2];
discProcessNoiseCov[4][3] = covQ3[1][0];
discProcessNoiseCov[4][4] = covQ3[1][1];
discProcessNoiseCov[4][5] = covQ3[1][2];
discProcessNoiseCov[5][0] = covQ2[2][0];
discProcessNoiseCov[5][1] = covQ2[2][1];
discProcessNoiseCov[5][2] = covQ2[2][2];
discProcessNoiseCov[5][3] = covQ3[2][0];
discProcessNoiseCov[5][4] = covQ3[2][1];
discProcessNoiseCov[5][5] = covQ3[2][2];
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
double fact11_1 = 1. / 3. * pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3);
MatrixOperations<double>::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3);
}
discProcessNoiseCov[0][0] = covQ11[0][0];
discProcessNoiseCov[0][1] = covQ11[0][1];
discProcessNoiseCov[0][2] = covQ11[0][2];
discProcessNoiseCov[0][3] = covQ12[0][0];
discProcessNoiseCov[0][4] = covQ12[0][1];
discProcessNoiseCov[0][5] = covQ12[0][2];
discProcessNoiseCov[1][0] = covQ11[1][0];
discProcessNoiseCov[1][1] = covQ11[1][1];
discProcessNoiseCov[1][2] = covQ11[1][2];
discProcessNoiseCov[1][3] = covQ12[1][0];
discProcessNoiseCov[1][4] = covQ12[1][1];
discProcessNoiseCov[1][5] = covQ12[1][2];
discProcessNoiseCov[2][0] = covQ11[2][0];
discProcessNoiseCov[2][1] = covQ11[2][1];
discProcessNoiseCov[2][2] = covQ11[2][2];
discProcessNoiseCov[2][3] = covQ12[2][0];
discProcessNoiseCov[2][4] = covQ12[2][1];
discProcessNoiseCov[2][5] = covQ12[2][2];
discProcessNoiseCov[3][0] = covQ12trans[0][0];
discProcessNoiseCov[3][1] = covQ12trans[0][1];
discProcessNoiseCov[3][2] = covQ12trans[0][2];
discProcessNoiseCov[3][3] = covQ22[0][0];
discProcessNoiseCov[3][4] = covQ22[0][1];
discProcessNoiseCov[3][5] = covQ22[0][2];
discProcessNoiseCov[4][0] = covQ12trans[1][0];
discProcessNoiseCov[4][1] = covQ12trans[1][1];
discProcessNoiseCov[4][2] = covQ12trans[1][2];
discProcessNoiseCov[4][3] = covQ22[1][0];
discProcessNoiseCov[4][4] = covQ22[1][1];
discProcessNoiseCov[4][5] = covQ22[1][2];
discProcessNoiseCov[5][0] = covQ12trans[2][0];
discProcessNoiseCov[5][1] = covQ12trans[2][1];
discProcessNoiseCov[5][2] = covQ12trans[2][2];
discProcessNoiseCov[5][3] = covQ22[2][0];
discProcessNoiseCov[5][4] = covQ22[2][1];
discProcessNoiseCov[5][5] = covQ22[2][2];
// State Transition Matrix phi
double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11[3][3], phi12[3][3];
double fact1 = sin(normRotEst * sampleTime);
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3);
double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3);
MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3);
double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
MatrixOperations<double>::add(*phi11_01, *phi11_2, *phi11, 3, 3);
double phi21[3][3], phi22[3][3];
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3);
double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 3, 3);
double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
MatrixOperations<double>::subtract(*phi12_01, *phi12_2, *phi12, 3, 3);
phi[0][0] = phi1[0][0];
phi[0][1] = phi1[0][1];
phi[0][2] = phi1[0][2];
phi[0][3] = phi2[0][0];
phi[0][4] = phi2[0][1];
phi[0][5] = phi2[0][2];
phi[1][0] = phi1[1][0];
phi[1][1] = phi1[1][1];
phi[1][2] = phi1[1][2];
phi[1][3] = phi2[1][0];
phi[1][4] = phi2[1][1];
phi[1][5] = phi2[1][2];
phi[2][0] = phi1[2][0];
phi[2][1] = phi1[2][1];
phi[2][2] = phi1[2][2];
phi[2][3] = phi2[2][0];
phi[2][4] = phi2[2][1];
phi[2][5] = phi2[2][2];
phi[0][0] = phi11[0][0];
phi[0][1] = phi11[0][1];
phi[0][2] = phi11[0][2];
phi[0][3] = phi12[0][0];
phi[0][4] = phi12[0][1];
phi[0][5] = phi12[0][2];
phi[1][0] = phi11[1][0];
phi[1][1] = phi11[1][1];
phi[1][2] = phi11[1][2];
phi[1][3] = phi12[1][0];
phi[1][4] = phi12[1][1];
phi[1][5] = phi12[1][2];
phi[2][0] = phi11[2][0];
phi[2][1] = phi11[2][1];
phi[2][2] = phi11[2][2];
phi[2][3] = phi12[2][0];
phi[2][4] = phi12[2][1];
phi[2][5] = phi12[2][2];
// Propagated Quaternion
double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
@ -1148,25 +1101,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3);
MatrixOperations<double>::subtract(*omega1, *skewSin, *omega1, 3, 3);
double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]},
{omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]},
{omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]},
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3);
double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]},
{subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]},
{subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]},
{-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
// Update Covariance Matrix
double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6];
double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6];
MatrixOperations<double>::transpose(*phi, *transPhi, 6);
MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6);
MatrixOperations<double>::multiply(*phi, *cov0, *cov0, 6, 6, 6);
MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6);
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
MatrixOperations<double>::transpose(*phi, *transPhi, 6);
MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6);
MatrixOperations<double>::multiply(*phi, *cov2, *cov2, 6, 6, 6);
MatrixOperations<double>::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6);
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
validMekf = true;
// Discrete Time Step

3
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -61,7 +61,7 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData);
const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
// Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
@ -79,7 +79,6 @@ class MultiplicativeKalmanFilter {
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4];
bool validInit;
double sampleTime = 0.1;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/

2
mission/controller/acs/Navigation.cpp
View File

@ -39,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(),
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
mekfData); // VALIDS FOR QUAT AND RATE ??
} else {
multiplicativeKalmanFilter.init(

202
mission/controller/acs/SensorProcessing.cpp
View File

@ -1,3 +1,10 @@
/*
* SensorProcessing.cpp
*
* Created on: 7 Mar 2022
* Author: Robin Marquardt
*/
#include "SensorProcessing.h"
#include <fsfw/datapool/PoolReadGuard.h>
@ -14,8 +21,7 @@
using namespace Math;
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
: savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
SensorProcessing::~SensorProcessing() {}
@ -27,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
Igrf13Model igrf13;
// So the line above should not be done here. Update: Can be done here as long updated coffs
// stored in acsParameters ?
igrf13.schmidtNormalization();
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
}
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
float zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float));
mgmDataProcessed->setValidity(false, true);
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
}
}
return;
@ -134,19 +156,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
Igrf13Model igrf13;
// So the line above should not be done here. Update: Can be done here as long updated coffs
// stored in acsParameters ?
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
}
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -182,6 +191,25 @@ void SensorProcessing::processSus(
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
// Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0};
double JC2000 = JD2000 / 36525.;
double meanLongitude =
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
sunModelParameters->p2 * sin(2 * meanAnomaly);
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
if (sus0valid) {
sus0valid = susConverter.checkSunSensorData(sus0Value);
}
@ -224,22 +252,24 @@ void SensorProcessing::processSus(
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
float zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
susDataProcessed->setValidity(false, true);
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
susDataProcessed->sunIjkModel.setValid(true);
}
}
return;
@ -258,16 +288,6 @@ void SensorProcessing::processSus(
susParameters->sus0coeffBeta),
sus0VecBody, 3, 3, 1);
}
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
susDataProcessed->sus0vec.setValid(sus0valid);
if (!sus0valid) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
}
}
}
if (sus1valid) {
MatrixOperations<float>::multiply(
susParameters->sus1orientationMatrix[0],
@ -275,16 +295,6 @@ void SensorProcessing::processSus(
susParameters->sus1coeffBeta),
sus1VecBody, 3, 3, 1);
}
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
susDataProcessed->sus1vec.setValid(sus1valid);
if (!sus1valid) {
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
}
}
}
if (sus2valid) {
MatrixOperations<float>::multiply(
susParameters->sus2orientationMatrix[0],
@ -389,30 +399,10 @@ void SensorProcessing::processSus(
for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i];
susVecTotDerivativeValid = true;
}
}
timeOfSavedSusDirEst = timeOfSusMeasurement;
/* -------- Sun Model Direction (IJK frame) ------- */
// if (useSunModel) eventuell
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
// Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0};
double JC2000 = JD2000 / 36525;
double meanLongitude =
(sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
sunModelParameters->p2 * sin(2 * meanAnomaly);
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -469,6 +459,7 @@ void SensorProcessing::processGyr(
{
PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
double zeroVector[3] = {0.0, 0.0, 0.0};
std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
@ -488,6 +479,8 @@ void SensorProcessing::processGyr(
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -497,6 +490,8 @@ void SensorProcessing::processGyr(
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -506,6 +501,8 @@ void SensorProcessing::processGyr(
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -515,6 +512,8 @@ void SensorProcessing::processGyr(
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -525,7 +524,7 @@ void SensorProcessing::processGyr(
// take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion
double gyrVecTot[3] = {0.0, 0.0, 0.0};
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) {
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) {
double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
@ -552,30 +551,45 @@ void SensorProcessing::processGyr(
}
}
void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
const double gpsAltitude, const double gpsUnixSeconds,
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process
double gdLongitude, gcLatitude;
double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gps0longitude * PI / 180;
double latitudeRad = gps0latitude * PI / 180;
gdLongitude = gpsLongitude * PI / 180.;
double latitudeRad = gpsLatitude * PI / 180.;
double eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad));
// validGcLatitude = true;
// Calculation of the satellite velocity in earth fixed frame
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
if (validSavedPosSatE &&
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
}
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
}
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->setValidity(validGps, validGps);
if (!validGps) {
gpsDataProcessed->gdLongitude.value = 0.0;
gpsDataProcessed->gcLatitude.value = 0.0;
}
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true);
}
}
}
@ -587,10 +601,12 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) {
sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
processGps(
sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()),
gpsDataProcessed);
sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()),
&acsParameters->gpsParameters, gpsDataProcessed);
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),

15
mission/controller/acs/SensorProcessing.h
View File

@ -65,17 +65,20 @@ class SensorProcessing {
void processStr();
void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
const double gpsUnixSeconds, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed);
double savedMgmVecTot[3];
double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3];
double savedSusVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedSusDirEst;
bool validMagField;
bool validGcLatitude;
bool validMagField = false;
const float zeroVector[3] = {0.0, 0.0, 0.0};
double savedPosSatE[3] = {0.0, 0.0, 0.0};
double timeOfSavedPosSatE = 0.0;
bool validSavedPosSatE = false;
SusConverter susConverter;
AcsParameters acsParameters;

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

@ -1,12 +1,12 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_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/GyroL3GD20Definitions.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"

16
mission/controller/acs/control/Detumble.cpp
View File

@ -22,7 +22,7 @@ Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParamet
Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
detumbleParameter = &(acsParameters_->detumbleParameter);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
@ -31,7 +31,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleCtrlParameters->gainD;
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK;
@ -50,3 +50,15 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
return returnvalue::OK;
}
ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
const double *magField, const bool *magFieldValid,
double *magMom) {
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
return returnvalue::OK;
}

9
mission/controller/acs/control/Detumble.h
View File

@ -25,7 +25,7 @@ class Detumble {
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class
/* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
@ -35,8 +35,13 @@ class Detumble {
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom);
private:
AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters;
AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter *magnetorquesParameter;
};

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

@ -21,21 +21,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws) {
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *qError, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
double om = pointingLawParameters->om;
double zeta = pointingLawParameters->zeta;
double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingLawParameters->omMax;
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);
@ -104,12 +104,14 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
mgtDpDes[0] = 0;
mgtDpDes[1] = 0;
mgtDpDes[2] = 0;
@ -127,17 +129,18 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
factor = (pointingLawParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};
@ -149,7 +152,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
wheelMomentum, 4);
double gainNs = pointingModeControllerParameters->gainNullspace;
double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
*nullSpaceMatrix, 4);
@ -157,3 +160,32 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
double *torqueCommand) {
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
(omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
}
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}

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

@ -33,7 +33,7 @@ class PtgCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class
/* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
@ -41,21 +41,32 @@ class PtgCtrl {
/* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws);
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes);
void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
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
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0};
};
#endif /* ACS_CONTROL_PTGCTRL_H_ */

11
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -87,7 +87,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
return returnvalue::OK;
}
// Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
// Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef,
@ -127,12 +127,9 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
double sinAngle = 0;
sinAngle = sin(acos(cos(cosAngleSunMag)));
if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
* is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
return;
}

36
mission/controller/acs/util/MathOperations.h
View File

@ -96,8 +96,16 @@ class MathOperations {
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
T2 *cartesianOutput) {
double radiusPolar = 6378137;
double radiusEqua = 6356752.314;
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
* longitude and altitude
* @param: lat geodetic latitude [rad]
* longi longitude [rad]
* alt altitude [m]
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
* Landis Markley and John L. Crassidis*/
double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
@ -106,13 +114,13 @@ class MathOperations {
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
}
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* Landis Markley and John L. Crassidis*/
static void dcmEJ(timeval time, T1 *outputDcmEJ) {
double JD2000Floor = 0;
double JD2000 = convertUnixToJD2000(time);
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
@ -143,6 +151,16 @@ class MathOperations {
outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
}
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
@ -215,7 +233,7 @@ class MathOperations {
precession[2][2] = cos(theta2);
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Nutation size
// Calculation of Transformation from earth Nutation N
//-------------------------------------------------------------------------------------
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// lunar asc node
@ -230,7 +248,6 @@ class MathOperations {
// % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
;
nutation[0][0] = cos(dp);
nutation[1][0] = cos(e + de) * sin(dp);
@ -260,10 +277,9 @@ class MathOperations {
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
}
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
int i, j;
double determinant;
double determinant = 0;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
{matrix[3], matrix[4], matrix[5]},
{matrix[6], matrix[7], matrix[8]}};
@ -272,8 +288,8 @@ class MathOperations {
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
}
// cout<<"\size\ndeterminant: "<<determinant;
// cout<<"\size\nInverse of matrix is: \size";
// cout<<"\n\ndeterminant: "<<determinant;
// cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -

8
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -86,6 +86,8 @@ enum PoolIds : lp_id_t {
// GPS Processed
GC_LATITUDE,
GD_LONGITUDE,
GPS_POSITION,
GPS_VELOCITY,
// MEKF
SAT_ROT_RATE_MEKF,
QUAT_MEKF,
@ -99,13 +101,13 @@ enum PoolIds : lp_id_t {
MTQ_TARGET_DIPOLE,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10;
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8;
static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@ -224,6 +226,8 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
private:
};

41
mission/system/tree/acsModeTree.cpp
View File

@ -55,13 +55,13 @@ auto ACS_TABLE_SAFE_TRANS_0 =
auto ACS_TABLE_SAFE_TRANS_1 =
std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::IDLE, FixedArrayList<ModeListEntry, 3>());
auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_IDLE_TGT =
std::make_pair((acs::AcsMode::IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>());
std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_IDLE_TRANS_0 =
std::make_pair((acs::AcsMode::IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>());
std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_IDLE_TRANS_1 =
std::make_pair((acs::AcsMode::IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>());
std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>());
auto ACS_TABLE_PTG_TRANS_0 =
std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>());
@ -81,18 +81,18 @@ auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 =
std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_NADIR =
std::make_pair(acs::AcsMode::PTG_TARGET_NADIR, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsMode::PTG_TARGET_NADIR << 24) | 1,
FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair(
(acs::AcsMode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());
std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_NADIR_TGT =
std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 =
std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_INERTIAL =
std::make_pair(acs::AcsMode::PTG_TARGET_INERTIAL, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair(
(acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair(
(acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());
std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT =
std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 =
std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());
void satsystem::acs::init() {
ModeListEntry entry;
@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc);
};
// Build IDLE target
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TGT.second);
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
@ -307,7 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
// Build IDLE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TRANS_1.second);
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second);
ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true);
// Build IDLE sequence
@ -350,8 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET,
ACS_TABLE_PTG_TARGET_TRANS_1.second);
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false,
true),
ctxc);
@ -399,7 +398,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_NADIR,
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR,
ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)),
@ -485,7 +484,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL,
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
@ -498,7 +497,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL,
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)),