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

Merge remote-tracking branch 'origin/develop' into thermal_controller
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2023-02-28 19:17:45 +01:00
parent 5f3bd5c754 db90c59557
commit 88db78fed9
139 changed files with 4458 additions and 1404 deletions
Download Patch File Download Diff File Expand all files Collapse all files

169
mission/controller/AcsController.cpp
View File

@ -1,9 +1,8 @@
#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include "mission/acsDefs.h"
#include "mission/config/torquer.h"
#include <mission/acsDefs.h>
#include <mission/config/torquer.h>
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId),
@ -46,6 +45,26 @@ ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
return result;
}
ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) {
switch (actionId) {
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
ReturnValue_t result = guidance.solarArrayDeploymentComplete();
if (result == returnvalue::FAILED) {
return FILE_DELETION_FAILED;
}
return HasActionsIF::EXECUTION_FINISHED;
}
case RESET_MEKF: {
navigation.resetMekf(&mekfData);
return HasActionsIF::EXECUTION_FINISHED;
}
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
}
}
MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
@ -60,6 +79,25 @@ void AcsController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "ACS & TCS PST");
#endif
{
PoolReadGuard pg(&mgmDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyMgmData();
}
}
{
PoolReadGuard pg(&susDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copySusData();
}
}
{
PoolReadGuard pg(&gyrDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyGyrData();
}
}
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
@ -95,25 +133,6 @@ void AcsController::performControlOperation() {
default:
break;
}
{
PoolReadGuard pg(&mgmDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyMgmData();
}
}
{
PoolReadGuard pg(&susDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copySusData();
}
}
{
PoolReadGuard pg(&gyrDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyGyrData();
}
}
}
void AcsController::performSafe() {
@ -124,8 +143,8 @@ void AcsController::performSafe() {
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
@ -139,7 +158,7 @@ void AcsController::performSafe() {
// if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
@ -189,8 +208,8 @@ void AcsController::performDetumble() {
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
@ -236,8 +255,8 @@ void AcsController::performPointingCtrl() {
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
@ -246,9 +265,10 @@ void AcsController::performPointingCtrl() {
triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
}
mekfInvalidCounter++;
commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
// commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration,
// cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// acsParameters.rwHandlingParameters.rampTime);
return;
} else {
mekfInvalidFlag = false;
@ -395,7 +415,7 @@ void AcsController::performPointingCtrl() {
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
updateCtrlValData(targetQuat, errorQuat, errorAngle);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
@ -432,64 +452,57 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
return returnvalue::OK;
}
void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) {
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
}
void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]) {
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
const int32_t *rwTargetSpeed,
const int16_t *mtqTargetDipole) {
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
void AcsController::updateCtrlValData(double errAng) {
double unitQuat[4] = {0, 0, 0, 1};
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(true, true);
}
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
const double *tgtRotRate) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.setValidity(true, true);
}
}
void AcsController::disableCtrlValData() {
double unitQuat[4] = {0, 0, 0, 1};
double errAng = 0;
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(false, true);
}
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.errAng.value = 0;
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
ctrlValData.setValidity(false, true);
}
}

40
mission/controller/AcsController.h
View File

@ -1,12 +1,17 @@
#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
#define MISSION_CONTROLLER_ACSCONTROLLER_H_
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
#include <mission/devices/devicedefinitions/SusDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <mission/trace.h>
#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
@ -17,11 +22,6 @@
#include "acs/control/PtgCtrl.h"
#include "acs/control/SafeCtrl.h"
#include "controllerdefinitions/AcsCtrlDefinitions.h"
#include "eive/objects.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/trace.h"
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public:
@ -40,6 +40,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl();
private:
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
static constexpr double ZERO_VEC[3] = {0, 0, 0};
static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
Navigation navigation;
@ -54,7 +59,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = true;
bool mekfInvalidFlag = false;
uint8_t mekfInvalidCounter = 0;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0};
@ -64,13 +69,23 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
#endif
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
/** Device command IDs */
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
static const DeviceCommandId_t RESET_MEKF = 0x1;
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override;
/* HasActionsIF overrides */
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
@ -84,11 +99,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
void updateActuatorCmdData(int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole);
void updateCtrlValData(double errAng);
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng);
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
const double* tgtRotRate);
void disableCtrlValData();
/* ACS Sensor Values */
@ -187,7 +203,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>();
PoolEntry<double> tgtRotRate = PoolEntry<double>(4);
PoolEntry<double> tgtRotRate = PoolEntry<double>(3);
// Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData;

16
mission/controller/ThermalController.cpp
View File

@ -3,13 +3,13 @@
#include <bsp_q7s/core/CoreDefinitions.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/thermal/ThermalComponentIF.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
#include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
@ -855,7 +855,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempGyro0 =
lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
@ -868,7 +868,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE);
lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
@ -882,7 +882,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempGyro2 =
lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
@ -895,7 +895,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE);
lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;
@ -909,7 +909,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempMgm0 =
lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl;
@ -923,7 +923,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempMgm2 =
lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl;

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

@ -1,8 +1,3 @@
/*******************************
* EIVE Flight Software Framework (FSFW)
* (c) 2022 IRS, Uni Stuttgart
*******************************/
#ifndef ACSPARAMETERS_H_
#define ACSPARAMETERS_H_

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

@ -551,3 +551,19 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}
ReturnValue_t Guidance::solarArrayDeploymentComplete() {
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
std::remove(SD_0_SKEWED_PTG_FILE);
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
return returnvalue::FAILED;
}
}
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
std::remove(SD_1_SKEWED_PTG_FILE);
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
return returnvalue::FAILED;
}
}
return returnvalue::OK;
}

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

@ -13,6 +13,7 @@ class Guidance {
virtual ~Guidance();
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
ReturnValue_t solarArrayDeploymentComplete();
// Function to get the target quaternion and refence rotation rate from gps position and
// position of the ground station

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

@ -189,12 +189,12 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
initialCovarianceMatrix[5][4] = initGyroCov[2][1];
initialCovarianceMatrix[5][5] = initGyroCov[2][2];
updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
return KALMAN_INITIALIZED;
return MEKF_INITIALIZED;
} else {
// no initialisation possible, no valid measurements
validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return KALMAN_UNINITIALIZED;
return MEKF_UNINITIALIZED;
}
}
@ -211,12 +211,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
return KALMAN_NO_GYR_DATA;
return MEKF_NO_GYR_DATA;
}
// Check for Model Calculations
else if (!validSSModel || !validMagModel) {
updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
return KALMAN_NO_MODEL_VECTORS;
return MEKF_NO_MODEL_VECTORS;
}
// Check Measurements available from SS, MAG, STR
if (validSS && validMagField_ && validSTR_) {
@ -854,7 +854,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (inversionFailed) {
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
}
// [K = P * H' / (H * P * H' + R)]
@ -1085,16 +1085,17 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
return KALMAN_RUNNING;
return MEKF_RUNNING;
}
void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1};
double resetCovarianceMatrix[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}};
std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED;
}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,

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

@ -1,17 +1,3 @@
/*
* MultiplicativeKalmanFilter.h
*
* Created on: 4 Feb 2022
* Author: Robin Marquardt
*
* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
* means of the spacecraft attitude sensors
*
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
* Marquardt
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
*/
#ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_
@ -22,6 +8,13 @@
#include "eive/resultClassIds.h"
class MultiplicativeKalmanFilter {
/* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
* means of the spacecraft attitude sensors
*
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
* Marquardt
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
*/
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
@ -29,7 +22,7 @@ class MultiplicativeKalmanFilter {
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
virtual ~MultiplicativeKalmanFilter();
void reset(acsctrl::MekfData *mekfData);
ReturnValue_t reset(acsctrl::MekfData *mekfData);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm
@ -74,15 +67,15 @@ class MultiplicativeKalmanFilter {
};
// resetting Mekf
static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2);
static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3);
static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4);
static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5);
static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED =
returnvalue::makeCode(IF_KAL_ID, 6);
static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7);
static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8);
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4);
static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
returnvalue::makeCode(IF_MEKF_ID, 6);
static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
private:
/*Parameters*/

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

@ -25,26 +25,25 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) {
return multiplicativeKalmanFilter.mekfEst(
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
mekfStatus = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
} else {
ReturnValue_t result;
result = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
kalmanInit = true;
return result;
return mekfStatus;
}
}
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
multiplicativeKalmanFilter.reset(mekfData);
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}

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

@ -22,7 +22,7 @@ class Navigation {
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters;
bool kalmanInit = false;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
};
#endif /* ACS_NAVIGATION_H_ */

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

@ -1,7 +1,3 @@
/*******************************
* EIVE Flight Software
* (c) 2022 IRS, Uni Stuttgart
*******************************/
#ifndef SENSORPROCESSING_H_
#define SENSORPROCESSING_H_

6
mission/controller/acs/SensorValues.cpp
View File

@ -1,9 +1,3 @@
/*
* SensorValues.cpp
*
* Created on: 30 Mar 2022
* Author: rooob
*/
#include "SensorValues.h"
#include <fsfw/datapool/PoolReadGuard.h>

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

@ -1,6 +1,7 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_H_
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
@ -9,7 +10,6 @@
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
namespace ACS {
@ -27,14 +27,12 @@ class SensorValues {
ReturnValue_t updateStr();
ReturnValue_t updateRw();
MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
RM3100::Rm3100PrimaryDataset mgm1Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
mgmLis3::MgmPrimaryDataset mgm0Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
mgmRm3100::Rm3100PrimaryDataset mgm1Rm3100Set =
mgmRm3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
mgmLis3::MgmPrimaryDataset mgm2Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
mgmRm3100::Rm3100PrimaryDataset mgm3Rm3100Set =
mgmRm3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
imtq::RawMtmMeasurementNoTorque imtqMgmSet =
imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER);

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

@ -1,16 +1,9 @@
/*
* SusConverter.cpp
*
* Created on: 17.01.2022
* Author: Timon Schwarz
*/
#include "SusConverter.h"
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/LocalPoolVector.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> //for atan2
#include <math.h>
#include <iostream>

9
mission/controller/acs/SusConverter.h
View File

@ -1,10 +1,3 @@
/*
* SusConverter.h
*
* Created on: Sep 22, 2022
* Author: marius
*/
#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
@ -28,8 +21,6 @@ class SusConverter {
private:
float alphaBetaRaw[2]; //[°]
// float coeffAlpha[9][10];
// float coeffBeta[9][10];
float alphaBetaCalibrated[2]; //[°]
float sunVectorSensorFrame[3]; //[-]

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

@ -300,6 +300,7 @@ class MathOperations {
}
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
/* do not use this. takes 300ms */
float det = 0;
T1 matrix[size][size], submatrix[size - 1][size - 1];
for (uint8_t row = 0; row < size; row++) {
@ -329,9 +330,7 @@ class MathOperations {
}
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
if (MathOperations<T1>::matrixDeterminant(inputMatrix, size) == 0) {
return 1; // Matrix is singular and not invertible
}
// Stopwatch stopwatch;
T1 matrix[size][size], identity[size][size];
// reformat array to matrix
for (uint8_t row = 0; row < size; row++) {
@ -346,7 +345,6 @@ class MathOperations {
}
// gauss-jordan algo
// sort matrix such as no diag entry shall be 0
// should not be needed as such a matrix has a det=0
for (uint8_t row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
bool swaped = false;

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

@ -110,7 +110,7 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;