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

Merge branch 'develop' into eggert/mgm-calibration-fix
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-07 09:50:26 +01:00
parent a9fcb6a183 5f81b0866a
commit b881b57a1b
35 changed files with 617 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
mission/controller/AcsController.cpp
View File

@ -2,6 +2,7 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "mission/acsDefs.h"
#include "mission/config/torquer.h"
AcsController::AcsController(object_id_t objectId)
@ -45,15 +46,15 @@ void AcsController::performControlOperation() {
case InternalState::READY: {
if (mode != MODE_OFF) {
switch (submode) {
case SUBMODE_SAFE:
case acs::SAFE:
performSafe();
break;
case SUBMODE_DETUMBLE:
case acs::DETUMBLE:
performDetumble();
break;
case SUBMODE_PTG_TARGET:
case SUBMODE_PTG_NADIR:
case SUBMODE_PTG_INERTIAL:
case acs::PTG_TARGET:
case acs::PTG_TARGET_NADIR:
case acs::PTG_TARGET_INERTIAL:
performPointingCtrl();
break;
}
@ -151,9 +152,9 @@ void AcsController::performSafe() {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_DETUMBLE;
detumbleCounter = 0;
triggerEvent(SAFE_RATE_VIOLATION);
// Triggers detubmle mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
}
{
@ -208,8 +209,9 @@ void AcsController::performDetumble() {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_SAFE;
detumbleCounter = 0;
// Triggers safe mode transition in subsystem
triggerEvent(acs::SAFE_RATE_RECOVERY);
}
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
@ -484,9 +486,9 @@ void AcsController::copyMgmData() {
}
void AcsController::copySusData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
6 * sizeof(uint16_t));

11
mission/controller/AcsController.h
View File

@ -24,16 +24,6 @@ class AcsController : public ExtendedControllerBase {
AcsController(object_id_t objectId);
static const Submode_t SUBMODE_SAFE = 2;
static const Submode_t SUBMODE_DETUMBLE = 3;
static const Submode_t SUBMODE_PTG_TARGET = 4;
static const Submode_t SUBMODE_PTG_NADIR = 5;
static const Submode_t SUBMODE_PTG_INERTIAL = 6;
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
static const Event SAFE_RATE_VIOLATION =
MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated.
protected:
void performSafe();
void performDetumble();
@ -70,6 +60,7 @@ class AcsController : public ExtendedControllerBase {
void announceMode(bool recursive);
/* ACS Datasets */
ACS::SensorValues sensorValues;
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
// MGMs
acsctrl::MgmDataRaw mgmDataRaw;

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

@ -901,8 +901,8 @@ class AcsParameters /*: public HasParametersIF*/ {
struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4 * M_PI / 180;
double omegaDetumbleStart = 2;
double omegaDetumbleEnd = 0.4;
double gainD = pow(10.0, -3.3);
} detumbleParameter;
};

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

@ -54,21 +54,22 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
// Convert to Unit frame
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) {
// Convert to actuator frame
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentUnits, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
dipolMoment, dipolMomentActuator, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentUnits[i]) > maxDipol) {
maxValue = abs(dipolMomentUnits[i]);
if (abs(dipolMomentActuator[i]) > maxDipol) {
maxValue = abs(dipolMomentActuator[i]);
}
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
VectorOperations<double>::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3);
}
// scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3);
}

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

@ -35,9 +35,9 @@ class ActuatorCmd {
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
*
* @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentUnits resulting dipol moment for every unit
* dipolMomentActuator resulting dipol moment in actuator reference frame
*/
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator);
protected:
private:

14
mission/controller/controllerdefinitions/AcsControllerDefinitions.h
View File

@ -1,14 +0,0 @@
#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_
#include <fsfw/modes/HasModesIF.h>
namespace acs {
enum CtrlModes { OFF = HasModesIF::MODE_OFF, SAFE = 1, DETUMBLE = 2, IDLE = 3, TARGET_PT = 4 };
static constexpr Submode_t IDLE_CHARGE = 1;
} // namespace acs
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ */