diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index d0d67691..cc1fd7b1 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -845,13 +845,13 @@ public: bool avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; - double blindRotRate = 1 * Math::PI /180; + double blindRotRate = 1 * M_PI /180; double zeta = 0.3; double zetaLow; double om = 0.3; double omLow; - double omMax = 1 * Math::PI / 180; + double omMax = 1 * M_PI / 180; double qiMin = 0.1; double gainNullspace = 0.01; @@ -867,7 +867,7 @@ public: struct StrParameters { - double exclusionAngle = 20 * Math::PI /180; + double exclusionAngle = 20 * M_PI /180; // double strOrientationMatrix[3][3]; double boresightAxis[3] = { 0.7593, 0.0000,-0.6508}; //in body/geometry frame } strParameters; @@ -876,8 +876,8 @@ public: } gpsParameters; struct GroundStationParameters { - double latitudeGs = 48.7495 * Math::PI / 180.; // [rad] Latitude - double longitudeGs = 9.10384 * Math::PI / 180.; // [rad] Longitude + 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] @@ -889,15 +889,15 @@ public: }; uint8_t useSunModel; float domega = 36000.771; - float omega_0 = 282.94 * Math::PI / 180.; //RAAN plus argument of perigee + float omega_0 = 282.94 * M_PI / 180.; //RAAN plus argument of perigee float m_0 = 357.5256; //coefficients for mean anomaly float dm = 35999.049; //coefficients for mean anomaly // ToDo: check correct assignment of e and e1. Both were assigned to e before - float e = 23.4392911 * Math::PI / 180.; //angle of earth's rotation axis - float e1 = 0.74508 * Math::PI / 180.; + float e = 23.4392911 * M_PI / 180.; //angle of earth's rotation axis + float e1 = 0.74508 * M_PI / 180.; // - float p1 = 6892. / 3600. * Math::PI / 180.; //some parameter - float p2 = 72. / 3600. * Math::PI / 180.; //some parameter + float p1 = 6892. / 3600. * M_PI / 180.; //some parameter + float p2 = 72. / 3600. * M_PI / 180.; //some parameter } sunModelParameters; struct KalmanFilterParameters { @@ -907,9 +907,9 @@ public: double processNoiseOmega[3]; double processNoiseQuaternion[4]; - double sensorNoiseSTR = 0.1 * Math::PI / 180; - double sensorNoiseSS = 8 * Math::PI / 180; - double sensorNoiseMAG = 4 * Math::PI / 180; + double sensorNoiseSTR = 0.1 * M_PI / 180; + double sensorNoiseSS = 8 * M_PI / 180; + double sensorNoiseMAG = 4 * M_PI / 180; double sensorNoiseRMU[3]; double sensorNoiseArwRmu; //Angular Random Walk diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h index a73dc43c..c9fc71f1 100644 --- a/mission/controller/acs/OutputValues.h +++ b/mission/controller/acs/OutputValues.h @@ -48,4 +48,4 @@ public: } -#endif OUTPUTVALUES_H_ +#endif /*OUTPUTVALUES_H_*/ diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index aa4dc3bd..097c88ed 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -86,5 +86,5 @@ protected: }; -#endif SENSORPROCESSING_H_ +#endif /*SENSORPROCESSING_H_*/ diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index c2d65133..13f23808 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -12,21 +12,6 @@ #include #include -void SunSensor::setSunSensorData() { - // ToDo: exchange dummy values with DataPool - susChannelValues[0] = {3913, 3912, 3799, 4056}; - susChannelValues[1] = {3913, 3912, 3799, 4056}; - susChannelValues[2] = {3913, 3912, 3799, 4056}; - susChannelValues[3] = {3913, 3912, 3799, 4056}; - susChannelValues[4] = {3913, 3912, 3799, 4056}; - susChannelValues[5] = {3913, 3912, 3799, 4056}; - susChannelValues[6] = {3913, 3912, 3799, 4056}; - susChannelValues[7] = {3913, 3912, 3799, 4056}; - susChannelValues[8] = {3913, 3912, 3799, 4056}; - susChannelValues[9] = {3913, 3912, 3799, 4056}; - susChannelValues[10] = {3913, 3912, 3799, 4056}; - susChannelValues[11] = {3913, 3912, 3799, 4056}; -} void SunSensor::checkSunSensorData(uint8_t susNumber) { uint16_t channelValueSum; @@ -261,14 +246,14 @@ void SunSensor::CalculateSunVector(uint8_t susNumber) { (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); } -float* SunSensor::getSunVectorBodyFrame() { +float* SunSensor::getSunVectorBodyFrame(uint8_t susNumber) { // return function for the sun vector in the body frame float* SunVectorBodyFrameReturn = 0; SunVectorBodyFrameReturn = new float[3]; - SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[0]; - SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[1]; - SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[2]; + SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[susNumber][0]; + SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[susNumber][1]; + SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[susNumber][2]; return SunVectorBodyFrameReturn; } @@ -289,7 +274,7 @@ float* SunSensor::TransferSunVector() { for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { // save the sun vector of each SUS in their body frame into an array for // further processing - float* SunVectorBodyFrame = SunVectorBodyFrame[susNumber]; + float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber]; sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2]; diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 3a5662ca..bfec1dfe 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -15,7 +15,6 @@ class SunSensor { public: SunSensor() {} - void setSunSensorData(); void checkSunSensorData(uint8_t susNumber); void calcAngle(uint8_t susNumber); void setCalibrationCoefficients(uint8_t susNumber); @@ -23,16 +22,35 @@ class SunSensor { void CalculateSunVector(uint8_t susNumber); bool getValidFlag(uint8_t susNumber); - float* getSunVectorBodyFrame(); + float* getSunVectorBodyFrame(uint8_t susNumber); float* TransferSunVector(); private: - uint16_t susChannelValues[12][4]; //[Bit] + // ToDo: remove statics and replace with actual data + uint16_t susChannelValues[12][4] = { + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}}; //[Bit] float alphaBetaRaw[12][2]; //[°] float alphaBetaCalibrated[12][2]; //[°] float sunVectorBodyFrame[12][3]; //[-] - bool validFlag[12] = returnvalue::OK; + bool validFlag[12] = {returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK}; uint16_t channelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h index 0c03b408..7db1ea72 100644 --- a/mission/controller/acs/config/classIds.h +++ b/mission/controller/acs/config/classIds.h @@ -7,11 +7,13 @@ #ifndef ACS_CONFIG_CLASSIDS_H_ #define ACS_CONFIG_CLASSIDS_H_ -#include "bsp_hosted/fsfwconfig/returnvalues/classIds.h" + +#include +#include namespace CLASS_ID { enum eiveclassIds: uint8_t { - EIVE_CLASS_ID_START = CLASS_ID_END, + EIVE_CLASS_ID_START = COMMON_CLASS_ID_END, KALMAN, SAFE, PTG, diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 835c32b0..b85aaf86 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -43,5 +43,5 @@ private: AcsParameters::MagnetorquesParameter* magnetorquesParameter; }; -#endif ACS_CONTROL_DETUMBLE_H_ +#endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index ff619f35..32029ad7 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -85,7 +85,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, VectorOperations::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3); *outputValid = true; - return RETURN_OK; + return returnvalue::OK; }