eive-obsw/mission/controller/acs/control/SafeCtrl.cpp

/*
 * SafeCtrl.cpp
 *
 *  Created on: 19 Apr 2022
 *      Author: Robin Marquardt
 */

#include "SafeCtrl.h"
#include "../util/MathOperations.h"
#include <math.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>


SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){
	loadAcsParameters(acsParameters_);
	MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3);
}

SafeCtrl::~SafeCtrl(){

}

void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){
	safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
	inertiaEIVE = &(acsParameters_->inertiaEIVE);
}

ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
		double *magFieldModel, bool *magFieldModelValid,
		double *sunDirModel, bool *sunDirModelValid,
		double *satRateMekf, bool *rateMekfValid,
		double *sunDirRef, double *satRatRef,
		double *outputMagMomB, bool *outputValid){

	if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
			!(*rateMekfValid)) {
		*outputValid = false;
		return SAFECTRL_MEKF_INPUT_INVALID;
	}

	double kRate = 0, kAlign = 0;
	kRate = safeModeControllerParameters->k_rate_mekf;
	kAlign = safeModeControllerParameters->k_align_mekf;

//	Calc sunDirB ,magFieldB with mekf output and model
	double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
	MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
	double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0};
	MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
	MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);

	double crossSun[3] = {0, 0, 0};

	VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
	double normCrossSun = VectorOperations<double>::norm(crossSun, 3);

//	calc angle alpha between sunDirRef and sunDIr
	double alpha = 0, dotSun = 0;
	dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
	alpha = acos(dotSun);

//	Law Torque calculations
	double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0},
			torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0};

	double scalarFac = 0;
	scalarFac = kAlign * alpha / normCrossSun;
	VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);

	double rateSafeMode[3] = {0,0,0};
	VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
	VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);

	VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
//	Adding factor of inertia for axes
	MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);

// MagMom B (orthogonal torque)
	double torqueMgt[3] = {0,0,0};
	VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
	double normMag = VectorOperations<double>::norm(magFieldB, 3);
	VectorOperations<double>::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3);
	*outputValid = true;

	return returnvalue::OK;

}

// Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
		double *sunRateB, bool *sunRateBValid,
		double *magFieldB, bool *magFieldBValid,
		double *magRateB, bool *magRateBValid,
		double *sunDirRef, double *satRateRef,
		double *outputMagMomB, bool *outputValid){

// Check for invalid Inputs
	if ( !susDirBValid || !magFieldBValid || !magRateBValid) {
		*outputValid = false;
		return;
	}

//	normalize sunDir and magDir
	double magDirB[3] = {0, 0, 0};
	VectorOperations<double>::normalize(magFieldB, magDirB, 3);
	VectorOperations<double>::normalize(susDirB, susDirB, 3);

//	Cosinus angle between sunDir and magDir
	double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);

//	Rate parallel to sun direction and magnetic field direction
	double rateParaSun = 0, rateParaMag = 0;
	double dotSunRateMag = 0, dotmagRateSun = 0,
			rateFactor = 0;
	dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
	dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
	rateFactor = 1 - pow(cosAngleSunMag,2);
	rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor;
	rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor;

//	Full rate or estimate
	double estSatRate[3] = {0, 0, 0};
	double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
    VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
    VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
    VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
    VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
    VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
    VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);

/*    Only valid if angle between sun direction and magnetic field direction
      is sufficiently large */

    double sinAngle = 0;
    sinAngle = sin(acos(cos(cosAngleSunMag)));

    if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
    	return;
    }

//    Rate for Torque Calculation
    double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
    VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);

//    Torque Align calculation
	double kRateNoMekf = 0, kAlignNoMekf = 0;
	kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
	kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;

    double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
    double crossSusSunRef[3] = {0, 0, 0};
    VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
    double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);

    double torqueAlign[3] = {0, 0, 0};
    double angleAlignErr = acos(cosAngleAlignErr);
    double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
    VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);

//Torque Rate Calculations
    double torqueRate[3] = {0, 0, 0};
    VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);

//Final torque
    double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
    VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
    MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1);

//Magnetic moment
    double magMomB[3] = {0, 0, 0};
    double crossMagFieldTorque[3] = {0, 0, 0};
    VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
    double magMomFactor = pow( VectorOperations<double>::norm(magFieldB, 3), 2 );
    VectorOperations<double>::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3);

    outputMagMomB[0] = magMomB[0];
    outputMagMomB[1] = magMomB[1];
    outputMagMomB[2] = magMomB[2];

    *outputValid = true;

}