From 338c8299e2ea85ee3d30717d91c2cf2a702fc254 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 19 Sep 2022 15:03:25 +0200
Subject: [PATCH] added Commanding as ActuatorCmd

---
 mission/controller/acs/ActuatorCmd.cpp | 83 ++++++++++++++++++++++++++
 mission/controller/acs/ActuatorCmd.h   | 48 +++++++++++++++
 2 files changed, 131 insertions(+)
 create mode 100644 mission/controller/acs/ActuatorCmd.cpp
 create mode 100644 mission/controller/acs/ActuatorCmd.h

diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
new file mode 100644
index 00000000..25eeca9c
--- /dev/null
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -0,0 +1,83 @@
+/*
+ * ActuatorCmd.cpp
+ *
+ *  Created on: 4 Aug 2022
+ *      Author: Robin Marquardt
+ */
+
+
+#include "ActuatorCmd.h"
+#include <math.h>
+#include <acs/util/MathOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <acs/util/CholeskyDecomposition.h>
+
+ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) {
+	acsParameters = *acsParameters_;
+}
+
+ActuatorCmd::~ActuatorCmd(){
+
+}
+
+void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2,
+		const double *speedRw3, const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed){
+
+	using namespace Math;
+	// Scaling the commanded torque to a maximum value
+	double torque[4] = {0, 0, 0, 0};
+	double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
+	VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
+
+	double maxValue = 0;
+	for (int i = 0; i < 4; i++) { //size of torque, always 4 ?
+		if (abs(torque[i]) > maxValue) {
+			maxValue = abs(torque[i]);
+		}
+	}
+
+	if (maxValue > maxTrq) {
+
+		double scalingFactor = maxTrq / maxValue;
+		VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
+
+	}
+
+	// Calculating the commanded speed in RPM for every reaction wheel
+	double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
+	double deltaSpeed[4] = {0, 0, 0, 0};
+	double commandTime = acsParameters.onBoardParams.sampleTime,
+			inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
+	double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
+	//	 W_RW = Torque_RW / I_RW * delta t [rad/s]
+	double factor = commandTime / inertiaWheel * radToRpm;
+	VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
+	VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
+
+
+
+}
+
+void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
+
+//	Convert to Unit frame
+	MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 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 (maxValue > maxDipol) {
+
+		double scalingFactor = maxDipol / maxValue;
+		VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
+
+	}
+}
diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h
new file mode 100644
index 00000000..9b721505
--- /dev/null
+++ b/mission/controller/acs/ActuatorCmd.h
@@ -0,0 +1,48 @@
+/*
+ * ActuatorCmd.h
+ *
+ *  Created on: 4 Aug 2022
+ *      Author: Robin Marquardt
+ */
+
+#ifndef ACTUATORCMD_H_
+#define ACTUATORCMD_H_
+
+
+#include "AcsParameters.h"
+#include "SensorProcessing.h"
+#include <acs/MultiplicativeKalmanFilter.h>
+#include <acs/SensorValues.h>
+#include <acs/OutputValues.h>
+
+class ActuatorCmd{
+public:
+	ActuatorCmd(AcsParameters *acsParameters_); //Input mode ?
+	virtual ~ActuatorCmd();
+
+	/*
+	 * @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
+	 * 								as Input to the RWs
+	 * @param:	rwTrqIn 			given torque from pointing controller
+	 * 			rwTrqNS				Nullspace torque
+	 * 			rwCmdSpeed			output revolutions per minute for every reaction wheel
+	 */
+	void cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3,
+			const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed);
+
+	/*
+	 * @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
+	 */
+	void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
+
+protected:
+
+private:
+	AcsParameters acsParameters;
+};
+
+#endif /* ACTUATORCMD_H_ */