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

Final Version of the ACS Controller #367

Merged
muellerr merged 78 commits from eggert/acs into develop 2023-02-08 13:50:11 +01:00
Conversation 0 Commits 78 Files Changed 27 +2011 -1234
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 ## Changed
- Readded calibration matrices for MGM calibration. - 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 ## Fixed
@ -32,6 +49,15 @@ change warranting a new major release:
- Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being - 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 transformed in body RF. However, the calibration values are in the body RF. Also fixed the
validity flag of 'mgmVecTotDerivative'. 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 # [v1.25.0] 2023-02-06

2
linux/devices/GpsHyperionLinuxController.cpp
View File

@ -157,7 +157,7 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
} }
}; };
// GPS is off, no point in reading data from GPSD. // GPS is off, no point in reading data from GPSD.
if(mode == MODE_OFF) { if (mode == MODE_OFF) {
return false; return false;
} }
if (readMode == ReadModes::SOCKET) { if (readMode == ReadModes::SOCKET) {

6
mission/acsDefs.h
View File

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

198
mission/controller/AcsController.cpp
View File

@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
detumble(&acsParameters), detumble(&acsParameters),
ptgCtrl(&acsParameters), ptgCtrl(&acsParameters),
detumbleCounter{0}, detumbleCounter{0},
parameterHelper(this),
mgmDataRaw(this), mgmDataRaw(this),
mgmDataProcessed(this), mgmDataProcessed(this),
susDataRaw(this), susDataRaw(this),
@ -27,7 +28,25 @@ AcsController::AcsController(object_id_t objectId)
actuatorCmdData(this) {} actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { 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() { void AcsController::performControlOperation() {
@ -52,9 +71,11 @@ void AcsController::performControlOperation() {
case acs::DETUMBLE: case acs::DETUMBLE:
performDetumble(); performDetumble();
break; break;
case acs::PTG_IDLE:
case acs::PTG_TARGET: case acs::PTG_TARGET:
case acs::PTG_TARGET_NADIR: case acs::PTG_TARGET_GS:
case acs::PTG_TARGET_INERTIAL: case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl(); performPointingCtrl();
break; break;
} }
@ -86,10 +107,6 @@ void AcsController::performControlOperation() {
} }
void AcsController::performSafe() { 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; ACS::SensorValues sensorValues;
timeval now; timeval now;
@ -128,10 +145,10 @@ void AcsController::performSafe() {
{ {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
double zeroQuat[4] = {0, 0, 0, 0}; double unitQuat[4] = {0, 0, 0, 1};
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false); 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.errQuat.setValid(false);
ctrlValData.errAng.value = errAng; ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true); ctrlValData.errAng.setValid(true);
@ -174,7 +191,8 @@ void AcsController::performSafe() {
// PoolReadGuard pg(&dipoleSet); // PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock()); // MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true; // 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); &mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; 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}, double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed 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}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
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 mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol 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) {
&(sensorValues.rw2Set.currSpeed.value), case acs::PTG_IDLE:
&(sensorValues.rw3Set.currSpeed.value), guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes); 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), torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
@ -371,6 +506,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// GPS Processed // GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); 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}); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
// MEKF // MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@ -426,7 +563,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE; return INVALID_SUBMODE;
} }
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { } 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; return INVALID_SUBMODE;
} else { } else {
return returnvalue::OK; return returnvalue::OK;
@ -488,7 +625,6 @@ void AcsController::copyMgmData() {
void AcsController::copySusData() { void AcsController::copySusData() {
{ {
PoolReadGuard pg(&sensorValues.susSets[0]); PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
6 * sizeof(uint16_t)); 6 * sizeof(uint16_t));

13
mission/controller/AcsController.h
View File

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

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

File diff suppressed because it is too large Load Diff

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

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

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

@ -21,26 +21,27 @@ ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsPa
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
using namespace Math;
// Scaling the commanded torque to a maximum value // Scaling the commanded torque to a maximum value
double torque[4] = {0, 0, 0, 0};
double maxTrq = acsParameters.rwHandlingParameters.maxTrq; double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
double maxValue = 0; double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ? for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(torque[i]) > maxValue) { if (abs(rwTrq[i]) > maxValue) {
maxValue = abs(torque[i]); maxValue = abs(rwTrq[i]);
} }
} }
if (maxValue > maxTrq) { if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue; 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 // Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; 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 double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s] // W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm; double factor = commandTime / inertiaWheel * radToRpm;
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 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_ #ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_ #define ACTUATORCMD_H_
@ -18,6 +11,14 @@ class ActuatorCmd {
ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
virtual ~ActuatorCmd(); 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 * @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 * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
@ -28,8 +29,7 @@ class ActuatorCmd {
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS, const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
double *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @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 <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> #include <math.h>
#include <filesystem>
#include "string.h" #include "string.h"
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
@ -22,57 +24,53 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete
Guidance::~Guidance() {} Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) { if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; 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,
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); 3 * sizeof(double));
} }
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, acsctrl::SusDataProcessed *susDataProcessed,
double targetQuat[4], double refSatRate[3]) { 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) // fixed/centered frame)
double groundStationCart[3] = {0, 0, 0}; double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.groundStationParameters.longitudeGs, acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.groundStationParameters.altitudeGs, acsParameters.targetModeControllerParameters.longitudeTgt,
groundStationCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value, double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
sensorValues->gpsSet.longitude.value, double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE); sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame // Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0}; 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 // Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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 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}}; 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 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); MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame // Transformation between ECEF and Body frame
@ -111,9 +109,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
// Calculation of reference rotation rate // Calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0}; double velSatE[3] = {0, 0, 0};
velSatE[0] = 0.0; // sensorValues->gps0Velocity[0]; std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
velSatE[1] = 0.0; // sensorValues->gps0Velocity[1];
velSatE[2] = 0.0; // sensorValues->gps0Velocity[2];
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; 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 // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); 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 // Calculation of reference rotation rate in case of star tracker blinding
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) { if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirJ[3] = {0, 0, 0};
double sunDirB[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0};
if (susDataProcessed->sunIjkModel.isValid()) { if (susDataProcessed->sunIjkModel.isValid()) {
double sunDirJ[3] = {0, 0, 0};
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
} else { } 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], void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { double *refSatRate) {
double quatRef[4] = {0, 0, 0, 0}; //-------------------------------------------------------------------------------------
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; // Calculation of reference rotation rate
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; //-------------------------------------------------------------------------------------
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; (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}; double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3); 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[1] = quatErrorComplete[1];
quatError[2] = quatErrorComplete[2]; quatError[2] = quatErrorComplete[2];
// target flag in matlab, importance, does look like it only gives // target flag in matlab, importance, does look like it only gives feedback if pointing control is
// feedback if pointing control is under 150 arcsec ?? // under 150 arcsec ??
} }
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { 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. // @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?! // 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 // 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[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];

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

@ -21,16 +21,49 @@ class Guidance {
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
// Function to get the target quaternion and refence rotation rate from gps position and position // Function to get the target quaternion and refence rotation rate from gps position and
// of the ground station // position of the ground station
void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], acsctrl::GpsDataProcessed *gpsDataProcessed,
double refSatRate[3]); 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 // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired // desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]); 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 // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h" // reation wheel maybe can be done in "commanding.h"
@ -39,6 +72,12 @@ class Guidance {
private: private:
AcsParameters acsParameters; AcsParameters acsParameters;
bool strBlindAvoidFlag = false; 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_ */ #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 "Igrf13Model.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <time.h>
#include <cmath>
#include "util/MathOperations.h" #include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {} Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {} Igrf13Model::~Igrf13Model() {}
@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
double* magFieldModelInertial) { double* magFieldModelInertial) {
double phi = longitude, theta = gcLatitude; // geocentric double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/ /* Here is the co-latitude needed*/
theta -= 90 * Math::PI / 180; theta -= 90 * PI / 180;
theta *= (-1); theta *= (-1);
double rE = 6371200.0; // radius earth [m] 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) */ /* Calculation of Legendre Polynoms (normalised) */
if (n == m) { if (n == m) {
P2 = sin(theta) * P11; P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 - cos(theta) * P11; dP2 = sin(theta) * dP11 + cos(theta) * P11;
P11 = P2; P11 = P2;
P10 = P11; P10 = P11;
P20 = 0; P20 = 0;
@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[0] += magFieldModel[0] +=
pow(rE / (altitude + rE), (n + 2)) * (n + 1) * pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); ((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] += magFieldModel[1] +=
pow(rE / (altitude + rE), (n + 2)) * pow(rE / (altitude + rE), (n + 2)) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); ((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] += magFieldModel[2] +=
pow(rE / (altitude + rE), (n + 2)) * pow(rE / (altitude + rE), (n + 2)) *
((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); ((-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[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta)); magFieldModel[2] *= (-1 / sin(theta));
/* Next step: transform into inertial KOS (IJK)*/
// Julean Centuries
double JD2000Floor = 0;
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
JD2000Floor = floor(JD2000); double UT1 = JD2000 / 36525.;
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 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] double lst = gst + longitude; // local sidereal time [rad]
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + magFieldModelInertial[0] =
magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + magFieldModel[2] * sin(lst);
magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); magFieldModelInertial[1] =
magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); (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}; double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3); VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
} }
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { 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] * - timeOfMagMeasurement: time of actual measurement [s]
* *
* Outputs: * 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. // Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3]; double magFieldModel[3];
void schmidtNormalization();
private: 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}, {-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}, {-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}, {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.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 {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}, {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}, {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}, {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.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 {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}, {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}, {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}, {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},
{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 {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}, {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}, {-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}, {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},
{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 {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 updatedG[14][13];
double updatedH[14][13]; double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation 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 "MultiplicativeKalmanFilter.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
@ -14,6 +7,8 @@
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <cmath>
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
@ -29,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
} }
void MultiplicativeKalmanFilter::reset() {} void MultiplicativeKalmanFilter::reset() {}
@ -41,14 +36,11 @@ void MultiplicativeKalmanFilter::init(
// check for valid mag/sun // check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true; validInit = true;
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
// QUEST ALGO ----------------------------------------------------------------------- // QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG; sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
sigmaGyro = 0.1 * 3.141 / 180; // acs parameters
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[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}}, matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
matrixSunMag[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}}; 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(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 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][3] = initGyroCov[2][0];
initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2]; 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 { } else {
// no initialisation possible, no valid measurements // no initialisation possible, no valid measurements
validInit = false; validInit = false;
@ -208,14 +197,15 @@ void MultiplicativeKalmanFilter::init(
} }
// --------------- MEKF DISCRETE TIME STEP ------------------------------- // --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::mekfEst( ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, const double *rateGYRs_, const bool validGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *magneticField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const bool validMagField_, const double *sunDir_,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) { const bool validSS, const double *sunDirJ,
const bool validSSModel, const double *magFieldJ,
const bool validMagModel, double sampleTime,
acsctrl::MekfData *mekfData) {
// Check for GYR Measurements // Check for GYR Measurements
// AcsParameters mekfEstParams;
// loadAcsParameters(&mekfEstParams);
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) { if (!validGYRs_) {
{ {
@ -960,10 +950,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
biasGYR[2] = updatedGyroBias[2]; biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/ /* ----------- PROPAGATION ----------*/
// double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
// double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = 3 * 3.141 / 180 / 3600;
double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, 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}}; {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 // 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}, 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}}; {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}}, double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
transCovQ2[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 < 3.141 / 10) { if (normRotEst * sampleTime < M_PI / 10) {
double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3); double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 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);
discProcessNoiseCov[0][0] = covQ1[0][0]; double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
discProcessNoiseCov[0][1] = covQ1[0][1]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
discProcessNoiseCov[0][2] = covQ1[0][2]; std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
discProcessNoiseCov[0][3] = covQ2[0][0];
discProcessNoiseCov[0][4] = covQ2[0][1]; double fact22 = pow(sigmaU, 2) * sampleTime;
discProcessNoiseCov[0][5] = covQ2[0][2]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
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];
} else { } else {
// double fact1 = sampleTime*pow(sigmaV,2); double fact22 = pow(sigmaU, 2) * sampleTime;
double covQ11[3][3], covQ12[3][3], covQ13[3][3]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 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 covQ21[3][3], covQ22[3][3], covQ23[3][3]; double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact4 = double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
(0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / 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); pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
double fact5 = 0.5 * pow(sampleTime, 2); MatrixOperations<double>::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); MatrixOperations<double>::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3);
MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3); MatrixOperations<double>::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3);
double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3); MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 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>::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
discProcessNoiseCov[0][0] = covQ1[0][0]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
discProcessNoiseCov[0][1] = covQ1[0][1]; double fact11_1 = 1. / 3. * pow(sampleTime, 3);
discProcessNoiseCov[0][2] = covQ1[0][2]; MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
discProcessNoiseCov[0][3] = covQ2[0][0]; double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
discProcessNoiseCov[0][4] = covQ2[0][1]; 1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
discProcessNoiseCov[0][5] = covQ2[0][2]; pow(normRotEst, 5);
discProcessNoiseCov[1][0] = covQ1[1][0]; MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
discProcessNoiseCov[1][1] = covQ1[1][1]; MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
discProcessNoiseCov[1][2] = covQ1[1][2]; MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
discProcessNoiseCov[1][3] = covQ2[1][0]; MatrixOperations<double>::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3);
discProcessNoiseCov[1][4] = covQ2[1][1]; MatrixOperations<double>::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3);
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];
} }
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 // State Transition Matrix phi
double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
phi2[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}, 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}}; {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 phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact1 = sin(normRotEst * sampleTime); double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3); MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3); MatrixOperations<double>::add(*phi11_01, *phi11_2, *phi11, 3, 3);
double phi21[3][3], phi22[3][3]; double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3); MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 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][0] = phi11[0][0];
phi[0][1] = phi1[0][1]; phi[0][1] = phi11[0][1];
phi[0][2] = phi1[0][2]; phi[0][2] = phi11[0][2];
phi[0][3] = phi2[0][0]; phi[0][3] = phi12[0][0];
phi[0][4] = phi2[0][1]; phi[0][4] = phi12[0][1];
phi[0][5] = phi2[0][2]; phi[0][5] = phi12[0][2];
phi[1][0] = phi1[1][0]; phi[1][0] = phi11[1][0];
phi[1][1] = phi1[1][1]; phi[1][1] = phi11[1][1];
phi[1][2] = phi1[1][2]; phi[1][2] = phi11[1][2];
phi[1][3] = phi2[1][0]; phi[1][3] = phi12[1][0];
phi[1][4] = phi2[1][1]; phi[1][4] = phi12[1][1];
phi[1][5] = phi2[1][2]; phi[1][5] = phi12[1][2];
phi[2][0] = phi1[2][0]; phi[2][0] = phi11[2][0];
phi[2][1] = phi1[2][1]; phi[2][1] = phi11[2][1];
phi[2][2] = phi1[2][2]; phi[2][2] = phi11[2][2];
phi[2][3] = phi2[2][0]; phi[2][3] = phi12[2][0];
phi[2][4] = phi2[2][1]; phi[2][4] = phi12[2][1];
phi[2][5] = phi2[2][2]; phi[2][5] = phi12[2][2];
// Propagated Quaternion // 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 rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3); 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}}; double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(rotSin, *skewSin); MathOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
MatrixOperations<double>::subtract(*omega1, *skewSin, *omega1, 3, 3); double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]}, MatrixOperations<double>::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3);
{omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]}, double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]},
{omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]}, {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}}; {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
// Update Covariance Matrix // 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>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6);
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
MatrixOperations<double>::transpose(*phi, *transPhi, 6); MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 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);
validMekf = true; validMekf = true;
// Discrete Time Step // 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 validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS, const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ, 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 // Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
@ -79,7 +79,6 @@ class MultiplicativeKalmanFilter {
AcsParameters::KalmanFilterParameters *kalmanFilterParameters; AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4]; double quaternion_STR_SB[4];
bool validInit; bool validInit;
double sampleTime = 0.1;
/*States*/ /*States*/
double initialQuaternion[4]; /*after reset?QUEST*/ 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, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
mekfData); // VALIDS FOR QUAT AND RATE ?? mekfData); // VALIDS FOR QUAT AND RATE ??
} else { } else {
multiplicativeKalmanFilter.init( multiplicativeKalmanFilter.init(

204
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 "SensorProcessing.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
@ -14,8 +21,7 @@
using namespace Math; using namespace Math;
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
: savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
SensorProcessing::~SensorProcessing() {} SensorProcessing::~SensorProcessing() {}
@ -27,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid, const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) { 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) { if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
{ {
PoolReadGuard pg(mgmDataProcessed); PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float)); float zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float)); std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double)); std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float));
mgmDataProcessed->setValidity(false, true); mgmDataProcessed->setValidity(false, true);
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
} }
} }
return; return;
@ -134,19 +156,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
} }
timeOfSavedMagFieldEst = timeOfMgmMeasurement; 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); PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -182,6 +191,25 @@ void SensorProcessing::processSus(
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) { 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) { if (sus0valid) {
sus0valid = susConverter.checkSunSensorData(sus0Value); sus0valid = susConverter.checkSunSensorData(sus0Value);
} }
@ -224,22 +252,24 @@ void SensorProcessing::processSus(
{ {
PoolReadGuard pg(susDataProcessed); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); float zeroVec[3] = {0.0, 0.0, 0.0};
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float)); std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double)); std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
susDataProcessed->setValidity(false, true); susDataProcessed->setValidity(false, true);
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
susDataProcessed->sunIjkModel.setValid(true);
} }
} }
return; return;
@ -258,16 +288,6 @@ void SensorProcessing::processSus(
susParameters->sus0coeffBeta), susParameters->sus0coeffBeta),
sus0VecBody, 3, 3, 1); 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) { if (sus1valid) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus1orientationMatrix[0], susParameters->sus1orientationMatrix[0],
@ -275,16 +295,6 @@ void SensorProcessing::processSus(
susParameters->sus1coeffBeta), susParameters->sus1coeffBeta),
sus1VecBody, 3, 3, 1); 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) { if (sus2valid) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus2orientationMatrix[0], susParameters->sus2orientationMatrix[0],
@ -389,30 +399,10 @@ void SensorProcessing::processSus(
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i]; savedSusVecTot[i] = susVecTot[i];
susVecTotDerivativeValid = true;
} }
} }
timeOfSavedSusDirEst = timeOfSusMeasurement; 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); PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -469,6 +459,7 @@ void SensorProcessing::processGyr(
{ {
PoolReadGuard pg(gyrDataProcessed); PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { 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->gyr0vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr2vec.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}; const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1); 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++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -497,6 +490,8 @@ void SensorProcessing::processGyr(
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1); 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++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -506,6 +501,8 @@ void SensorProcessing::processGyr(
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1); 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++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -515,6 +512,8 @@ void SensorProcessing::processGyr(
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1); 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++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -525,7 +524,7 @@ void SensorProcessing::processGyr(
// take ADIS measurements, if both avail // take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion // if just one ADIS measurement avail, perform sensor fusion
double gyrVecTot[3] = {0.0, 0.0, 0.0}; 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]; double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 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 bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) { acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process // 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) { if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gps0longitude * PI / 180; gdLongitude = gpsLongitude * PI / 180.;
double latitudeRad = gps0latitude * PI / 180; double latitudeRad = gpsLatitude * PI / 180.;
double eccentricityWgs84 = 0.0818195; double eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2); double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad)); 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); PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude; gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->setValidity(validGps, validGps); std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
if (!validGps) { std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->gdLongitude.value = 0.0; gpsDataProcessed->setValidity(validGps, true);
gpsDataProcessed->gcLatitude.value = 0.0;
}
} }
} }
} }
@ -587,10 +601,12 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) { const AcsParameters *acsParameters) {
sensorValues->update(); sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, processGps(
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.isValid()), sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
gpsDataProcessed); (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()),
&acsParameters->gpsParameters, gpsDataProcessed);
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),

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

@ -65,17 +65,20 @@ class SensorProcessing {
void processStr(); 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); acsctrl::GpsDataProcessed *gpsDataProcessed);
double savedMgmVecTot[3]; double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst; timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3]; double savedSusVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedSusDirEst; timeval timeOfSavedSusDirEst;
bool validMagField; bool validMagField = false;
bool validGcLatitude;
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; SusConverter susConverter;
AcsParameters acsParameters; AcsParameters acsParameters;

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

@ -1,12 +1,12 @@
#ifndef SENSORVALUES_H_ #ifndef SENSORVALUES_H_
#define SENSORVALUES_H_ #define SENSORVALUES_H_
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.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() {} Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); detumbleParameter = &(acsParameters_->detumbleParameter);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter); magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
} }
@ -31,7 +31,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
if (!magRateValid || !magFieldValid) { if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double gain = detumbleCtrlParameters->gainD; double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2); double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3); VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK; return returnvalue::OK;
@ -50,3 +50,15 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
return returnvalue::OK; 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 uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); 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 * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void loadAcsParameters(AcsParameters *acsParameters_); 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 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: private:
AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters; AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter *magnetorquesParameter; AcsParameters::MagnetorquesParameter *magnetorquesParameter;
}; };

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

@ -21,21 +21,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices); rwMatrices = &(acsParameters_->rwMatrices);
} }
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate, void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *rwPseudoInv, double *torqueRws) { const double *qError, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix // Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om; double om = pointingLawParameters->om;
double zeta = pointingModeControllerParameters->zeta; double zeta = pointingLawParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin; double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax; double omMax = pointingLawParameters->omMax;
double cInt = 2 * om * zeta; double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2); 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}; double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3); VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); 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 *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) { int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
mgtDpDes[0] = 0; mgtDpDes[0] = 0;
mgtDpDes[1] = 0; mgtDpDes[1] = 0;
mgtDpDes[2] = 0; mgtDpDes[2] = 0;
@ -127,17 +129,18 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3); VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error // calculating momentum error
double deltaMomentum[3] = {0, 0, 0}; double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract( VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); deltaMomentum, 3);
// resulting magnetic dipole command // resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0}; double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField); VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0; double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; factor = (pointingLawParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); 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) { const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; 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>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
wheelMomentum, 4); 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}}; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
*nullSpaceMatrix, 4); *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, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, 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 uint8_t INTERFACE_ID = CLASS_ID::PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); 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 * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
@ -41,21 +41,32 @@ class PtgCtrl {
/* @brief: Calculates the needed torque for the pointing control mechanism /* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *rwPseudoInv, double *torqueRws); 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, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes); 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); 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: private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices; AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0};
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #endif /* ACS_CONTROL_PTGCTRL_H_ */

15
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; 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, void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid, bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef, double *magRateB, bool magRateBValid, double *sunDirRef,
@ -127,20 +127,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3); VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
/* Only valid if angle between sun direction and magnetic field direction /* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */ * is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
double sinAngle = 0; if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
sinAngle = sin(acos(cos(cosAngleSunMag)));
if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
return; return;
} }
// Rate for Torque Calculation // Rate for Torque Calculation
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3); VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation // Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0; double kRateNoMekf = 0, kAlignNoMekf = 0;
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;

46
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, static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
T2 *cartesianOutput) { T2 *cartesianOutput) {
double radiusPolar = 6378137; /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
double radiusEqua = 6356752.314; * 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 eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 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[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); 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 /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time * @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff * outputDotDcmEJ Derivative of transformation matrix [3][3]
* Landis Markley and John L. Crassidis*/ * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
static void dcmEJ(timeval time, T1 *outputDcmEJ) { * Landis Markley and John L. Crassidis*/
double JD2000Floor = 0; double JD2000Floor = 0;
double JD2000 = convertUnixToJD2000(time); double JD2000 = convertUnixToJD2000(time);
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0) // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
@ -143,6 +151,16 @@ class MathOperations {
outputDcmEJ[6] = 0; outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0; outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1; 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 /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
@ -215,7 +233,7 @@ class MathOperations {
precession[2][2] = cos(theta2); 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}}; double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// lunar asc node // lunar asc node
@ -230,7 +248,6 @@ class MathOperations {
// % true obliquity of the ecliptic eps p.71 (simplified) // % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
;
nutation[0][0] = cos(dp); nutation[0][0] = cos(dp);
nutation[1][0] = cos(e + de) * sin(dp); nutation[1][0] = cos(e + de) * sin(dp);
@ -260,10 +277,9 @@ class MathOperations {
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
} }
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
int i, j; int i, j;
double determinant; double determinant = 0;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
{matrix[3], matrix[4], matrix[5]}, {matrix[3], matrix[4], matrix[5]},
{matrix[6], matrix[7], matrix[8]}}; {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] - 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])); mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
} }
// cout<<"\size\ndeterminant: "<<determinant; // cout<<"\n\ndeterminant: "<<determinant;
// cout<<"\size\nInverse of matrix is: \size"; // cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) { for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) - 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 // GPS Processed
GC_LATITUDE, GC_LATITUDE,
GD_LONGITUDE, GD_LONGITUDE,
GPS_POSITION,
GPS_VELOCITY,
// MEKF // MEKF
SAT_ROT_RATE_MEKF, SAT_ROT_RATE_MEKF,
QUAT_MEKF, QUAT_MEKF,
@ -99,13 +101,13 @@ enum PoolIds : lp_id_t {
MTQ_TARGET_DIPOLE, 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 MGM_SET_PROCESSED_ENTRIES = 8;
static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; 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 MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_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> 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_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: 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 = auto ACS_TABLE_SAFE_TRANS_1 =
std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 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 = 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 = 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 = 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 = auto ACS_TABLE_PTG_TRANS_0 =
std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>()); 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>()); std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_NADIR = auto ACS_SEQUENCE_PTG_TARGET_NADIR =
std::make_pair(acs::AcsMode::PTG_TARGET_NADIR, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsMode::PTG_TARGET_NADIR << 24) | 1, auto ACS_TABLE_PTG_TARGET_NADIR_TGT =
FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 =
(acs::AcsMode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());
auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = auto ACS_SEQUENCE_PTG_TARGET_INERTIAL =
std::make_pair(acs::AcsMode::PTG_TARGET_INERTIAL, FixedArrayList<ModeListEntry, 3>()); std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 3>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT =
(acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>()); std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 =
(acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>()); std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());
void satsystem::acs::init() { void satsystem::acs::init() {
ModeListEntry entry; ModeListEntry entry;
@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc); check(sequence.insert(eh), ctxc);
}; };
// Build IDLE target // 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::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASS, 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); 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); ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
// Build IDLE transition 1 // 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); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true);
// Build IDLE sequence // Build IDLE sequence
@ -350,8 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built // Transition 0 already built
// Build TARGET PT transition 1 // Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second);
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, check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false,
true), true),
ctxc); ctxc);
@ -399,7 +398,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
// Transition 0 already built // Transition 0 already built
// Build TARGET PT transition 1 // 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); ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)),
@ -485,7 +484,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build TARGET PT table // 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); ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, 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); 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 // Transition 0 already built
// Build TARGET PT transition 1 // 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); ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)),